From e899cbf9bf66e17b058837a921d92912b8c5a1b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 13:14:56 +1100 Subject: [PATCH 01/16] AP_NavEKF: restructure nav_filter_status to be more useful for scripting --- libraries/AP_NavEKF/AP_Nav_Common.h | 44 +++++++++++++++-------------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/libraries/AP_NavEKF/AP_Nav_Common.h b/libraries/AP_NavEKF/AP_Nav_Common.h index 10037c4e1578d..d819aa35cb2cd 100644 --- a/libraries/AP_NavEKF/AP_Nav_Common.h +++ b/libraries/AP_NavEKF/AP_Nav_Common.h @@ -19,28 +19,30 @@ #include #include +struct nav_filter_status_flags_t { + bool attitude : 1; // 0 - true if attitude estimate is valid + bool horiz_vel : 1; // 1 - true if horizontal velocity estimate is valid + bool vert_vel : 1; // 2 - true if the vertical velocity estimate is valid + bool horiz_pos_rel : 1; // 3 - true if the relative horizontal position estimate is valid + bool horiz_pos_abs : 1; // 4 - true if the absolute horizontal position estimate is valid + bool vert_pos : 1; // 5 - true if the vertical position estimate is valid + bool terrain_alt : 1; // 6 - true if the terrain height estimate is valid + bool const_pos_mode : 1; // 7 - true if we are in const position mode + bool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff + bool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff + bool takeoff_detected : 1; // 10 - true if optical flow takeoff has been detected + bool takeoff : 1; // 11 - true if filter is compensating for baro errors during takeoff + bool touchdown : 1; // 12 - true if filter is compensating for baro errors during touchdown + bool using_gps : 1; // 13 - true if we are using GPS position + bool gps_glitching : 1; // 14 - true if GPS glitching is affecting navigation accuracy + bool gps_quality_good : 1; // 15 - true if we can use GPS for navigation + bool initialized : 1; // 16 - true if the EKF has ever been healthy + bool rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed data + bool dead_reckoning : 1; // 18 - true if we are dead reckoning (e.g. no position or velocity source) +}; + union nav_filter_status { - struct { - bool attitude : 1; // 0 - true if attitude estimate is valid - bool horiz_vel : 1; // 1 - true if horizontal velocity estimate is valid - bool vert_vel : 1; // 2 - true if the vertical velocity estimate is valid - bool horiz_pos_rel : 1; // 3 - true if the relative horizontal position estimate is valid - bool horiz_pos_abs : 1; // 4 - true if the absolute horizontal position estimate is valid - bool vert_pos : 1; // 5 - true if the vertical position estimate is valid - bool terrain_alt : 1; // 6 - true if the terrain height estimate is valid - bool const_pos_mode : 1; // 7 - true if we are in const position mode - bool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff - bool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff - bool takeoff_detected : 1; // 10 - true if optical flow takeoff has been detected - bool takeoff : 1; // 11 - true if filter is compensating for baro errors during takeoff - bool touchdown : 1; // 12 - true if filter is compensating for baro errors during touchdown - bool using_gps : 1; // 13 - true if we are using GPS position - bool gps_glitching : 1; // 14 - true if GPS glitching is affecting navigation accuracy - bool gps_quality_good : 1; // 15 - true if we can use GPS for navigation - bool initalized : 1; // 16 - true if the EKF has ever been healthy - bool rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed data - bool dead_reckoning : 1; // 18 - true if we are dead reckoning (e.g. no position or velocity source) - } flags; + nav_filter_status_flags_t flags; uint32_t value; }; From c8e22aea8967bd45c2f49c5ba4f1232c0f891033 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 13:28:13 +1100 Subject: [PATCH 02/16] AP_ExternalAHRS: fixed spelling of initialized --- libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp | 4 ++-- libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index ed6304429c28d..5aaf2d0982075 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -250,7 +250,7 @@ void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status) { memset(&status, 0, sizeof(status)); if (last_imu_pkt != 0 && last_gps_pkt != 0) { - status.flags.initalized = true; + status.flags.initialized = true; } if (healthy() && last_imu_pkt != 0) { status.flags.attitude = true; @@ -304,7 +304,7 @@ void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const if (filterStatus.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } - if (!filterStatus.flags.initalized) { + if (!filterStatus.flags.initialized) { flags |= EKF_UNINITIALIZED; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 179f05fd9f1e2..da1c32674f696 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -718,7 +718,7 @@ void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) con memset(&status, 0, sizeof(status)); if (type == TYPE::VN_300) { if (last_pkt1 && last_pkt2) { - status.flags.initalized = true; + status.flags.initialized = true; } if (healthy() && last_pkt2) { status.flags.attitude = true; @@ -736,7 +736,7 @@ void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) con } } } else { - status.flags.initalized = initialised(); + status.flags.initialized = initialised(); if (healthy()) { status.flags.attitude = true; } @@ -783,7 +783,7 @@ void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const if (filterStatus.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } - if (!filterStatus.flags.initalized) { + if (!filterStatus.flags.initialized) { flags |= EKF_UNINITIALIZED; } From 165195681aa767845db8b88a2f94b5d3b1da1e9c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 13:28:13 +1100 Subject: [PATCH 03/16] AP_NavEKF2: fixed spelling of initialized --- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 459d3de3983a3..07232ffe35d5b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -523,7 +523,7 @@ void NavEKF2_core::updateFilterStatus(void) filterStatus.flags.rejecting_airspeed = lastTasFailTime_ms != 0 && (imuSampleTime_ms - lastTasFailTime_ms) < 1000 && (imuSampleTime_ms - lastTasPassTime_ms) > 3000; - filterStatus.flags.initalized = filterStatus.flags.initalized || healthy(); + filterStatus.flags.initialized = filterStatus.flags.initialized || healthy(); } void NavEKF2_core::runYawEstimatorPrediction() diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 32820bd9ddd71..9594c43609612 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -531,7 +531,7 @@ void NavEKF2_core::send_status_report(GCS_MAVLINK &link) const if (filterStatus.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } - if (!filterStatus.flags.initalized) { + if (!filterStatus.flags.initialized) { flags |= EKF_UNINITIALIZED; } From 8de0526d312ae40bb0e231547b24e2e33c5057e7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 13:28:14 +1100 Subject: [PATCH 04/16] AP_NavEKF3: fixed spelling of initialized --- libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index d2179f348cbed..fc9508efb4e74 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -739,7 +739,7 @@ void NavEKF3_core::updateFilterStatus(void) status.flags.rejecting_airspeed = lastTasFailTime_ms != 0 && (imuSampleTime_ms - lastTasFailTime_ms) < 1000 && (imuSampleTime_ms - lastTasPassTime_ms) > 3000; - status.flags.initalized = status.flags.initalized || healthy(); + status.flags.initialized = status.flags.initialized || healthy(); status.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav); filterStatus.value = status.value; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index 04bf81ec20829..e1d992e5ceb48 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -595,7 +595,7 @@ void NavEKF3_core::send_status_report(GCS_MAVLINK &link) const if (filterStatus.flags.pred_horiz_pos_abs) { flags |= EKF_PRED_POS_HORIZ_ABS; } - if (!filterStatus.flags.initalized) { + if (!filterStatus.flags.initialized) { flags |= EKF_UNINITIALIZED; } if (filterStatus.flags.gps_glitching) { From 8602280cff0138ef0d6bc5c23d75678207a93855 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Dec 2023 13:57:44 +1100 Subject: [PATCH 05/16] AP_Airspeed: support external AHRS airspeed sensor --- libraries/AP_Airspeed/AP_Airspeed.cpp | 2 ++ libraries/AP_Airspeed/AP_Airspeed.h | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index a5d94d31aacdf..eabeaea5ca777 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -672,6 +672,8 @@ void AP_Airspeed::read(uint8_t i) // read all airspeed sensors void AP_Airspeed::update() { + WITH_SEMAPHORE(sem); + if (!lib_enabled()) { return; } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 8f6666f62b822..1c844f44977be 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -225,8 +225,14 @@ class AP_Airspeed // get aggregate calibration state for the Airspeed library: CalibrationState get_calibration_state() const; + // allow threads to lock against update + HAL_Semaphore &get_semaphore(void) { + return sem; + } + private: static AP_Airspeed *_singleton; + HAL_Semaphore sem; AP_Int8 _enable; bool lib_enabled() const; From d2e86c9c0d5ad3e61c772c4ddd2d2bf27296524d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 18:29:59 +1100 Subject: [PATCH 06/16] AP_Compass: fixed race in external AHRS compass via scripting --- libraries/AP_Compass/AP_Compass.cpp | 1 + libraries/AP_Compass/AP_Compass.h | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 039ade2d5a70e..0c88c8a3d1c45 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -1717,6 +1717,7 @@ Compass::read(void) if (!available()) { return false; } + WITH_SEMAPHORE(sem); #if HAL_LOGGING_ENABLED const bool old_healthy = healthy(); diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index f08a63e81c96b..58448e5e2c866 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -365,8 +365,14 @@ friend class AP_Compass_Backend; // get the first compass marked for use by COMPASSx_USE uint8_t get_first_usable(void) const { return _first_usable; } + // allow threads to lock against update + HAL_Semaphore &get_semaphore(void) { + return sem; + } + private: static Compass *_singleton; + HAL_Semaphore sem; // Use Priority and StateIndex typesafe index types // to distinguish between two different type of indexing From 842a60e2140e83a81c41d37971a47ef26f4d1ada Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Dec 2023 15:57:21 +1100 Subject: [PATCH 07/16] SITL: added randomness to ILabs baro simulation this prevents an unhealthy error due to no change --- libraries/SITL/SIM_InertialLabs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_InertialLabs.cpp b/libraries/SITL/SIM_InertialLabs.cpp index edaadf0b8756d..5be35255dd9d0 100644 --- a/libraries/SITL/SIM_InertialLabs.cpp +++ b/libraries/SITL/SIM_InertialLabs.cpp @@ -42,7 +42,7 @@ void InertialLabs::send_packet(void) float sigma, delta, theta; AP_Baro::SimpleAtmosphere((fdm.altitude+rand_float()*0.25) * 0.001, sigma, delta, theta); pkt.baro_data.pressure_pa2 = SSL_AIR_PRESSURE * delta * 0.5; - pkt.baro_data.baro_alt = fdm.altitude; + pkt.baro_data.baro_alt = fdm.altitude+rand_float()*0.25; pkt.temperature = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta); pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS)*0.1; From 553f277a3a9c9bf18e499188a223a1119755db4d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Dec 2023 09:00:30 +1100 Subject: [PATCH 08/16] AP_ExternalAHRS: added scripting backend and a common implementation of mavlink message send --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 15 +++- libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 14 +++ .../AP_ExternalAHRS_InertialLabs.cpp | 54 ++--------- .../AP_ExternalAHRS_InertialLabs.h | 1 - .../AP_ExternalAHRS_MicroStrain5.cpp | 51 +---------- .../AP_ExternalAHRS_MicroStrain5.h | 1 - .../AP_ExternalAHRS_MicroStrain7.cpp | 57 ++---------- .../AP_ExternalAHRS_MicroStrain7.h | 1 - .../AP_ExternalAHRS_Scripting.cpp | 90 +++++++++++++++++++ .../AP_ExternalAHRS_Scripting.h | 61 +++++++++++++ .../AP_ExternalAHRS_VectorNav.cpp | 65 +++----------- .../AP_ExternalAHRS_VectorNav.h | 1 - .../AP_ExternalAHRS_backend.cpp | 51 +++++++++++ .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 7 +- .../AP_ExternalAHRS/AP_ExternalAHRS_config.h | 5 ++ 15 files changed, 269 insertions(+), 205 deletions(-) create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.cpp create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.h diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index dc3ff2a34b268..7630e39dddb78 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -26,6 +26,7 @@ #include "AP_ExternalAHRS_MicroStrain5.h" #include "AP_ExternalAHRS_MicroStrain7.h" #include "AP_ExternalAHRS_InertialLabs.h" +#include "AP_ExternalAHRS_Scripting.h" #include #include @@ -129,6 +130,12 @@ void AP_ExternalAHRS::init(void) return; #endif +#if AP_EXTERNAL_AHRS_SCRIPTING_ENABLED + case DevType::Scripting: + backend = new AP_ExternalAHRS_Scripting(this, state); + return; +#endif + } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype)); @@ -278,7 +285,7 @@ bool AP_ExternalAHRS::get_accel(Vector3f &accel) void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const { if (backend) { - backend->send_status_report(link); + backend->send_EKF_status_report(link); } } @@ -347,6 +354,12 @@ const char* AP_ExternalAHRS::get_name() const return nullptr; } +// handle input from scripting backend +bool AP_ExternalAHRS::handle_scripting(const state_t &_state, nav_filter_status_flags_t &_filter_status) +{ + return backend->handle_scripting(_state, _filter_status); +} + namespace AP { AP_ExternalAHRS &externalAHRS() diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index 72e3161f799de..e8b19912beeec 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -51,6 +51,9 @@ class AP_ExternalAHRS { #endif #if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED InertialLabs = 5, +#endif +#if AP_EXTERNAL_AHRS_SCRIPTING_ENABLED + Scripting = 10, #endif // 3 reserved for AdNav // 4 reserved for CINS @@ -100,6 +103,14 @@ class AP_ExternalAHRS { bool have_location; bool have_velocity; + // variances for EKF_STATUS_REPORT + float velocity_variance; + float pos_horiz_variance; + float pos_vert_variance; + float compass_variance; + float terrain_alt_variance; + float airspeed_variance; + uint32_t last_location_update_us; } state; @@ -122,6 +133,9 @@ class AP_ExternalAHRS { // update backend void update(); + // handle input from scripting backend + bool handle_scripting(const state_t &state, nav_filter_status_flags_t &filter_status); + /* structures passed to other subsystems */ diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp index 43e858a74007c..caa3dc8e045ba 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -622,7 +622,7 @@ void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status) const uint32_t dt_limit_gps = 500; memset(&status, 0, sizeof(status)); const bool init_ok = (state2.unit_status & (ILABS_UNIT_STATUS_ALIGNMENT_FAIL|ILABS_UNIT_STATUS_OPERATION_FAIL))==0; - status.flags.initalized = init_ok; + status.flags.initialized = init_ok; status.flags.attitude = init_ok && (now - last_att_ms < dt_limit) && init_ok; status.flags.vert_vel = init_ok && (now - last_vel_ms < dt_limit); status.flags.vert_pos = init_ok && (now - last_pos_ms < dt_limit); @@ -637,59 +637,15 @@ void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status) (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) != 0 && (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) == 0; status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL); -} - -// send an EKF_STATUS message to GCS -void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const -{ - // prepare flags - uint16_t flags = 0; - nav_filter_status filterStatus; - get_filter_status(filterStatus); - if (filterStatus.flags.attitude) { - flags |= EKF_ATTITUDE; - } - if (filterStatus.flags.horiz_vel) { - flags |= EKF_VELOCITY_HORIZ; - } - if (filterStatus.flags.vert_vel) { - flags |= EKF_VELOCITY_VERT; - } - if (filterStatus.flags.horiz_pos_rel) { - flags |= EKF_POS_HORIZ_REL; - } - if (filterStatus.flags.horiz_pos_abs) { - flags |= EKF_POS_HORIZ_ABS; - } - if (filterStatus.flags.vert_pos) { - flags |= EKF_POS_VERT_ABS; - } - if (filterStatus.flags.terrain_alt) { - flags |= EKF_POS_VERT_AGL; - } - if (filterStatus.flags.const_pos_mode) { - flags |= EKF_CONST_POS_MODE; - } - if (filterStatus.flags.pred_horiz_pos_rel) { - flags |= EKF_PRED_POS_HORIZ_REL; - } - if (filterStatus.flags.pred_horiz_pos_abs) { - flags |= EKF_PRED_POS_HORIZ_ABS; - } - if (!filterStatus.flags.initalized) { - flags |= EKF_UNINITIALIZED; - } - // send message const float vel_gate = 5; const float pos_gate = 5; const float hgt_gate = 5; const float mag_var = 0; - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - state2.kf_vel_covariance.length()/vel_gate, - state2.kf_pos_covariance.xy().length()/pos_gate, - state2.kf_pos_covariance.z/hgt_gate, - mag_var, 0, 0); + state.velocity_variance = state2.kf_vel_covariance.length()/vel_gate; + state.pos_horiz_variance = state2.kf_pos_covariance.xy().length()/pos_gate; + state.pos_vert_variance = state2.kf_pos_covariance.z/hgt_gate; + state.compass_variance = mag_var; } #endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index bee7d5c28e0cc..5792f31cd14a3 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -37,7 +37,6 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { bool initialised(void) const override; bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; void get_filter_status(nav_filter_status &status) const override; - void send_status_report(class GCS_MAVLINK &link) const override; // check for new data void update() override { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index 5aaf2d0982075..a617dfd062e36 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -266,58 +266,15 @@ void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status) status.flags.using_gps = true; } } -} - -void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const -{ - // prepare flags - uint16_t flags = 0; - nav_filter_status filterStatus; - get_filter_status(filterStatus); - if (filterStatus.flags.attitude) { - flags |= EKF_ATTITUDE; - } - if (filterStatus.flags.horiz_vel) { - flags |= EKF_VELOCITY_HORIZ; - } - if (filterStatus.flags.vert_vel) { - flags |= EKF_VELOCITY_VERT; - } - if (filterStatus.flags.horiz_pos_rel) { - flags |= EKF_POS_HORIZ_REL; - } - if (filterStatus.flags.horiz_pos_abs) { - flags |= EKF_POS_HORIZ_ABS; - } - if (filterStatus.flags.vert_pos) { - flags |= EKF_POS_VERT_ABS; - } - if (filterStatus.flags.terrain_alt) { - flags |= EKF_POS_VERT_AGL; - } - if (filterStatus.flags.const_pos_mode) { - flags |= EKF_CONST_POS_MODE; - } - if (filterStatus.flags.pred_horiz_pos_rel) { - flags |= EKF_PRED_POS_HORIZ_REL; - } - if (filterStatus.flags.pred_horiz_pos_abs) { - flags |= EKF_PRED_POS_HORIZ_ABS; - } - if (!filterStatus.flags.initialized) { - flags |= EKF_UNINITIALIZED; - } - // send message const float vel_gate = 4; // represents hz value data is posted at const float pos_gate = 4; // represents hz value data is posted at const float hgt_gate = 4; // represents hz value data is posted at const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - gnss_data[gnss_instance].speed_accuracy/vel_gate, gnss_data[gnss_instance].horizontal_position_accuracy/pos_gate, gnss_data[gnss_instance].vertical_position_accuracy/hgt_gate, - mag_var, 0, 0); - + state.velocity_variance = gnss_data[0].speed_accuracy/vel_gate; + state.pos_horiz_variance = gnss_data[0].horizontal_position_accuracy/pos_gate; + state.pos_vert_variance = gnss_data[0].vertical_position_accuracy/hgt_gate; + state.compass_variance = mag_var; } - #endif // AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h index e6b2b9e6bdcb1..66541bd9a58e0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h @@ -42,7 +42,6 @@ class AP_ExternalAHRS_MicroStrain5: public AP_ExternalAHRS_backend, public AP_Mi bool initialised(void) const override; bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; void get_filter_status(nav_filter_status &status) const override; - void send_status_report(class GCS_MAVLINK &link) const override; // check for new data void update() override { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 09762f67d6aa8..afe4924869481 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -296,7 +296,7 @@ void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status) { memset(&status, 0, sizeof(status)); if (last_imu_pkt != 0 && last_gps_pkt != 0) { - status.flags.initalized = true; + status.flags.initialized = true; } if (healthy() && last_imu_pkt != 0) { status.flags.attitude = true; @@ -313,60 +313,17 @@ void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status) status.flags.using_gps = true; } } -} - -void AP_ExternalAHRS_MicroStrain7::send_status_report(GCS_MAVLINK &link) const -{ - // prepare flags - uint16_t flags = 0; - nav_filter_status filterStatus; - get_filter_status(filterStatus); - if (filterStatus.flags.attitude) { - flags |= EKF_ATTITUDE; - } - if (filterStatus.flags.horiz_vel) { - flags |= EKF_VELOCITY_HORIZ; - } - if (filterStatus.flags.vert_vel) { - flags |= EKF_VELOCITY_VERT; - } - if (filterStatus.flags.horiz_pos_rel) { - flags |= EKF_POS_HORIZ_REL; - } - if (filterStatus.flags.horiz_pos_abs) { - flags |= EKF_POS_HORIZ_ABS; - } - if (filterStatus.flags.vert_pos) { - flags |= EKF_POS_VERT_ABS; - } - if (filterStatus.flags.terrain_alt) { - flags |= EKF_POS_VERT_AGL; - } - if (filterStatus.flags.const_pos_mode) { - flags |= EKF_CONST_POS_MODE; - } - if (filterStatus.flags.pred_horiz_pos_rel) { - flags |= EKF_PRED_POS_HORIZ_REL; - } - if (filterStatus.flags.pred_horiz_pos_abs) { - flags |= EKF_PRED_POS_HORIZ_ABS; - } - if (!filterStatus.flags.initalized) { - flags |= EKF_UNINITIALIZED; - } - // send message + // TODO fix to use NED filter speed accuracy instead of first gnss + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_ned_vel_uncertainty.htm const float vel_gate = 4; // represents hz value data is posted at const float pos_gate = 4; // represents hz value data is posted at const float hgt_gate = 4; // represents hz value data is posted at const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav - - // TODO fix to use NED filter speed accuracy instead of first gnss - // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_ned_vel_uncertainty.htm - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - gnss_data[0].speed_accuracy/vel_gate, gnss_data[0].horizontal_position_accuracy/pos_gate, gnss_data[0].vertical_position_accuracy/hgt_gate, - mag_var, 0, 0); - + state.velocity_variance = gnss_data[0].speed_accuracy/vel_gate; + state.pos_horiz_variance = gnss_data[0].horizontal_position_accuracy/pos_gate; + state.pos_vert_variance = gnss_data[0].vertical_position_accuracy/hgt_gate; + state.compass_variance = mag_var; } bool AP_ExternalAHRS_MicroStrain7::filter_state_healthy(FilterState state) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h index d9e0832272715..83f2beecf78d4 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h @@ -42,7 +42,6 @@ class AP_ExternalAHRS_MicroStrain7: public AP_ExternalAHRS_backend, public AP_Mi bool initialised(void) const override; bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; void get_filter_status(nav_filter_status &status) const override; - void send_status_report(class GCS_MAVLINK &link) const override; // check for new data void update() override diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.cpp new file mode 100644 index 0000000000000..b4577654df76f --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.cpp @@ -0,0 +1,90 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + support for scripting based external AHRS + */ + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_SCRIPTING_ENABLED + +#include "AP_ExternalAHRS_Scripting.h" + +extern const AP_HAL::HAL &hal; + +// accessors for AP_AHRS +bool AP_ExternalAHRS_Scripting::healthy(void) const +{ + WITH_SEMAPHORE(state.sem); + return AP_HAL::millis() - last_update_ms < 100; +} + +bool AP_ExternalAHRS_Scripting::initialised(void) const +{ + WITH_SEMAPHORE(state.sem); + return last_update_ms != 0; +} + +// handle input from scripting backend +bool AP_ExternalAHRS_Scripting::handle_scripting(const AP_ExternalAHRS::state_t &_state, nav_filter_status_flags_t &_filter_status) +{ + WITH_SEMAPHORE(state.sem); + + state.accel = _state.accel; + state.gyro = _state.gyro; + + if (_state.have_quaternion) { + state.quat = _state.quat; + } + + if (_state.have_location) { + state.location = _state.location; + } + + if (_state.have_velocity) { + state.velocity = _state.velocity; + } + + if (_state.have_origin && !state.have_origin) { + state.origin = _state.origin; + state.have_origin = _state.have_origin; + } + + state.have_quaternion = _state.have_quaternion; + state.have_location = _state.have_location; + state.have_velocity = _state.have_velocity; + + filter_status.flags = _filter_status; + + last_update_ms = AP_HAL::millis(); + return true; +} + +/* + get filter status. We don't know the meaning of the status bits yet, + so assume all OK if we have GPS lock + */ +void AP_ExternalAHRS_Scripting::get_filter_status(nav_filter_status &status) const +{ + WITH_SEMAPHORE(state.sem); + if (healthy()) { + status = filter_status; + } else { + memset(&status, 0, sizeof(status)); + } +} + +#endif // AP_EXTERNAL_AHRS_SCRIPTING_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.h new file mode 100644 index 0000000000000..ffe1fefd45a8b --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_Scripting.h @@ -0,0 +1,61 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + support for scripting based external AHRS + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_SCRIPTING_ENABLED + +#include "AP_ExternalAHRS_backend.h" + +class AP_ExternalAHRS_Scripting : public AP_ExternalAHRS_backend { + +public: + using AP_ExternalAHRS_backend::AP_ExternalAHRS_backend; + + // get serial port number, assume first port + int8_t get_port(void) const override { return 0; } + + // accessors for AP_AHRS + bool healthy(void) const override; + bool initialised(void) const override; + bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override { + // do pre-arm via scripting arming checks + return true; + } + void get_filter_status(nav_filter_status &status) const override; + + // check for new data, done by script + void update() override {} + + // Get model/type name + const char* get_name() const override { + return "Scripting"; + } + + // handle input from scripting backend + bool handle_scripting(const AP_ExternalAHRS::state_t &_state, nav_filter_status_flags_t &_filter_status) override; + +private: + uint32_t last_update_ms; + nav_filter_status filter_status; +}; + +#endif // AP_EXTERNAL_AHRS_SCRIPTING_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index da1c32674f696..e6d912317d6a7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -715,6 +715,8 @@ bool AP_ExternalAHRS_VectorNav::pre_arm_check(char *failure_msg, uint8_t failure */ void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) const { + WITH_SEMAPHORE(state.sem); + memset(&status, 0, sizeof(status)); if (type == TYPE::VN_300) { if (last_pkt1 && last_pkt2) { @@ -741,61 +743,18 @@ void AP_ExternalAHRS_VectorNav::get_filter_status(nav_filter_status &status) con status.flags.attitude = true; } } -} -// send an EKF_STATUS message to GCS -void AP_ExternalAHRS_VectorNav::send_status_report(GCS_MAVLINK &link) const -{ - if (!last_pkt1) { - return; - } - // prepare flags - uint16_t flags = 0; - nav_filter_status filterStatus; - get_filter_status(filterStatus); - if (filterStatus.flags.attitude) { - flags |= EKF_ATTITUDE; - } - if (filterStatus.flags.horiz_vel) { - flags |= EKF_VELOCITY_HORIZ; - } - if (filterStatus.flags.vert_vel) { - flags |= EKF_VELOCITY_VERT; - } - if (filterStatus.flags.horiz_pos_rel) { - flags |= EKF_POS_HORIZ_REL; + if (last_pkt1) { + const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)last_pkt1; + const float vel_gate = 5; + const float pos_gate = 5; + const float hgt_gate = 5; + const float mag_var = 0; + state.velocity_variance = pkt1.velU/vel_gate; + state.pos_horiz_variance = pkt1.posU/pos_gate; + state.pos_vert_variance = pkt1.posU/hgt_gate; + state.compass_variance = mag_var; } - if (filterStatus.flags.horiz_pos_abs) { - flags |= EKF_POS_HORIZ_ABS; - } - if (filterStatus.flags.vert_pos) { - flags |= EKF_POS_VERT_ABS; - } - if (filterStatus.flags.terrain_alt) { - flags |= EKF_POS_VERT_AGL; - } - if (filterStatus.flags.const_pos_mode) { - flags |= EKF_CONST_POS_MODE; - } - if (filterStatus.flags.pred_horiz_pos_rel) { - flags |= EKF_PRED_POS_HORIZ_REL; - } - if (filterStatus.flags.pred_horiz_pos_abs) { - flags |= EKF_PRED_POS_HORIZ_ABS; - } - if (!filterStatus.flags.initialized) { - flags |= EKF_UNINITIALIZED; - } - - // send message - const struct VN_packet1 &pkt1 = *(struct VN_packet1 *)last_pkt1; - const float vel_gate = 5; - const float pos_gate = 5; - const float hgt_gate = 5; - const float mag_var = 0; - mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - pkt1.velU/vel_gate, pkt1.posU/pos_gate, pkt1.posU/hgt_gate, - mag_var, 0, 0); } #endif // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index 3ce1fe7f5e2d5..dcb011543ebe1 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -37,7 +37,6 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { bool initialised(void) const override; bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; void get_filter_status(nav_filter_status &status) const override; - void send_status_report(class GCS_MAVLINK &link) const override; // check for new data void update() override { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp index 94cf8c41b7d2b..e1f70d70f1d0f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.cpp @@ -21,6 +21,8 @@ #if HAL_EXTERNAL_AHRS_ENABLED +#include + AP_ExternalAHRS_backend::AP_ExternalAHRS_backend(AP_ExternalAHRS *_frontend, AP_ExternalAHRS::state_t &_state) : frontend(*_frontend), @@ -43,5 +45,54 @@ bool AP_ExternalAHRS_backend::in_fly_forward(void) const return AP::ahrs().get_fly_forward(); } +// send an EKF_STATUS message to GCS +void AP_ExternalAHRS_backend::send_EKF_status_report(class GCS_MAVLINK &link) +{ + // prepare flags + uint16_t flags = 0; + nav_filter_status filterStatus {}; + get_filter_status(filterStatus); + if (filterStatus.flags.attitude) { + flags |= EKF_ATTITUDE; + } + if (filterStatus.flags.horiz_vel) { + flags |= EKF_VELOCITY_HORIZ; + } + if (filterStatus.flags.vert_vel) { + flags |= EKF_VELOCITY_VERT; + } + if (filterStatus.flags.horiz_pos_rel) { + flags |= EKF_POS_HORIZ_REL; + } + if (filterStatus.flags.horiz_pos_abs) { + flags |= EKF_POS_HORIZ_ABS; + } + if (filterStatus.flags.vert_pos) { + flags |= EKF_POS_VERT_ABS; + } + if (filterStatus.flags.terrain_alt) { + flags |= EKF_POS_VERT_AGL; + } + if (filterStatus.flags.const_pos_mode) { + flags |= EKF_CONST_POS_MODE; + } + if (filterStatus.flags.pred_horiz_pos_rel) { + flags |= EKF_PRED_POS_HORIZ_REL; + } + if (filterStatus.flags.pred_horiz_pos_abs) { + flags |= EKF_PRED_POS_HORIZ_ABS; + } + if (!filterStatus.flags.initialized) { + flags |= EKF_UNINITIALIZED; + } + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, + state.velocity_variance, + state.pos_horiz_variance, + state.pos_vert_variance, + state.compass_variance, + state.terrain_alt_variance, + state.airspeed_variance); +} + #endif // HAL_EXTERNAL_AHRS_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index ed987a01ced3a..5611b40361fc1 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -41,13 +41,18 @@ class AP_ExternalAHRS_backend { virtual bool initialised(void) const = 0; virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0; virtual void get_filter_status(nav_filter_status &status) const {} - virtual void send_status_report(class GCS_MAVLINK &link) const {} // Check for new data. // This is used when there's not a separate thread for EAHRS. // This can also copy interim state protected by locking. virtual void update() = 0; + // handle input from scripting backend + virtual bool handle_scripting(const AP_ExternalAHRS::state_t &_state, nav_filter_status_flags_t &_filter_status) { return false; } + + // send an EKF_STATUS message to GCS + void send_EKF_status_report(class GCS_MAVLINK &link); + protected: AP_ExternalAHRS::state_t &state; uint16_t get_rate(void) const; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index 75e8e5228021b..c4e0d16d9aed4 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -1,6 +1,7 @@ #pragma once #include +#include #ifndef HAL_EXTERNAL_AHRS_ENABLED #define HAL_EXTERNAL_AHRS_ENABLED BOARD_FLASH_SIZE > 1024 @@ -29,3 +30,7 @@ #ifndef AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED #define AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_EXTERNAL_AHRS_SCRIPTING_ENABLED +#define AP_EXTERNAL_AHRS_SCRIPTING_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED +#endif From 3f3325cc5949407db48f16673a89cda7baf0fa2a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 19:32:26 +1100 Subject: [PATCH 09/16] AP_ExternalAHRS: added variance to EAHR logging common logging for all EAHRS backends --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 7630e39dddb78..f9bc6debbbfbd 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -326,21 +326,26 @@ void AP_ExternalAHRS::update(void) // @Field: Lon: longitude // @Field: Alt: altitude AMSL // @Field: Flg: nav status flags + // @Field: VVar: velocity variance + // @Field: PVar: position variance + // @Field: HVar: height variance float roll, pitch, yaw; state.quat.to_euler(roll, pitch, yaw); nav_filter_status filterStatus {}; get_filter_status(filterStatus); - AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg", - "sdddnnnDUm-", - "F000000GG0-", - "QffffffLLfI", + AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg,VVar,PVar,HVar", + "sdddnnnDUm-nmm", + "F000000GG0-000", + "QffffffLLfIfff", AP_HAL::micros64(), degrees(roll), degrees(pitch), degrees(yaw), state.velocity.x, state.velocity.y, state.velocity.z, state.location.lat, state.location.lng, state.location.alt*0.01, - filterStatus.value); + filterStatus.value, + state.velocity_variance, state.pos_horiz_variance, + state.pos_vert_variance); } #endif // HAL_LOGGING_ENABLED } From 58bb475532861ee30c49b6680b18dfd69b9a66b6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 16:13:04 +1100 Subject: [PATCH 10/16] AP_Baro: fixed calibration of external AHRS barometers --- libraries/AP_Baro/AP_Baro_Backend.cpp | 13 +++++++++++++ libraries/AP_Baro/AP_Baro_Backend.h | 5 +++++ libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp | 3 +++ 3 files changed, 21 insertions(+) diff --git a/libraries/AP_Baro/AP_Baro_Backend.cpp b/libraries/AP_Baro/AP_Baro_Backend.cpp index f315bcd2b9726..71ae3b0f1a8e5 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.cpp +++ b/libraries/AP_Baro/AP_Baro_Backend.cpp @@ -99,3 +99,16 @@ bool AP_Baro_Backend::pressure_ok(float press) } return ret; } + +/* + set an instance as calibrated + */ +void AP_Baro_Backend::set_calibrated(uint8_t instance, float pressure) +{ + if (!_frontend.sensors[instance].calibrated) { + // ahrs external baro may not update at boot, so mark + // calibrated on first reading + _frontend.sensors[instance].ground_pressure.set_and_save(pressure); + _frontend.sensors[instance].calibrated = true; + } +} diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 94b155e4836bd..b5abdb9906676 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -80,4 +80,9 @@ class AP_Baro_Backend void set_bus_id(uint8_t instance, uint32_t id) { _frontend.sensors[instance].bus_id.set(int32_t(id)); } + + /* + set an instance as calibrated + */ + void set_calibrated(uint8_t instance, float pressure); }; diff --git a/libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp b/libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp index 79a3c8bd3be80..1da4a06ac1134 100644 --- a/libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp +++ b/libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp @@ -15,6 +15,9 @@ void AP_Baro_ExternalAHRS::update(void) if (count) { WITH_SEMAPHORE(_sem); _copy_to_frontend(instance, sum_pressure/count, sum_temp/count); + + set_calibrated(instance, sum_pressure/count); + sum_pressure = sum_temp = 0; count = 0; } From cb48532a7482d5154871879f5202545b5d601bd0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 18:30:27 +1100 Subject: [PATCH 11/16] AP_GPS: fixed race condition in external AHRS GPS --- libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp | 44 +++++++++++++----------- libraries/AP_GPS/AP_GPS_ExternalAHRS.h | 2 ++ 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp index e19e433055ceb..57bd0b198bea3 100644 --- a/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp +++ b/libraries/AP_GPS/AP_GPS_ExternalAHRS.cpp @@ -26,8 +26,10 @@ // the caller is consuming the new data; bool AP_GPS_ExternalAHRS::read(void) { + WITH_SEMAPHORE(sem); if (new_data) { new_data = false; + state = interim_state; return true; } return false; @@ -37,43 +39,45 @@ bool AP_GPS_ExternalAHRS::read(void) // corresponding gps data appropriately; void AP_GPS_ExternalAHRS::handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt) { + WITH_SEMAPHORE(sem); + check_new_itow(pkt.ms_tow, sizeof(pkt)); - state.time_week = pkt.gps_week; - state.time_week_ms = pkt.ms_tow; + interim_state.time_week = pkt.gps_week; + interim_state.time_week_ms = pkt.ms_tow; if (pkt.fix_type == 0) { - state.status = AP_GPS::NO_FIX; + interim_state.status = AP_GPS::NO_FIX; } else { - state.status = (AP_GPS::GPS_Status)pkt.fix_type; + interim_state.status = (AP_GPS::GPS_Status)pkt.fix_type; } - state.num_sats = pkt.satellites_in_view; + interim_state.num_sats = pkt.satellites_in_view; Location loc = {}; loc.lat = pkt.latitude; loc.lng = pkt.longitude; loc.alt = pkt.msl_altitude; - state.location = loc; - state.hdop = pkt.hdop; - state.vdop = pkt.vdop; + interim_state.location = loc; + interim_state.hdop = pkt.hdop; + interim_state.vdop = pkt.vdop; - state.have_vertical_velocity = true; - state.velocity.x = pkt.ned_vel_north; - state.velocity.y = pkt.ned_vel_east; - state.velocity.z = pkt.ned_vel_down; + interim_state.have_vertical_velocity = true; + interim_state.velocity.x = pkt.ned_vel_north; + interim_state.velocity.y = pkt.ned_vel_east; + interim_state.velocity.z = pkt.ned_vel_down; velocity_to_speed_course(state); - state.have_speed_accuracy = true; - state.have_horizontal_accuracy = true; - state.have_vertical_accuracy = true; - state.have_vertical_velocity = true; + interim_state.have_speed_accuracy = true; + interim_state.have_horizontal_accuracy = true; + interim_state.have_vertical_accuracy = true; + interim_state.have_vertical_velocity = true; - state.horizontal_accuracy = pkt.horizontal_pos_accuracy; - state.vertical_accuracy = pkt.vertical_pos_accuracy; - state.speed_accuracy = pkt.horizontal_vel_accuracy; + interim_state.horizontal_accuracy = pkt.horizontal_pos_accuracy; + interim_state.vertical_accuracy = pkt.vertical_pos_accuracy; + interim_state.speed_accuracy = pkt.horizontal_vel_accuracy; - state.last_gps_time_ms = AP_HAL::millis(); + interim_state.last_gps_time_ms = AP_HAL::millis(); new_data = true; } diff --git a/libraries/AP_GPS/AP_GPS_ExternalAHRS.h b/libraries/AP_GPS/AP_GPS_ExternalAHRS.h index b046748034e62..2a2ef808f86e6 100644 --- a/libraries/AP_GPS/AP_GPS_ExternalAHRS.h +++ b/libraries/AP_GPS/AP_GPS_ExternalAHRS.h @@ -41,6 +41,8 @@ class AP_GPS_ExternalAHRS : public AP_GPS_Backend private: bool new_data; + AP_GPS::GPS_State interim_state; + HAL_Semaphore sem; }; #endif // HAL_EXTERNAL_AHRS_ENABLED From 932f1cb36f15ff5e25ee80064c513a8aac0e1abf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 13:15:10 +1100 Subject: [PATCH 12/16] AP_Scripting: added bindings for scripting external AHRS --- .../generator/description/bindings.desc | 110 ++++++++++++++++++ 1 file changed, 110 insertions(+) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c02040c7098d2..20d07ff35fe73 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -112,6 +112,7 @@ singleton AP_BattMonitor method handle_scripting depends AP_BATTERY_SCRIPTING_EN include AP_GPS/AP_GPS.h singleton AP_GPS depends (AP_GPS_ENABLED && (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_GPS))) +singleton AP_GPS semaphore singleton AP_GPS rename gps singleton AP_GPS enum NO_GPS NO_FIX GPS_OK_FIX_2D GPS_OK_FIX_3D GPS_OK_FIX_3D_DGPS GPS_OK_FIX_3D_RTK_FLOAT GPS_OK_FIX_3D_RTK_FIXED singleton AP_GPS method num_sensors uint8_t @@ -405,6 +406,7 @@ singleton AP_SerialManager method find_serial AP_HAL::UARTDriver AP_SerialManage include AP_Baro/AP_Baro.h singleton AP_Baro depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BARO)) +singleton AP_Baro semaphore singleton AP_Baro rename baro singleton AP_Baro method get_pressure float singleton AP_Baro method get_temperature float @@ -790,6 +792,7 @@ singleton AP_IOMCU method healthy boolean include AP_Compass/AP_Compass.h singleton Compass rename compass singleton Compass depends AP_COMPASS_ENABLED +singleton Compass semaphore singleton Compass method healthy boolean uint8_t'skip_check -- ----EFI Library---- @@ -839,6 +842,113 @@ singleton AP_EFI method get_state void EFI_State'Ref -- ----END EFI Library---- +-- ----ExternalAHRS Library---- +include AP_ExternalAHRS/AP_ExternalAHRS.h depends HAL_EXTERNAL_AHRS_ENABLED +include AP_ExternalAHRS/AP_ExternalAHRS_config.h +include AP_ExternalAHRS/AP_ExternalAHRS_Scripting.h depends AP_EXTERNAL_AHRS_SCRIPTING_ENABLED +include AP_NavEKF/AP_Nav_Common.h +singleton AP_ExternalAHRS rename ExternalAHRS +singleton AP_ExternalAHRS depends (!defined(HAL_BUILD_AP_PERIPH)) + +userdata AP_ExternalAHRS::state_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) +userdata AP_ExternalAHRS::state_t rename EAHRS_State + +userdata AP_ExternalAHRS::state_t field accel Vector3f write +userdata AP_ExternalAHRS::state_t field gyro Vector3f write +userdata AP_ExternalAHRS::state_t field quat Quaternion write +userdata AP_ExternalAHRS::state_t field location Location write +userdata AP_ExternalAHRS::state_t field velocity Vector3f write +userdata AP_ExternalAHRS::state_t field origin Location write +userdata AP_ExternalAHRS::state_t field have_quaternion boolean write +userdata AP_ExternalAHRS::state_t field have_origin boolean write +userdata AP_ExternalAHRS::state_t field have_location boolean write +userdata AP_ExternalAHRS::state_t field have_velocity boolean write +userdata AP_ExternalAHRS::state_t field velocity_variance float'skip_check write +userdata AP_ExternalAHRS::state_t field pos_horiz_variance float'skip_check write +userdata AP_ExternalAHRS::state_t field pos_vert_variance float'skip_check write +userdata AP_ExternalAHRS::state_t field compass_variance float'skip_check write +userdata AP_ExternalAHRS::state_t field terrain_alt_variance float'skip_check write +userdata AP_ExternalAHRS::state_t field airspeed_variance float'skip_check write + +userdata nav_filter_status_flags_t field attitude boolean write +userdata nav_filter_status_flags_t field horiz_vel boolean write +userdata nav_filter_status_flags_t field vert_vel boolean write +userdata nav_filter_status_flags_t field horiz_pos_rel boolean write +userdata nav_filter_status_flags_t field horiz_pos_abs boolean write +userdata nav_filter_status_flags_t field vert_pos boolean write +userdata nav_filter_status_flags_t field terrain_alt boolean write +userdata nav_filter_status_flags_t field const_pos_mode boolean write +userdata nav_filter_status_flags_t field pred_horiz_pos_rel boolean write +userdata nav_filter_status_flags_t field pred_horiz_pos_abs boolean write +userdata nav_filter_status_flags_t field takeoff_detected boolean write +userdata nav_filter_status_flags_t field takeoff boolean write +userdata nav_filter_status_flags_t field touchdown boolean write +userdata nav_filter_status_flags_t field using_gps boolean write +userdata nav_filter_status_flags_t field gps_glitching boolean write +userdata nav_filter_status_flags_t field gps_quality_good boolean write +userdata nav_filter_status_flags_t field initialized boolean write +userdata nav_filter_status_flags_t field rejecting_airspeed boolean write +userdata nav_filter_status_flags_t field dead_reckoning boolean write + +userdata AP_ExternalAHRS::baro_data_message_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) +userdata AP_ExternalAHRS::baro_data_message_t rename baro_data_message_t +userdata AP_ExternalAHRS::baro_data_message_t field instance uint8_t'skip_check write +userdata AP_ExternalAHRS::baro_data_message_t field pressure_pa float'skip_check write +userdata AP_ExternalAHRS::baro_data_message_t field temperature float'skip_check write + +userdata AP_ExternalAHRS::mag_data_message_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) && (!defined(HAL_BUILD_AP_PERIPH)) +userdata AP_ExternalAHRS::mag_data_message_t rename mag_data_message_t +userdata AP_ExternalAHRS::mag_data_message_t field field Vector3f write + +userdata AP_ExternalAHRS::gps_data_message_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) +userdata AP_ExternalAHRS::gps_data_message_t rename gps_data_message_t +userdata AP_ExternalAHRS::gps_data_message_t field gps_week uint16_t'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field ms_tow uint32_t'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field fix_type uint8_t'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field satellites_in_view uint8_t'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field horizontal_pos_accuracy float'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field vertical_pos_accuracy float'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field hdop float'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field vdop float'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field longitude int32_t'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field latitude int32_t'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field msl_altitude int32_t'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field ned_vel_north float'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field ned_vel_east float'skip_check write +userdata AP_ExternalAHRS::gps_data_message_t field ned_vel_down float'skip_check write + +userdata AP_ExternalAHRS::ins_data_message_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) +userdata AP_ExternalAHRS::ins_data_message_t rename ins_data_message_t +userdata AP_ExternalAHRS::ins_data_message_t field accel Vector3f write +userdata AP_ExternalAHRS::ins_data_message_t field gyro Vector3f write +userdata AP_ExternalAHRS::ins_data_message_t field temperature float'skip_check write + +userdata AP_ExternalAHRS::airspeed_data_message_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) +userdata AP_ExternalAHRS::airspeed_data_message_t rename airspeed_data_message_t +userdata AP_ExternalAHRS::airspeed_data_message_t field differential_pressure float'skip_check write +userdata AP_ExternalAHRS::airspeed_data_message_t field temperature float'skip_check write + +singleton AP_ExternalAHRS method handle_scripting boolean AP_ExternalAHRS::state_t nav_filter_status_flags_t +singleton AP_InertialSensor method handle_external void AP_ExternalAHRS::ins_data_message_t +singleton AP_InertialSensor method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_Baro method handle_external void AP_ExternalAHRS::baro_data_message_t +singleton AP_Baro method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_GPS method get_first_external_instance boolean uint8_t'Null +singleton AP_GPS method get_first_external_instance depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_GPS method handle_external void AP_ExternalAHRS::gps_data_message_t uint8_t'skip_check +singleton AP_GPS method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton Compass method handle_external void AP_ExternalAHRS::mag_data_message_t +singleton Compass method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) + +include AP_Airspeed/AP_Airspeed.h +singleton AP_Airspeed depends (AP_AIRSPEED_EXTERNAL_ENABLED == 1) && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI) +singleton AP_Airspeed semaphore +singleton AP_Airspeed rename airspeed +singleton AP_Airspeed method handle_external void AP_ExternalAHRS::airspeed_data_message_t + +-- ----END ExternalAHRS Library--- + + include AP_Logger/AP_Logger.h singleton AP_Logger depends HAL_LOGGING_ENABLED singleton AP_Logger rename logger From e6ba64836a26027f0696998fc7f0d717b0e5738c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 16:27:29 +1100 Subject: [PATCH 13/16] AP_Scripting: added docs for external ahrs --- libraries/AP_Scripting/docs/docs.lua | 323 +++++++++++++++++++++++++++ 1 file changed, 323 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 0807c406d1dfc..e452f6f7b1224 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -340,6 +340,307 @@ function efi:get_state() end ---@return AP_EFI_Backend_ud function efi:get_backend(instance) end +-- desc +---@class airspeed +airspeed = {} + +-- pass in external airspeed data +---@param param1 airspeed_data_message_t_ud +function airspeed:handle_external(param1) end + + +-- desc +---@class ExternalAHRS +ExternalAHRS = {} + +-- desc +---@param param1 EAHRS_State_ud +---@param param2 nav_filter_status_flags_t_ud +---@return boolean +function ExternalAHRS:handle_scripting(param1, param2) end + +-- desc +---@class mag_data_message_t_ud +local mag_data_message_t_ud = {} + +---@return mag_data_message_t_ud +function mag_data_message_t() end + +-- set field +---@param value Vector3f_ud +function mag_data_message_t_ud:field(value) end + +-- desc +---@class baro_data_message_t_ud +local baro_data_message_t_ud = {} + +---@return baro_data_message_t_ud +function baro_data_message_t() end + +-- set field +---@param value number +function baro_data_message_t_ud:temperature(value) end + +-- set field +---@param value number +function baro_data_message_t_ud:pressure_pa(value) end + +-- set field +---@param value integer +function baro_data_message_t_ud:instance(value) end + +-- desc +---@class airspeed_data_message_t_ud +local airspeed_data_message_t_ud = {} + +---@return airspeed_data_message_t_ud +function airspeed_data_message_t() end + +-- set field +---@param value number +function airspeed_data_message_t_ud:temperature(value) end + +-- set field +---@param value number +function airspeed_data_message_t_ud:differential_pressure(value) end + +-- desc +---@class gps_data_message_t_ud +local gps_data_message_t_ud = {} + +---@return gps_data_message_t_ud +function gps_data_message_t() end + +-- set field +---@param value number +function gps_data_message_t_ud:ned_vel_down(value) end + +-- set field +---@param value number +function gps_data_message_t_ud:ned_vel_east(value) end + +-- set field +---@param value number +function gps_data_message_t_ud:ned_vel_north(value) end + +-- set field +---@param value integer +function gps_data_message_t_ud:msl_altitude(value) end + +-- set field +---@param value integer +function gps_data_message_t_ud:latitude(value) end + +-- set field +---@param value integer +function gps_data_message_t_ud:longitude(value) end + +-- set field +---@param value number +function gps_data_message_t_ud:vdop(value) end + +-- set field +---@param value number +function gps_data_message_t_ud:hdop(value) end + +-- set field +---@param value number +function gps_data_message_t_ud:vertical_pos_accuracy(value) end + +-- set field +---@param value number +function gps_data_message_t_ud:horizontal_pos_accuracy(value) end + +-- set field +---@param value integer +function gps_data_message_t_ud:satellites_in_view(value) end + +-- set field +---@param value integer +function gps_data_message_t_ud:fix_type(value) end + +-- set field +---@param value uint32_t_ud +function gps_data_message_t_ud:ms_tow(value) end + +-- set field +---@param value integer +function gps_data_message_t_ud:gps_week(value) end + +-- desc +---@class ins_data_message_t_ud +local ins_data_message_t_ud = {} + +---@return ins_data_message_t_ud +function ins_data_message_t() end + +-- set field +---@param value number +function ins_data_message_t_ud:temperature(value) end + +-- set field +---@param value Vector3f_ud +function ins_data_message_t_ud:gyro(value) end + +-- set field +---@param value Vector3f_ud +function ins_data_message_t_ud:accel(value) end + +-- desc +---@class nav_filter_status_flags_t_ud +local nav_filter_status_flags_t_ud = {} + +---@return nav_filter_status_flags_t_ud +function nav_filter_status_flags_t() end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:dead_reckoning(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:rejecting_airspeed(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:initialized(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:gps_quality_good(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:gps_glitching(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:using_gps(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:touchdown(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:takeoff(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:takeoff_detected(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:pred_horiz_pos_abs(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:pred_horiz_pos_rel(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:const_pos_mode(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:terrain_alt(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:vert_pos(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:horiz_pos_abs(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:horiz_pos_rel(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:vert_vel(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:horiz_vel(value) end + +-- set field +---@param value boolean +function nav_filter_status_flags_t_ud:attitude(value) end + + +-- desc +---@class EAHRS_State_ud +local EAHRS_State_ud = {} + +---@return EAHRS_State_ud +function EAHRS_State() end + +-- set field +---@param value boolean +function EAHRS_State_ud:have_velocity(value) end + +-- set field +---@param value boolean +function EAHRS_State_ud:have_location(value) end + +-- set field +---@param value boolean +function EAHRS_State_ud:have_origin(value) end + +-- set field +---@param value boolean +function EAHRS_State_ud:have_quaternion(value) end + +-- set field +---@param value Location_ud +function EAHRS_State_ud:origin(value) end + +-- set field +---@param value Vector3f_ud +function EAHRS_State_ud:velocity(value) end + +-- set field +---@param value Location_ud +function EAHRS_State_ud:location(value) end + +-- set field +---@param value Quaternion_ud +function EAHRS_State_ud:quat(value) end + +-- set field +---@param value Vector3f_ud +function EAHRS_State_ud:gyro(value) end + +-- set field +---@param value Vector3f_ud +function EAHRS_State_ud:accel(value) end + +-- set field +---@param value float +function EAHRS_State_ud:velocity_variance(value) end + +-- set field +---@param value float +function EAHRS_State_ud:pos_horiz_variance(value) end + +-- set field +---@param value float +function EAHRS_State_ud:pos_vert_variance(value) end + +-- set field +---@param value float +function EAHRS_State_ud:compass_variance(value) end + +-- set field +---@param value float +function EAHRS_State_ud:terrain_alt_variance(value) end + +-- set field +---@param value float +function EAHRS_State_ud:airspeed_variance(value) end + -- CAN bus interaction ---@class CAN CAN = {} @@ -1221,6 +1522,10 @@ compass = {} ---@return boolean function compass:healthy(instance) end +-- add external compass data +---@param param1 mag_data_message_t_ud +function compass:handle_external(param1) end + -- desc ---@class camera camera = {} @@ -1515,6 +1820,7 @@ function ins:accels_consistent(threshold) end ---@return boolean function ins:get_accel_health(instance) end +<<<<<<< HEAD -- Get if the INS is currently calibrating ---@return boolean function ins:calibrating() end @@ -1528,6 +1834,11 @@ function ins:get_gyro(instance) end ---@param instance integer -- the 0-based index of the accelerometer instance to return. ---@return Vector3f_ud function ins:get_accel(instance) end +======= +-- pass in external INS data +---@param param1 ins_data_message_t_ud +function ins:handle_external(param1) end +>>>>>>> 317165a736 (AP_Scripting: added docs for external ahrs) -- desc ---@class Motors_dynamic @@ -2015,6 +2326,9 @@ function baro:get_altitude() end ---@return boolean function baro:healthy(instance) end +-- pass in external baro data +---@param param1 baro_data_message_t_ud +function baro:handle_external(param1) end -- desc ---@class serial @@ -2821,6 +3135,15 @@ function notify:release_text(row) end ---@field NO_GPS number gps = {} +-- pass in external gps data +---@param param1 gps_data_message_t_ud +---@param param2 integer +function gps:handle_external(param1, param2) end + +-- get first external GPS instance +---@return integer|nil +function gps:get_first_external_instance() end + -- desc ---@return integer|nil function gps:first_unconfigured_gps() end From a508dd0afaaf6dd3801c507dbea24e4137929058 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 16:17:16 +1100 Subject: [PATCH 14/16] AP_Scripting: added InertialLabs external AHRS scripting driver --- .../drivers/EAHRS_InertialLabs.lua | 282 ++++++++++++++++++ .../drivers/EAHRS_InertialLabs.md | 29 ++ 2 files changed, 311 insertions(+) create mode 100644 libraries/AP_Scripting/drivers/EAHRS_InertialLabs.lua create mode 100644 libraries/AP_Scripting/drivers/EAHRS_InertialLabs.md diff --git a/libraries/AP_Scripting/drivers/EAHRS_InertialLabs.lua b/libraries/AP_Scripting/drivers/EAHRS_InertialLabs.lua new file mode 100644 index 0000000000000..0184f8b4c23b0 --- /dev/null +++ b/libraries/AP_Scripting/drivers/EAHRS_InertialLabs.lua @@ -0,0 +1,282 @@ +--[[ + Scripting backend driver for InertialLabs external INS +--]] + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +PARAM_TABLE_KEY = 45 +PARAM_TABLE_PREFIX = "ILABS_" + +-- bind a parameter to a variable given +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- Setup Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'ILabs: could not add param table') + +--[[ + // @Param: ILABS_ENABLE + // @DisplayName: Inertial Labs enable + // @Description: Enable InertialLabs driver + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local ILABS_ENABLE = bind_add_param('ENABLE', 1, 0) + +--[[ + // @Param: ILABS_BAUD + // @DisplayName: Inertial Labs uart baudrate + // @Description: Inertial Labs uart baudrate + // @CopyFieldsFrom: SERIAL1_BAUD + // @User: Standard +--]] +local ILABS_BAUD = bind_add_param('BAUD', 2, 460800) + +--[[ + // @Param: ILABS_DEBUG + // @DisplayName: Inertial Labs debug enable + // @Description: Inertial Labs debug enable + // @Values: 0:Disable,1:Enabled + // @User: Advanced +--]] +local ILABS_DEBUG = bind_add_param('DEBUG', 3, 0) + +if ILABS_ENABLE:get() == 0 then + gcs:send_text(MAV_SEVERITY.INFO, "ILabs disabled") + return +end + +local uart = serial:find_serial(0) -- first scripting serial +if not uart then + gcs:send_text(MAV_SEVERITY.ERROR, "ILabs: unable to find scripting serial") + return +end +uart:begin(ILABS_BAUD:get()) + +local GRAVITY_MSS = 9.80665 + +--[[ + discard pending bytes +--]] +local function discard_pending() + local n = uart:available():toint() + for _ = 1, n do + uart:read() + end +end + +local csum = 0 + +--[[ + read n bytes from the uart, returning as a lua string, and updating csum +--]] +local function read_bytes(n) + local ret = uart:readstring(n) + local len = #ret + for i = 1, len do + csum = csum + string.byte(ret, i) + end + return ret +end + +local function scaled_vector3(x,y,z,scale) + local v = Vector3f() + v:x(x) + v:y(y) + v:z(z) + v = v:scale(scale) + return v +end + +local state = EAHRS_State() +local last_pkt_ms = uint32_t(0) + +--[[ + check for incoming data +--]] +local function update() + local n_bytes = uart:available():toint() + -- only handle one packet form at the moment + local pkt_size = 172 + if n_bytes < pkt_size then + return + end + if n_bytes > pkt_size then + discard_pending() + return + end + local magic = string.unpack(" 0 then + local dt = (now_ms - last_pkt_ms):tofloat()*0.001 + if dt > 0.01 then + gcs:send_text(MAV_SEVERITY.INFO, string.format("ILabs: dt=%.3f", dt)) + end + end + last_pkt_ms = now_ms + + local accel = scaled_vector3(accel_y, accel_x, -accel_z, GRAVITY_MSS*1.0e-6) + local gyro = scaled_vector3(gyro_y, gyro_x, -gyro_z, math.rad(1)*1.0e-5) + local quat = Quaternion() + quat:from_euler(math.rad(roll*0.01), math.rad(pitch*0.01), math.rad(yaw*0.01)) + local vel = scaled_vector3(vel_y, vel_x, -vel_z, 0.01) + local loc = Location() + loc:lat(lat) + loc:lng(lon) + loc:alt(alt) + + state:accel(accel) + state:gyro(gyro) + state:quat(quat) + state:velocity(vel) + state:location(loc) + state:origin(loc) + + state:have_quaternion(true) + state:have_velocity(true) + state:have_location(true) + state:have_quaternion(true) + if fix_type > 1 and num_sats > 3 then + state:have_origin(true) + end + + local vel_gate = 5 + local pos_gate = 5 + local hgt_gate = 5 + local mag_var = 0 + state:velocity_variance(scaled_vector3(kf_vel_x,kf_vel_y,kf_vel_z,0.001):length()/vel_gate) + state:pos_horiz_variance(scaled_vector3(kf_pos_x,kf_pos_y,0,0.001):length()/pos_gate) + state:pos_vert_variance(math.abs(kf_pos_z*0.001)/hgt_gate) + state:compass_variance(mag_var) + + local filter_status = nav_filter_status_flags_t() + filter_status:attitude(true) + filter_status:horiz_vel(true) + filter_status:vert_vel(true) + filter_status:horiz_pos_rel(true) + filter_status:horiz_pos_abs(true) + filter_status:vert_pos(true) + filter_status:pred_horiz_pos_rel(true) + filter_status:pred_horiz_pos_abs(true) + filter_status:using_gps(fix_type>1) + filter_status:initialized(true) + + -- provide position, velocity, attitude + ExternalAHRS:handle_scripting(state, filter_status) + + -- provide IMU data (this will be discarded if EAHRS_SENSORS doesn't include IMU) + local ins_data = ins_data_message_t() + ins_data:accel(accel) + ins_data:gyro(gyro) + ins_data:temperature(temperature*0.1) + ins:handle_external(ins_data) + + -- provide baro data + local baro_data = baro_data_message_t() + baro_data:instance(0) + baro_data:pressure_pa(pressure_pa2*2) + baro_data:temperature(temperature*0.1) + baro:handle_external(baro_data) + + -- provide mag data + local mag_data = mag_data_message_t() + mag_data:field(scaled_vector3(mag_y,mag_x,-mag_z,10*0.01)) + compass:handle_external(mag_data) + + -- provide gps data + if gnss_new_data ~= 0 then + local gps_data = gps_data_message_t() + gps_data:gps_week(gps_week) + gps_data:ms_tow(gps_ins_time_ms) + gps_data:fix_type(fix_type+1) + gps_data:satellites_in_view(num_sats) + gps_data:horizontal_pos_accuracy(0.1) + gps_data:vertical_pos_accuracy(0.1) + gps_data:longitude(gps_lon) + gps_data:latitude(gps_lat) + gps_data:msl_altitude(gps_alt) + gps_data:ned_vel_north(vel_y*0.01) + gps_data:ned_vel_east(vel_x*0.01) + gps_data:ned_vel_down(-vel_z*0.01) + local gps_instance = gps:get_first_external_instance() + if gps_instance ~= nil then + gps:handle_external(gps_data, gps_instance) + end + end + + -- provide airspeed data + local airspeed_data = airspeed_data_message_t() + airspeed_data:differential_pressure(differential_pressure*1.0e-4) + airspeed_data:temperature(temperature*0.1) + airspeed:handle_external(airspeed_data) + +end + +gcs:send_text(MAV_SEVERITY.INFO, "ILabs loaded") + +-- wrapper around update(). This calls update() and if update faults +-- then an error is displayed, but the script is not stopped +function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(MAV_SEVERITY.ERROR, "Internal Error: " .. err) + return protected_wrapper, 250 + end + return protected_wrapper, 2 +end + +-- start running update loop +return protected_wrapper() diff --git a/libraries/AP_Scripting/drivers/EAHRS_InertialLabs.md b/libraries/AP_Scripting/drivers/EAHRS_InertialLabs.md new file mode 100644 index 0000000000000..2c85e68e44d95 --- /dev/null +++ b/libraries/AP_Scripting/drivers/EAHRS_InertialLabs.md @@ -0,0 +1,29 @@ +# InertialLabs INS Driver + +This driver implements support for the InertialLabs INS +external AHRS navigation system + + https://inertiallabs.com/ + +# Parameters + +The script used the following parameters: + +## ILABS_ENABLE + +this must be set to 1 to enable the driver + +# Operation + +This driver should be loaded by placing the lua script in the +APM/SCRIPTS directory on the microSD card, which can be done either +directly or via MAVFTP. The following key parameters should be set: + + - SCR_ENABLE should be set to 1 + - EAHRS_TYPE should be set to 10 + - EAHRS_SENSORS should be set as needed + - SERIALn_PROTOCOL should be set to 28 for the connected EFI serial + - GPS_TYPE should be set to 21 for GPS functionality + +then the flight controller should rebooted and parameters should be +refreshed. From 04fdd961a8acedac8740146a2ca94cab15c723d0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 2 Dec 2023 17:51:19 +1100 Subject: [PATCH 15/16] Tools: added external AHRS scripting test --- .../InertialLabsEAHRSScripting/ap1.txt | 7 +++++ Tools/autotest/arduplane.py | 31 +++++++++++++++++-- Tools/autotest/vehicle_test_suite.py | 13 ++++++++ 3 files changed, 49 insertions(+), 2 deletions(-) create mode 100644 Tools/autotest/ArduPlane_Tests/InertialLabsEAHRSScripting/ap1.txt diff --git a/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRSScripting/ap1.txt b/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRSScripting/ap1.txt new file mode 100644 index 0000000000000..ab2be0d1a4547 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRSScripting/ap1.txt @@ -0,0 +1,7 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 +5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2c4b78f25f8eb..e9852c31e7aa5 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3168,10 +3168,11 @@ def TerrainLoiter(self): self.progress("Returning home") self.fly_home_land_and_disarm(240) - def fly_external_AHRS(self, sim, eahrs_type, mission): + def fly_external_AHRS(self, sim, eahrs_type, mission, extra_params=None, script=None): """Fly with external AHRS (VectorNav)""" self.customise_SITL_commandline(["--serial4=sim:%s" % sim]) + self.context_push() self.set_parameters({ "EAHRS_TYPE": eahrs_type, "SERIAL4_PROTOCOL": 36, @@ -3180,7 +3181,20 @@ def fly_external_AHRS(self, sim, eahrs_type, mission): "AHRS_EKF_TYPE": 11, "INS_GYR_CAL": 1, }) + if script is not None: + # optionally install a driver script + self.install_driver_script_context(script) + self.set_parameters({ + "SCR_ENABLE" : 1, + "SCR_VM_I_COUNT" : 1000000, + }) self.reboot_sitl() + if extra_params is not None: + # optional extra parameters + self.set_parameters(extra_params) + self.reboot_sitl() + # set again for ones that need to be after reboot + self.set_parameters(extra_params) self.delay_sim_time(5) self.progress("Running accelcal") self.run_cmd( @@ -3192,6 +3206,7 @@ def fly_external_AHRS(self, sim, eahrs_type, mission): self.wait_ready_to_arm() self.arm_vehicle() self.fly_mission(mission) + self.context_pop() def wait_and_maintain_wind_estimate( self, @@ -3291,7 +3306,18 @@ def MicroStrainEAHRS7(self): def InertialLabsEAHRS(self): '''Test InertialLabs EAHRS support''' - self.fly_external_AHRS("ILabs", 5, "ap1.txt") + self.fly_external_AHRS("ILabs", 5, "ap1.txt", + extra_params={'SIM_SPEEDUP': 10}) + + def InertialLabsEAHRSScripting(self): + '''Test InertialLabs EAHRS support with scripting''' + self.fly_external_AHRS("ILabs", 10, "ap1.txt", + extra_params={"ILABS_ENABLE": 1, + 'SERIAL4_PROTOCOL': 28, + 'SIM_SPEEDUP': 1, + 'EAHRS_SENSORS': 13, + 'ARSPD_TYPE': 16}, + script="EAHRS_InertialLabs.lua") def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -5408,6 +5434,7 @@ def tests(self): self.MicroStrainEAHRS5, self.MicroStrainEAHRS7, self.InertialLabsEAHRS, + self.InertialLabsEAHRSScripting, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 7fb44e0861b0c..514be70325748 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -4580,6 +4580,12 @@ def install_applet_script_context(self, scriptname): self.install_applet_script(scriptname) self.context_get().installed_scripts.append(scriptname) + def install_driver_script_context(self, scriptname): + '''installs a driver script which will be removed when the context goes + away''' + self.install_driver_script(scriptname) + self.context_get().installed_scripts.append(scriptname) + def rootdir(self): this_dir = os.path.dirname(__file__) return os.path.realpath(os.path.join(this_dir, "../..")) @@ -8126,6 +8132,9 @@ def script_test_source_path(self, scriptname): def script_applet_source_path(self, scriptname): return os.path.join(self.rootdir(), "libraries", "AP_Scripting", "applets", scriptname) + def script_drivers_source_path(self, scriptname): + return os.path.join(self.rootdir(), "libraries", "AP_Scripting", "drivers", scriptname) + def installed_script_path(self, scriptname): return os.path.join("scripts", os.path.basename(scriptname)) @@ -8166,6 +8175,10 @@ def install_applet_script(self, scriptname, install_name=None): source = self.script_applet_source_path(scriptname) self.install_script(source, scriptname, install_name=install_name) + def install_driver_script(self, scriptname, install_name=None): + source = self.script_drivers_source_path(scriptname) + self.install_script(source, scriptname, install_name=install_name) + def remove_installed_script(self, scriptname): dest = self.installed_script_path(os.path.basename(scriptname)) try: From 4463e46d3a08fe9ee12e4654cbd6e72c20767716 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Dec 2023 17:21:26 +1100 Subject: [PATCH 16/16] AP_Scripting: fixed depdencies for ESP32 build --- .../generator/description/bindings.desc | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 20d07ff35fe73..8474cb9a73338 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -848,7 +848,7 @@ include AP_ExternalAHRS/AP_ExternalAHRS_config.h include AP_ExternalAHRS/AP_ExternalAHRS_Scripting.h depends AP_EXTERNAL_AHRS_SCRIPTING_ENABLED include AP_NavEKF/AP_Nav_Common.h singleton AP_ExternalAHRS rename ExternalAHRS -singleton AP_ExternalAHRS depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_ExternalAHRS depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) userdata AP_ExternalAHRS::state_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) userdata AP_ExternalAHRS::state_t rename EAHRS_State @@ -870,6 +870,7 @@ userdata AP_ExternalAHRS::state_t field compass_variance float'skip_check write userdata AP_ExternalAHRS::state_t field terrain_alt_variance float'skip_check write userdata AP_ExternalAHRS::state_t field airspeed_variance float'skip_check write +userdata nav_filter_status_flags_t depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) userdata nav_filter_status_flags_t field attitude boolean write userdata nav_filter_status_flags_t field horiz_vel boolean write userdata nav_filter_status_flags_t field vert_vel boolean write @@ -930,15 +931,15 @@ userdata AP_ExternalAHRS::airspeed_data_message_t field temperature float'skip_c singleton AP_ExternalAHRS method handle_scripting boolean AP_ExternalAHRS::state_t nav_filter_status_flags_t singleton AP_InertialSensor method handle_external void AP_ExternalAHRS::ins_data_message_t -singleton AP_InertialSensor method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_InertialSensor method handle_external depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) singleton AP_Baro method handle_external void AP_ExternalAHRS::baro_data_message_t -singleton AP_Baro method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_Baro method handle_external depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) singleton AP_GPS method get_first_external_instance boolean uint8_t'Null -singleton AP_GPS method get_first_external_instance depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_GPS method get_first_external_instance depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) singleton AP_GPS method handle_external void AP_ExternalAHRS::gps_data_message_t uint8_t'skip_check -singleton AP_GPS method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton AP_GPS method handle_external depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) singleton Compass method handle_external void AP_ExternalAHRS::mag_data_message_t -singleton Compass method handle_external depends (!defined(HAL_BUILD_AP_PERIPH)) +singleton Compass method handle_external depends (AP_EXTERNAL_AHRS_SCRIPTING_ENABLED == 1) include AP_Airspeed/AP_Airspeed.h singleton AP_Airspeed depends (AP_AIRSPEED_EXTERNAL_ENABLED == 1) && (APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI)