ExternalAHRS: Add the ability to send commands to the EAHRS

libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp

@@ -375,6 +375,22 @@ void AP_ExternalAHRS::send_status_report(GCS_MAVLINK &link) const
                                        mag_var, 0, 0);
 }
 
+bool AP_ExternalAHRS::write_bytes(const char *bytes, uint8_t len)
+{
+    if (!backend) {
+        return false;
+    }
+    return backend->write_bytes(bytes, len);
+}
+
+bool AP_ExternalAHRS::handle_command(ExternalAHRS_command command)
+{
+    if (!backend) {
+        return false;
+    }
+    return backend->handle_command(command);
+}
+
 void AP_ExternalAHRS::update(void)
 {
     if (backend) {

libraries/AP_ExternalAHRS/AP_ExternalAHRS.h

@@ -28,6 +28,8 @@
 #include <AP_NavEKF/AP_Nav_Common.h>
 #include <AP_GPS/AP_GPS_FixType.h>
 
+#include "AP_ExternalAHRS_command_context.h"
+
 class AP_ExternalAHRS_backend;
 
 class AP_ExternalAHRS {
@@ -121,6 +123,8 @@ 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;
+    bool write_bytes(const char *bytes, uint8_t len);
+    bool handle_command(ExternalAHRS_command command);
 
     // update backend
     void update();

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,27 @@ bool AP_ExternalAHRS_InertialLabs::get_variances(float &velVar, float &posVar, f
     return true;
 }
 
+bool AP_ExternalAHRS_InertialLabs::write_bytes(const char *bytes, uint8_t len)
+{
+    return uart->write(reinterpret_cast<const uint8_t *>(bytes), len);
+}
+
+bool AP_ExternalAHRS_InertialLabs::handle_command(ExternalAHRS_command command)
+{
+    switch (command) {
+        case ExternalAHRS_command::START:
+            return write_bytes(Command::START_UDD, sizeof(Command::START_UDD) - 1);
+        case ExternalAHRS_command::STOP:
+            return write_bytes(Command::STOP, sizeof(Command::STOP) - 1);
+        case ExternalAHRS_command::ENABLE_GNSS:
+            return write_bytes(Command::ENABLE_GNSS, sizeof(Command::ENABLE_GNSS) - 1);
+        case ExternalAHRS_command::DISABLE_GNSS:
+            return write_bytes(Command::DISABLE_GNSS, sizeof(Command::DISABLE_GNSS) - 1);
+        default:
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ILAB: Invalid command for handling");
+            return false;
+    }
+}
+
 #endif  // AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
 

libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h

@@ -23,6 +23,7 @@
 #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
 
 #include "AP_ExternalAHRS_backend.h"
+#include "AP_ExternalAHRS_command_context.h"
 
 class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend {
 
@@ -220,6 +221,8 @@ 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) override;
+    bool handle_command(ExternalAHRS_command command) override;
 
     // re-sync on header bytes
     void re_sync(void);

libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h

@@ -42,6 +42,8 @@ class AP_ExternalAHRS_backend {
     virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0;
     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; }
+    virtual bool write_bytes(const char *bytes, uint8_t len) { return false; }
+    virtual bool handle_command(ExternalAHRS_command command) { return false; }
 
     // Check for new data.
     // This is used when there's not a separate thread for EAHRS.

libraries/AP_ExternalAHRS/AP_ExternalAHRS_command_context.h

@@ -0,0 +1,15 @@
+#pragma once
+
+#include "AP_ExternalAHRS_config.h"
+
+#if HAL_EXTERNAL_AHRS_ENABLED
+
+enum class ExternalAHRS_command {
+    INVALID,
+    START,
+    STOP,
+    ENABLE_GNSS,
+    DISABLE_GNSS,
+};
+
+#endif

libraries/GCS_MAVLink/GCS.h

@@ -632,6 +632,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_EXTERNAL_ENABLED
+    MAV_RESULT handle_EAHRS_message(const mavlink_command_int_t &packet);
+#endif
 
     virtual void send_banner();
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -110,6 +110,10 @@
 extern AP_IOMCU iomcu;
 #endif
 
+#if AP_AHRS_EXTERNAL_ENABLED
+#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
+#endif
+
 #include <ctype.h>
 
 extern const AP_HAL::HAL& hal;
@@ -4878,6 +4882,39 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_autopilot_capabilities(const mavl
 }
 #endif
 
+#if AP_AHRS_EXTERNAL_ENABLED
+MAV_RESULT GCS_MAVLINK::handle_EAHRS_message(const mavlink_command_int_t &packet)
+{
+    switch (packet.command) {
+        case MAV_CMD_EAHRS_START:
+        {
+            const bool result = AP::externalAHRS().handle_command(ExternalAHRS_command::START);
+            return result ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
+        }
+        case MAV_CMD_EAHRS_STOP:
+        {
+            const bool result = AP::externalAHRS().handle_command(ExternalAHRS_command::STOP);
+            return result ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
+        }
+
+        case MAV_CMD_EAHRS_ENABLE_GNSS:
+        {
+            const bool result = AP::externalAHRS().handle_command(ExternalAHRS_command::ENABLE_GNSS);
+            return result ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
+        }
+
+        case MAV_CMD_EAHRS_DISABLE_GNSS:
+        {
+            const bool result = AP::externalAHRS().handle_command(ExternalAHRS_command::DISABLE_GNSS);
+            return result ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
+        }
+
+        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 +5675,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_EXTERNAL_ENABLED
+    case MAV_CMD_EAHRS_START:
+    case MAV_CMD_EAHRS_STOP:
+    case MAV_CMD_EAHRS_ENABLE_GNSS:
+    case MAV_CMD_EAHRS_DISABLE_GNSS:
+        return handle_EAHRS_message(packet);
+#endif
     }
 
     return MAV_RESULT_UNSUPPORTED;