AP_ExternalAHRS: Add pre-arm for misconfigured EAHRS_SENORS and GPS_TYPE

* This catches when there's a mismatch of GPSx_TYPE and EAHRS_SENSORS when GPS is enabled * Before this pre-arm, failure to set GPS_TYPE2 to 21 (ExternalAHRS) resulted in silent rejection of the data in AP_GPS because the default is off Signed-off-by: Ryan Friedman <[email protected]>
Ryanf55 · Mar 25, 2024 · 19c81e9 · 19c81e9
1 parent 18578b3
commit 19c81e9
Show file tree

Hide file tree

Showing 7 changed files with 51 additions and 0 deletions.
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
@@ -29,6 +29,7 @@
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_GPS/AP_GPS.h>
 #include <AP_Logger/AP_Logger.h>
 
 extern const AP_HAL::HAL &hal;
@@ -240,6 +241,29 @@ bool AP_ExternalAHRS::pre_arm_check(char *failure_msg, uint8_t failure_msg_len)
         hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: No origin");
         return false;
     }
+
+    // Verify the user has configured the GPS to accept EAHRS data.
+    if (has_sensor(AvailableSensor::GPS)) {
+        const auto eahrs_gps_sensors = backend->num_gps_sensors();
+
+        if (AP::gps().num_sensors() < eahrs_gps_sensors) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: Not enough GPS sensors");
+            return false;
+        }
+        uint8_t n_configured_eahrs_gps = 0;
+        for (uint8_t i = 0; i < GPS_MAX_INSTANCES; ++i) {
+            const auto gps_type = AP::gps().get_type(i);
+            if (gps_type == AP_GPS::GPS_TYPE_EXTERNAL_AHRS) {
+                n_configured_eahrs_gps++;
+            }
+        }
+
+        if (n_configured_eahrs_gps != eahrs_gps_sensors) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "ExternalAHRS: Incorrect number of GPS sensors configured for EAHRS");
+            return false;
+        }
+    }
+
     return backend->pre_arm_check(failure_msg, failure_msg_len);
 }
 

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h
@@ -181,6 +181,12 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
     uint16_t buffer_ofs;
     uint8_t buffer[256]; // max for normal message set is 167+8
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override {
+        return 1;
+    }
+
 private:
     AP_HAL::UARTDriver *uart;
     int8_t port_num;

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.h
@@ -49,6 +49,12 @@ class AP_ExternalAHRS_MicroStrain5: public AP_ExternalAHRS_backend, public AP_Mi
         build_packet();
     };
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override {
+        return 1;
+    }
+
 private:
 
     uint32_t baudrate;

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp
@@ -18,6 +18,7 @@
     param set AHRS_EKF_TYPE 11
     param set EAHRS_TYPE 7
     param set GPS1_TYPE 21
+    param set GPS2_TYPE 21
     param set SERIAL3_BAUD 115
     param set SERIAL3_PROTOCOL 36
   UDEV rules for repeatable USB connection:

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h
@@ -50,6 +50,12 @@ class AP_ExternalAHRS_MicroStrain7: public AP_ExternalAHRS_backend, public AP_Mi
         build_packet();
     };
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override {
+        return AP_MicroStrain::NUM_GNSS_INSTANCES;
+    }
+
 private:
 
     // GQ7 Filter States

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
@@ -47,6 +47,11 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
     // Get model/type name
     const char* get_name() const override;
 
+protected:
+
+    uint8_t num_gps_sensors(void) const override {
+        return 1;
+    }
 private:
     AP_HAL::UARTDriver *uart;
     int8_t port_num;

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h
@@ -48,6 +48,9 @@ class AP_ExternalAHRS_backend {
     // This can also copy interim state protected by locking.
     virtual void update() = 0;
 
+    // Return the number of GPS sensors sharing data to AP_GPS.
+    virtual uint8_t num_gps_sensors(void) const = 0;
+
 protected:
     AP_ExternalAHRS::state_t &state;
     uint16_t get_rate(void) const;