From a0939dd5189ef58926dd023cdba134179004a939 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 23 Mar 2024 22:09:21 -0600 Subject: [PATCH] Save flash, reduce code duplication * Add generic health and time utils * Fix bug only checking first GNSS system * Use common logging struct * Improve pre-arm log checks Signed-off-by: Ryan Friedman --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 1 - .../AP_ExternalAHRS_MicroStrain7.cpp | 88 ++++++++++++------- .../AP_ExternalAHRS_MicroStrain7.h | 6 ++ 3 files changed, 62 insertions(+), 33 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index dc3ff2a34b..0ad2a7233f 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -238,7 +238,6 @@ bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) if (!state.have_origin) { hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin"); - return false; } return backend->pre_arm_check(failure_msg, failure_msg_len); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 9906843d39..6ac218afb0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -45,6 +45,8 @@ #include #include +static const char* LOG_FMT = "%s ExternalAHRS: %s"; + extern const AP_HAL::HAL &hal; AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_frontend, @@ -57,12 +59,12 @@ AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_fro port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); if (!uart) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MicroStrain7 ExternalAHRS no UART"); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, LOG_FMT, get_name(), "no UART"); return; } if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain7::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { - AP_BoardConfig::allocation_error("MicroStrain7 failed to allocate ExternalAHRS update thread"); + AP_BoardConfig::allocation_error("MicroStrain7 ExternalAHRS failed to allocate ExternalAHRS update thread"); } // don't offer IMU by default, at 100Hz it is too slow for many aircraft @@ -72,7 +74,7 @@ AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_fro hal.scheduler->delay(5000); if (!initialised()) { - GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MicroStrain7 ExternalAHRS failed to initialise."); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, LOG_FMT, get_name(), "failed to initialise."); } } @@ -95,7 +97,7 @@ void AP_ExternalAHRS_MicroStrain7::check_initialise_state(void) const auto new_init_state = initialised(); // Only send the message after fully booted up, otherwise it gets dropped. if (!last_init_state && new_init_state && AP_HAL::millis() > 5000) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised."); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, LOG_FMT, get_name(), "initialised."); last_init_state = new_init_state; } } @@ -250,31 +252,12 @@ int8_t AP_ExternalAHRS_MicroStrain7::get_port(void) const // Get model/type name const char* AP_ExternalAHRS_MicroStrain7::get_name() const { - return "MICROSTRAIN7"; + return "MicroStrain7"; } bool AP_ExternalAHRS_MicroStrain7::healthy(void) const { - uint32_t now = AP_HAL::millis(); - - // Expect the following rates: - // * Navigation Filter: 25Hz = 40mS - // * GPS: 2Hz = 500mS - // * IMU: 25Hz = 40mS - - // Allow for some slight variance of 10% - constexpr float RateFoS = 1.1; - - constexpr uint32_t expected_filter_time_delta_ms = 40; - constexpr uint32_t expected_gps_time_delta_ms = 500; - constexpr uint32_t expected_imu_time_delta_ms = 40; - - const bool times_healthy = (now - last_imu_pkt < expected_imu_time_delta_ms * RateFoS && \ - now - last_gps_pkt < expected_gps_time_delta_ms * RateFoS && \ - now - last_filter_pkt < expected_filter_time_delta_ms * RateFoS); - const auto filter_state = static_cast(filter_status.state); - const bool filter_healthy = (filter_state == FilterState::GQ7_FULL_NAV || filter_state == FilterState::GQ7_AHRS); - return times_healthy && filter_healthy; + return times_healthy() && filter_healthy(); } bool AP_ExternalAHRS_MicroStrain7::initialised(void) const @@ -285,17 +268,29 @@ bool AP_ExternalAHRS_MicroStrain7::initialised(void) const bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { - if (!healthy()) { - hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 unhealthy"); + if (!initialised()) { + hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "not initialised"); return false; } - // TODO is this necessary? hard coding the first instance. - if (gnss_data[0].fix_type < 3) { - hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 no GPS lock"); + if (!times_healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "data is stale"); + return false; + } + if (!filter_healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "filter is unhealthy"); + return false; + } + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, 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"); return false; } if (!filter_state_healthy(FilterState(filter_status.state))) { - hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 filter not running"); + hal.util->snprintf(failure_msg, failure_msg_len, get_name(), "filter not healthy"); return false; } @@ -384,6 +379,36 @@ void AP_ExternalAHRS_MicroStrain7::send_status_report(GCS_MAVLINK &link) const } +bool AP_ExternalAHRS_MicroStrain7::times_healthy() const +{ + uint32_t now = AP_HAL::millis(); + + // Expect the following rates: + // * Navigation Filter: 25Hz = 40mS + // * GPS: 2Hz = 500mS + // * IMU: 25Hz = 40mS + + // Allow for some slight variance of 10% + constexpr float RateFoS = 1.1; + + constexpr uint32_t expected_filter_time_delta_ms = 40; + constexpr uint32_t expected_gps_time_delta_ms = 500; + constexpr uint32_t expected_imu_time_delta_ms = 40; + + const bool times_healthy = (now - last_imu_pkt < expected_imu_time_delta_ms * RateFoS && \ + now - last_gps_pkt < expected_gps_time_delta_ms * RateFoS && \ + now - last_filter_pkt < expected_filter_time_delta_ms * RateFoS); + + return times_healthy; +} + +bool AP_ExternalAHRS_MicroStrain7::filter_healthy() const +{ + const auto filter_state = static_cast(filter_status.state); + const bool filter_healthy = filter_state_healthy(filter_state); + return filter_healthy; +} + bool AP_ExternalAHRS_MicroStrain7::filter_state_healthy(FilterState state) { switch (state) { @@ -393,7 +418,6 @@ bool AP_ExternalAHRS_MicroStrain7::filter_state_healthy(FilterState state) default: return false; } - // return state == FilterState::GQ7_FULL_NAV || state == FilterState::GQ7_AHRS; } #endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h index 1b9967e9b8..5cff5af56a 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h @@ -76,6 +76,12 @@ private: void update_thread(); void check_initialise_state(); + // Returns true when data is not stale. + bool times_healthy() const; + + // Returns true when the filter is currently healthy. + bool filter_healthy() const; + // Only some of the fix types satisfy a healthy filter. // GQ7_VERT_GYRO is NOT considered healthy for now. // This may be vehicle-dependent in the future.