From 016f28aaeaa1610d69837addb42ffe2de9e275ef Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Sun, 27 Oct 2024 17:56:07 +0400 Subject: [PATCH] ExternalAHRS: Add the ability to send commands to the EAHRS --- libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp | 16 ++++++++++ libraries/AP_ExternalAHRS/AP_ExternalAHRS.h | 4 +++ .../AP_ExternalAHRS_InertialLabs.cpp | 30 +++++++++++++++++++ .../AP_ExternalAHRS_InertialLabs.h | 3 ++ .../AP_ExternalAHRS/AP_ExternalAHRS_backend.h | 2 ++ .../AP_ExternalAHRS_command_context.h | 15 ++++++++++ 6 files changed, 70 insertions(+) create mode 100644 libraries/AP_ExternalAHRS/AP_ExternalAHRS_command_context.h diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 07d875e57de959..7b50e60f8fa0f3 100644 --- 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 index c5913baa8c6dbb..80fd6fc7d1bc38 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -28,6 +28,8 @@ #include #include +#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 index fb057d000fde15..63f49cd73329cd 100644 --- 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(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 index 0db51358e79b34..be91db572429af 100644 --- 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 index 8ee9af82ce5944..2dce0d24734e03 100644 --- 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 new file mode 100644 index 00000000000000..3e199b3931549e --- /dev/null +++ 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