diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp index 6cf14cd62bbbc5..0126b0815522f6 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.cpp @@ -132,36 +132,47 @@ AP_ExternalAHRS_AdvancedNavigation::AP_ExternalAHRS_AdvancedNavigation(AP_Extern set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); - - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_AdvancedNavigation::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { AP_HAL::panic("Failed to start ExternalAHRS update thread"); } +} + +void AP_ExternalAHRS_AdvancedNavigation::update() +{ + get_packets(); +} + +void AP_ExternalAHRS_AdvancedNavigation::update_thread(void) +{ + _uart->begin(_baudrate); uint32_t tstart = AP_HAL::millis(); + // Ensure device is responsive by requesting its information. while (!_last_device_info_pkt_ms) { const uint32_t tnow = AP_HAL::millis(); if (tnow - tstart >= AN_TIMEOUT) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Advanced Navigation Device Unresponsive"); + if (!request_data()) + { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Request Data Error"); + } tstart = tnow; } - hal.scheduler->delay(50); - } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised: %s", get_name()); -} + // Sleep the scheduler + hal.scheduler->delay_microseconds(1000); -void AP_ExternalAHRS_AdvancedNavigation::update() -{ - get_packets(); -} + // Collect the requested packets from the UART manager + // This will still process all received packets like normal and feed data out. This ensures that it won't fail completely if the TX fails. + if (!get_packets()) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS: Error Receiving Initialization Packets"); + } + } -void AP_ExternalAHRS_AdvancedNavigation::update_thread(void) -{ - _uart->begin(_baudrate); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ExternalAHRS initialised: %s", get_name()); while (true) { // Request data. If error occurs notify. @@ -229,10 +240,11 @@ bool AP_ExternalAHRS_AdvancedNavigation::request_device_information(void) bool AP_ExternalAHRS_AdvancedNavigation::request_data(void) { - // Update device info every 20 secs - if ((AP_HAL::millis() - _last_device_info_pkt_ms > 20000) || (_last_device_info_pkt_ms == 0)) { - request_device_information(); + if ((AP_HAL::millis() - _last_device_info_pkt_ms > 20000) || !_last_device_info_pkt_ms) { + if (!request_device_information()) { + return false; + } } // Don't send a packet request unless the device is healthy diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h index e8d31c94f86a43..0a647c3b59f423 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_AdvancedNavigation.h @@ -297,6 +297,7 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend uint32_t serial_2; uint32_t serial_3; }; + struct PACKED AN_STATUS{ union { uint16_t r; @@ -529,13 +530,13 @@ class AP_ExternalAHRS_AdvancedNavigation: public AP_ExternalAHRS_backend void update_thread(); bool get_packets(void); + bool request_device_information(void); bool request_data(void); bool sendPacketRequest(void); bool get_gnss_capability(void) const; bool get_baro_capability(void) const; bool set_filter_options(bool gnss_en, vehicle_type_e vehicle_type, bool permanent = false); bool set_filter_options(AN_FILTER_OPTIONS options_packet); - bool request_device_information(); bool send_airspeed_aiding(void); float get_airspeed_error(float airspeed); float get_pressure_error(void);