AP_ExternalAHRS_AdvancedNavigation: Rearranged the way initialization…

… is handled to prevent locking of thread.
ArduPilot · May 29, 2024 · fc9529c · fc9529c
1 parent fa7ef26
commit fc9529c
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 19 deletions.
diff --git 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
@@ -607,9 +619,9 @@ bool AP_ExternalAHRS_AdvancedNavigation::set_filter_options(bool gnss_en, vehicl
     options_packet.permanent = permanent;
     options_packet.vehicle_type = vehicle_type;
     options_packet.internal_gnss_enabled = gnss_en;
-    options_packet.magnetometers_enabled = true;
+    options_packet.magnetometers_enabled = false;
     options_packet.atmospheric_altitude_enabled = true;
-    options_packet.velocity_heading_enabled = true;
+    options_packet.velocity_heading_enabled = false;
     options_packet.reversing_detection_enabled = false;
     options_packet.motion_analysis_enabled = false;
     options_packet.automatic_magnetic_calibration_enabled = true;

diff --git 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);