Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_ExternalAHRS: Use state watching instead

Browse Source 
Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-03-23 17:14:23 -06:00 committed by Andrew Tridgell
parent 95257aa7df
commit de6244d078
2 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp
View File

@ -71,9 +71,7 @@ AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_fro
uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS));
hal.scheduler->delay(5000); hal.scheduler->delay(5000);
if(initialised()) { if(!initialised()) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised.");
} else {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MicroStrain7 ExternalAHRS failed to initialise."); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MicroStrain7 ExternalAHRS failed to initialise.");
} }
} }
@ -88,9 +86,18 @@ void AP_ExternalAHRS_MicroStrain7::update_thread(void)
while (true) { while (true) {
build_packet(); build_packet();
hal.scheduler->delay_microseconds(100); 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. // 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 bool AP_ExternalAHRS_MicroStrain7::initialised(void) const
{ {
const bool got_packets = last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0; const bool got_packets = last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0;
const auto filter_state = static_cast<FilterState>(filter_status.state); return got_packets;
const bool filter_healthy = filter_state_healthy(filter_state);
return got_packets && filter_healthy;
} }
bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const

4
libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h
View File

@ -74,6 +74,7 @@ private:
void post_filter() const; void post_filter() const;
void update_thread(); void update_thread();
void check_initialise_state();
// Only some of the fix types satisfy a healthy filter. // Only some of the fix types satisfy a healthy filter.
// GQ7_VERT_GYRO is NOT considered healthy for now. // GQ7_VERT_GYRO is NOT considered healthy for now.
@ -83,6 +84,9 @@ private:
AP_HAL::UARTDriver *uart; AP_HAL::UARTDriver *uart;
HAL_Semaphore sem; HAL_Semaphore sem;
// Used to monitor initialization state.
bool last_init_state = false;
}; };
#endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED #endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED