ExternalAHRS: Add the ability to send commands to the EAHRS

ArduPilot · Nov 29, 2024 · 016f28a · 016f28a
1 parent 5648795
commit 016f28a
Show file tree

Hide file tree

Showing 6 changed files with 70 additions and 0 deletions.
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/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) {

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/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();

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/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
 
diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/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);

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/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.

diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_command_context.h b/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