AHRS: add ability to configure AHRS via mavlink commands

libraries/AP_AHRS/AP_AHRS.cpp

@@ -3629,6 +3629,50 @@ float AP_AHRS::get_air_density_ratio(void) const
     return 1.0 / sq(eas2tas);
 }
 
+void AP_AHRS::set_data_sending_state(DATA_SENDING_STATE data_sending_state)
+{
+    switch (active_EKF_type()) {
+#if AP_AHRS_EXTERNAL_ENABLED
+    case EKFType::EXTERNAL:
+        return external.set_data_sending_state(data_sending_state == DATA_SENDING_STATE::DISABLED ?
+                                               AP_AHRS_Backend::DATA_SENDING_STATE::DISABLED :
+                                               AP_AHRS_Backend::DATA_SENDING_STATE::ENABLED);
+#endif
+    default:
+        break;
+    }
+}
+
+void AP_AHRS::set_gps_state(GPS_STATE gps_state)
+{
+    switch (active_EKF_type()) {
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+#if AP_GPS_ENABLED
+        AP::gps().force_disable(gps_state == GPS_STATE::DISABLED);
+#endif
+        return;
+#endif //HAL_NAVEKF2_AVAILABLE
+
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+#if AP_GPS_ENABLED
+        AP::gps().force_disable(gps_state == GPS_STATE::DISABLED);
+#endif
+        return;
+#endif //HAL_NAVEKF3_AVAILABLE
+
+#if AP_AHRS_EXTERNAL_ENABLED
+    case EKFType::EXTERNAL:
+        return external.set_gps_state(gps_state == GPS_STATE::DISABLED ?
+                                      AP_AHRS_Backend::GPS_STATE::DISABLED :
+                                      AP_AHRS_Backend::GPS_STATE::ENABLED);
+#endif
+    default:
+        break;
+    }
+}
+
 // singleton instance
 AP_AHRS *AP_AHRS::_singleton;
 

libraries/AP_AHRS/AP_AHRS.h

@@ -683,6 +683,19 @@ class AP_AHRS {
     // get access to an EKFGSF_yaw estimator
     const EKFGSF_yaw *get_yaw_estimator(void) const;
 
+    enum class DATA_SENDING_STATE {
+        ENABLED,
+        DISABLED
+    };
+    // enable/disable sending data to autopilot. Can be used to simulate AHRS failure.
+    void set_data_sending_state(DATA_SENDING_STATE data_sending_state);
+
+    enum class GPS_STATE {
+        ENABLED,
+        DISABLED
+    };
+    void set_gps_state(GPS_STATE gps_state);
+
 private:
 
     // roll/pitch/yaw euler angles, all in radians

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -329,4 +329,17 @@ class AP_AHRS_Backend
     virtual void set_terrain_hgt_stable(bool stable) {}
 
     virtual void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const = 0;
+
+    enum class DATA_SENDING_STATE {
+        ENABLED,
+        DISABLED
+    };
+    // enable/disable sending data to autopilot. Can be used to simulate AHRS failure.
+    virtual void set_data_sending_state(DATA_SENDING_STATE state) {};
+
+    enum class GPS_STATE {
+        ENABLED,
+        DISABLED
+    };
+    virtual void set_gps_state(GPS_STATE state) {};
 };

libraries/AP_AHRS/AP_AHRS_External.cpp

@@ -137,4 +137,18 @@ void AP_AHRS_External::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVe
     ekfNavVelGainScaler = 0.5;
 }
 
+void AP_AHRS_External::set_data_sending_state(DATA_SENDING_STATE state)
+{
+    AP::externalAHRS().set_data_sending_state(state == DATA_SENDING_STATE::ENABLED ?
+                                              AP_ExternalAHRS::DATA_SENDING_STATE::ENABLED :
+                                              AP_ExternalAHRS::DATA_SENDING_STATE::DISABLED);
+}
+
+void AP_AHRS_External::set_gps_state(GPS_STATE state)
+{
+    AP::externalAHRS().set_gps_state(state == GPS_STATE::ENABLED ?
+                                     AP_ExternalAHRS::GPS_STATE::ENABLED :
+                                     AP_ExternalAHRS::GPS_STATE::DISABLED);
+}
+
 #endif

libraries/AP_AHRS/AP_AHRS_External.h

@@ -87,6 +87,10 @@ class AP_AHRS_External : public AP_AHRS_Backend {
     void send_ekf_status_report(class GCS_MAVLINK &link) const override;
 
     void get_control_limits(float &ekfGndSpdLimit, float &controlScaleXY) const override;
+
+    // enable/disable sending data to autopilot. Can be used to simulate AHRS failure.
+    void set_data_sending_state(DATA_SENDING_STATE state) override;
+    void set_gps_state(GPS_STATE state) override;
 };
 
 #endif

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -375,6 +375,31 @@ void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const
                                        mag_var, 0, 0);
 }
 
+void AP_ExternalAHRS::set_data_sending_state(DATA_SENDING_STATE data_sending_state)
+{
+    if (!backend) {
+        return;
+    }
+
+    backend->set_data_sending_state(data_sending_state == DATA_SENDING_STATE::ENABLED ?
+                                    AP_ExternalAHRS::DATA_SENDING_STATE::ENABLED :
+                                    AP_ExternalAHRS::DATA_SENDING_STATE::DISABLED);
+}
+
+void AP_ExternalAHRS::set_gps_state(GPS_STATE gps_state)
+{
+    // set GNSS disable for auxillary function GPS_DISABLE
+    gnss_is_disabled = (gps_state == GPS_STATE::DISABLED);
+
+    if (!backend) {
+        return;
+    }
+
+    backend->set_gps_state(gps_state == GPS_STATE::ENABLED ?
+                           AP_ExternalAHRS::GPS_STATE::ENABLED :
+                           AP_ExternalAHRS::GPS_STATE::DISABLED);
+}
+
 void AP_ExternalAHRS::update(void)
 {
     if (backend) {

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -121,6 +121,19 @@ class AP_ExternalAHRS {
     bool get_accel(Vector3f &accel);
     void send_status_report(class GCS_MAVLINK &link) const;
     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const;
+
+    enum class DATA_SENDING_STATE {
+        ENABLED,
+        DISABLED
+    };
+    // enable/disable sending data to autopilot. Can be used to simulate EAHRS failure.
+    void set_data_sending_state(DATA_SENDING_STATE data_sending_state);
+
+    enum class GPS_STATE {
+        ENABLED,
+        DISABLED
+    };
+    void set_gps_state(GPS_STATE gps_state);
 
     // update backend
     void update();
@@ -167,11 +180,6 @@ class AP_ExternalAHRS {
         float temperature; // degC
     } airspeed_data_message_t;
 
-    // set GNSS disable for auxillary function GPS_DISABLE
-    void set_gnss_disable(bool disable) {
-        gnss_is_disabled = disable;
-    }
-
 protected:
 
     enum class OPTIONS {

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp

@@ -79,6 +79,14 @@ extern const AP_HAL::HAL &hal;
 #define ILABS_AIRDATA_AIRSPEED_FAIL               0x0200
 #define ILABS_AIRDATA_BELOW_THRESHOLD             0x0400
 
+namespace Command {
+
+const char START_UDD[] = "\xAA\x55\x00\x00\x07\x00\x95\x9C\x00";
+const char STOP[] = "\xAA\x55\x00\x00\x07\x00\xFE\x05\x01";
+const char ENABLE_GNSS[] = "\xAA\x55\x00\x00\x07\x00\x71\x78\x00";
+const char DISABLE_GNSS[] = "\xAA\x55\x00\x00\x07\x00\x72\x79\x00";
+
+} // namespace Command
 
 // constructor
 AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *_frontend,
@@ -1120,5 +1128,29 @@ bool AP_ExternalAHRS_InertialLabs::get_variances(float &velVar, float &posVar, f
     return true;
 }
 
+void AP_ExternalAHRS_InertialLabs::set_data_sending_state(AP_ExternalAHRS::DATA_SENDING_STATE data_sending_state)
+{
+    if (data_sending_state == AP_ExternalAHRS::DATA_SENDING_STATE::ENABLED) {
+        write_bytes(Command::START_UDD, sizeof(Command::START_UDD) - 1);
+        return;
+    }
+    write_bytes(Command::STOP, sizeof(Command::STOP) - 1);
+}
+
+void AP_ExternalAHRS_InertialLabs::set_gps_state(AP_ExternalAHRS::GPS_STATE gps_state)
+{
+    if (gps_state == AP_ExternalAHRS::GPS_STATE::ENABLED) {
+        write_bytes(Command::ENABLE_GNSS, sizeof(Command::ENABLE_GNSS) - 1);
+        return;
+    }
+
+    write_bytes(Command::DISABLE_GNSS, sizeof(Command::DISABLE_GNSS) - 1);
+}
+
+bool AP_ExternalAHRS_InertialLabs::write_bytes(const char *bytes, uint8_t len)
+{
+    return uart->write(reinterpret_cast<const uint8_t *>(bytes), len);
+}
+
 #endif  // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h

@@ -39,6 +39,10 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
     void get_filter_status(nav_filter_status &status) const override;
     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
 
+    // enable/disable sending data to autopilot. Can be used to simulate EAHRS failure.
+    void set_data_sending_state(AP_ExternalAHRS::DATA_SENDING_STATE data_sending_state) override;
+    void set_gps_state(AP_ExternalAHRS::GPS_STATE gps_state) override;
+
     // check for new data
     void update() override {
         check_uart();
@@ -220,6 +224,7 @@ class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
     void update_thread();
     bool check_uart();
     bool check_header(const ILabsHeader *h) const;
+    bool write_bytes(const char *bytes, uint8_t len);
 
     // re-sync on header bytes
     void re_sync(void);

libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h

@@ -43,6 +43,10 @@ class AP_ExternalAHRS_backend {
     virtual void get_filter_status(nav_filter_status &status) const {}
     virtual bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const { return false; }
 
+    // enable/disable sending data to autopilot. Can be used to simulate EAHRS failure.
+    virtual void set_data_sending_state(AP_ExternalAHRS::DATA_SENDING_STATE data_sending_state) {}
+    virtual void set_gps_state(AP_ExternalAHRS::GPS_STATE gps_state) {}
+
     // Check for new data.
     // This is used when there's not a separate thread for EAHRS.
     // This can also copy interim state protected by locking.

libraries/GCS_MAVLink/GCS.h

@@ -631,6 +631,9 @@ class GCS_MAVLINK
 #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
     MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_int_t &packet);
 #endif
+#if AP_AHRS_ENABLED
+    MAV_RESULT handle_AHRS_message(const mavlink_command_int_t &packet);
+#endif
 
     virtual void send_banner();
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4878,6 +4878,32 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_autopilot_capabilities(const mavl
 }
 #endif
 
+#if AP_AHRS_ENABLED
+MAV_RESULT GCS_MAVLINK::handle_AHRS_message(const mavlink_command_int_t &packet)
+{
+    switch (packet.command) {
+        case MAV_CMD_AHRS_START:
+            AP::ahrs().set_data_sending_state(AP_AHRS::DATA_SENDING_STATE::ENABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        case MAV_CMD_AHRS_STOP:
+            AP::ahrs().set_data_sending_state(AP_AHRS::DATA_SENDING_STATE::DISABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        case MAV_CMD_AHRS_GPS_ENABLE:
+            AP::ahrs().set_gps_state(AP_AHRS::GPS_STATE::ENABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        case MAV_CMD_AHRS_GPS_DISABLE:
+            AP::ahrs().set_gps_state(AP_AHRS::GPS_STATE::DISABLED);
+            return MAV_RESULT_ACCEPTED;
+
+        default:
+            return MAV_RESULT_FAILED;
+    }
+}
+#endif
+
 MAV_RESULT GCS_MAVLINK::handle_command_do_set_mode(const mavlink_command_int_t &packet)
 {
     const MAV_MODE _base_mode = (MAV_MODE)packet.param1;
@@ -5638,6 +5664,13 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
     case MAV_CMD_REQUEST_MESSAGE:
         return handle_command_request_message(packet);
 
+#if AP_AHRS_ENABLED
+    case MAV_CMD_AHRS_START:
+    case MAV_CMD_AHRS_STOP:
+    case MAV_CMD_AHRS_GPS_ENABLE:
+    case MAV_CMD_AHRS_GPS_DISABLE:
+        return handle_AHRS_message(packet);
+#endif
     }
 
     return MAV_RESULT_UNSUPPORTED;

libraries/RC_Channel/RC_Channel.cpp

@@ -1592,7 +1592,9 @@ bool RC_Channel::do_aux_function(const AuxFuncTrigger &trigger)
     case AUX_FUNC::GPS_DISABLE:
         AP::gps().force_disable(ch_flag == AuxSwitchPos::HIGH);
 #if HAL_EXTERNAL_AHRS_ENABLED
-        AP::externalAHRS().set_gnss_disable(ch_flag == AuxSwitchPos::HIGH);
+        AP::externalAHRS().set_gps_state(ch_flag == AuxSwitchPos::HIGH ?
+                                         AP_ExternalAHRS::GPS_STATE::DISABLED :
+                                         AP_ExternalAHRS::GPS_STATE::ENABLED);
 #endif
         break;