From 40a26236e7ae1e75563397023dc90f0722f7671e Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sun, 24 Mar 2024 20:40:31 -0600 Subject: [PATCH 1/2] AP_ExternalAHRS: Add pre-arm for misconfigured EAHRS_SENSORS and GPS_TYPE * This catches when there's a mismatch of GPSx_TYPE and EAHRS_SENSORS when GPS is enabled * Before this pre-arm, failure to set GPS_TYPE2 to 21 (ExternalAHRS) resulted in silent rejection of the data in AP_GPS because the default is off * And fix a little logging bug Signed-off-by: Ryan Friedman --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 22 +++++++++++++++++++ .../AP_ExternalAHRS_InertialLabs.h | 6 +++++ .../AP_ExternalAHRS_MicroStrain5.h | 6 +++++ .../AP_ExternalAHRS_MicroStrain7.cpp | 13 ++++++----- .../AP_ExternalAHRS_MicroStrain7.h | 7 ++++++ .../AP_ExternalAHRS_VectorNav.h | 5 +++++ .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 3 +++ 7 files changed, 56 insertions(+), 6 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 315cc17b22765..7900bab8256e9 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -29,6 +29,7 @@ #include #include +#include #include extern const AP_HAL::HAL &hal; @@ -238,6 +239,27 @@ bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) if (!backend->pre_arm_check(failure_msg, failure_msg_len)) { return false; } + // Verify the user has configured the GPS to accept EAHRS data. + if (has_sensor(AvailableSensor::GPS)) { + const auto eahrs_gps_sensors = backend->num_gps_sensors(); + + const auto &gps = AP::gps(); + uint8_t n_configured_eahrs_gps = 0; + for (uint8_t i = 0; i < GPS_MAX_INSTANCES; ++i) { + const auto gps_type = gps.get_type(i); + if (gps_type == AP_GPS::GPS_TYPE_EXTERNAL_AHRS) { + n_configured_eahrs_gps++; + } + } + + // Once AP supports at least 3 GPS's, change to == and remove the second condition. + // At that point, enforce that all GPS's in EAHRS can report to AP_GPS. + if (n_configured_eahrs_gps < 1 && eahrs_gps_sensors >= 1) { + hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: Incorrect number of GPS sensors configured for EAHRS"); + return false; + } + } + if (!state.have_origin) { hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin"); return false; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h index bee7d5c28e0cc..9de2c9526e931 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -181,6 +181,12 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { uint16_t buffer_ofs; uint8_t buffer[256]; // max for normal message set is 167+8 +protected: + + uint8_t num_gps_sensors(void) const override { + return 1; + } + private: AP_HAL::UARTDriver *uart; int8_t port_num; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h index e6b2b9e6bdcb1..7d5c199eb1365 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h @@ -49,6 +49,12 @@ class AP_ExternalAHRS_MicroStrain5: public AP_ExternalAHRS_backend, public AP_Mi build_packet(); }; +protected: + + uint8_t num_gps_sensors(void) const override { + return 1; + } + private: uint32_t baudrate; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 901fff1c84cf5..6a576ab52cab2 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -18,6 +18,7 @@ param set AHRS_EKF_TYPE 11 param set EAHRS_TYPE 7 param set GPS1_TYPE 21 + param set GPS2_TYPE 21 param set SERIAL3_BAUD 115 param set SERIAL3_PROTOCOL 36 UDEV rules for repeatable USB connection: @@ -269,28 +270,28 @@ bool AP_ExternalAHRS_MicroStrain7::initialised(void) const bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { if (!initialised()) { - hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "not initialised"); + hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "not initialised"); return false; } if (!times_healthy()) { - hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "data is stale"); + hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "data is stale"); return false; } if (!filter_healthy()) { - hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "filter is unhealthy"); + hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "filter is unhealthy"); return false; } if (!healthy()) { - hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "unhealthy"); + hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "unhealthy"); return false; } static_assert(NUM_GNSS_INSTANCES == 2, "This check only works if there are two GPS types."); if (gnss_data[0].fix_type < GPS_FIX_TYPE_3D_FIX && gnss_data[1].fix_type < GPS_FIX_TYPE_3D_FIX) { - hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "missing 3D GPS fix on either GPS"); + hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "missing 3D GPS fix on either GPS"); return false; } if (!filter_state_healthy(FilterState(filter_status.state))) { - hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "filter not healthy"); + hal.util->snprintf(failure_msg, failure_msg_len, LOG_FMT, get_name(), "filter not healthy"); return false; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h index 5cff5af56a6dc..f278289d15fb4 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h @@ -50,6 +50,13 @@ class AP_ExternalAHRS_MicroStrain7: public AP_ExternalAHRS_backend, public AP_Mi build_packet(); }; +protected: + + uint8_t num_gps_sensors(void) const override + { + return AP_MicroStrain::NUM_GNSS_INSTANCES; + } + private: // GQ7 Filter States diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h index 3ce1fe7f5e2d5..c0e54c88bf2a4 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h @@ -47,6 +47,11 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend { // Get model/type name const char* get_name() const override; +protected: + + uint8_t num_gps_sensors(void) const override { + return 1; + } private: AP_HAL::UARTDriver *uart; int8_t port_num; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index ed987a01ced3a..e5a22a87cbe44 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -48,6 +48,9 @@ class AP_ExternalAHRS_backend { // This can also copy interim state protected by locking. virtual void update() = 0; + // Return the number of GPS sensors sharing data to AP_GPS. + virtual uint8_t num_gps_sensors(void) const = 0; + protected: AP_ExternalAHRS::state_t &state; uint16_t get_rate(void) const; From 4b4ed67a339958014a4b818d26e13b53815fde60 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 25 Mar 2024 22:17:30 -0600 Subject: [PATCH 2/2] Tools: Autotest: Test for EAHRS misconfiguration prearm failure * And test for single GPS reporting on Microstrain7 Signed-off-by: Ryan Friedman --- Tools/autotest/arduplane.py | 52 ++++++++++++++++++++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 2787be202135e..51218ab3000ec 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3169,7 +3169,7 @@ def TerrainLoiter(self): self.fly_home_land_and_disarm(240) def fly_external_AHRS(self, sim, eahrs_type, mission): - """Fly with external AHRS (VectorNav)""" + """Fly with external AHRS""" self.customise_SITL_commandline(["--serial4=sim:%s" % sim]) self.set_parameters({ @@ -3293,6 +3293,55 @@ def InertialLabsEAHRS(self): '''Test InertialLabs EAHRS support''' self.fly_external_AHRS("ILabs", 5, "ap1.txt") + def GpsSensorPreArmEAHRS(self): + '''Test pre-arm checks related to EAHRS_SENSORS using the MicroStrain7 driver''' + self.customise_SITL_commandline(["--serial4=sim:MicroStrain7"]) + + self.set_parameters({ + "EAHRS_TYPE": 7, + "SERIAL4_PROTOCOL": 36, + "SERIAL4_BAUD": 230400, + "GPS1_TYPE": 0, # Disabled (simulate user setup error) + "GPS2_TYPE": 0, # Disabled (simulate user setup error) + "AHRS_EKF_TYPE": 11, + "INS_GYR_CAL": 1, + "EAHRS_SENSORS": 13, # GPS is enabled + }) + self.reboot_sitl() + self.delay_sim_time(5) + self.progress("Running accelcal") + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=5, + ) + + self.assert_prearm_failure("ExternalAHRS: Incorrect number", # Cut short due to message limits. + timeout=30, + other_prearm_failures_fatal=False) + + self.set_parameters({ + "EAHRS_TYPE": 7, + "SERIAL4_PROTOCOL": 36, + "SERIAL4_BAUD": 230400, + "GPS1_TYPE": 1, # Auto + "GPS2_TYPE": 21, # EARHS + "AHRS_EKF_TYPE": 11, + "INS_GYR_CAL": 1, + "EAHRS_SENSORS": 13, # GPS is enabled + }) + self.reboot_sitl() + self.delay_sim_time(5) + self.progress("Running accelcal") + self.run_cmd( + mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, + p5=4, + timeout=5, + ) + # Check prearm success with MicroStrain when the first GPS is occupied by another GPS. + # This supports the use case of comparing MicroStrain dual antenna to another GPS. + self.wait_ready_to_arm() + def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -5408,6 +5457,7 @@ def tests(self): self.MicroStrainEAHRS5, self.MicroStrainEAHRS7, self.InertialLabsEAHRS, + self.GpsSensorPreArmEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch,