From de6244d0786f85a6b4431c11be1aebdab3ff131d Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 23 Mar 2024 17:14:23 -0600 Subject: [PATCH] AP_ExternalAHRS: Use state watching instead Signed-off-by: Ryan Friedman --- .../AP_ExternalAHRS_MicroStrain7.cpp | 17 +++++++++++------ .../AP_ExternalAHRS_MicroStrain7.h | 4 ++++ 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 53be7b6e08..b8757f1ac7 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -71,9 +71,7 @@ AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_fro uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); hal.scheduler->delay(5000); - if(initialised()) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised."); - } else { + if(!initialised()) { GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MicroStrain7 ExternalAHRS failed to initialise."); } } @@ -88,9 +86,18 @@ void AP_ExternalAHRS_MicroStrain7::update_thread(void) while (true) { build_packet(); hal.scheduler->delay_microseconds(100); + check_initialise_state(); } } +void AP_ExternalAHRS_MicroStrain7::check_initialise_state(void) +{ + const auto new_init_state = initialised(); + if (!last_init_state && new_init_state) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised."); + } + last_init_state = new_init_state; +} // Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet. @@ -272,9 +279,7 @@ bool AP_ExternalAHRS_MicroStrain7::healthy(void) const bool AP_ExternalAHRS_MicroStrain7::initialised(void) const { const bool got_packets = last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0; - const auto filter_state = static_cast(filter_status.state); - const bool filter_healthy = filter_state_healthy(filter_state); - return got_packets && filter_healthy; + return got_packets; } bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h index d9e0832272..1b9967e9b8 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h @@ -74,6 +74,7 @@ private: void post_filter() const; void update_thread(); + void check_initialise_state(); // Only some of the fix types satisfy a healthy filter. // GQ7_VERT_GYRO is NOT considered healthy for now. @@ -83,6 +84,9 @@ private: AP_HAL::UARTDriver *uart; HAL_Semaphore sem; + // Used to monitor initialization state. + bool last_init_state = false; + }; #endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED