diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index 07d875e57de95..7b50e60f8fa0f 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 c5913baa8c6db..80fd6fc7d1bc3 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 fb057d000fde1..63f49cd73329c 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 0db51358e79b3..be91db572429a 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 8ee9af82ce594..2dce0d24734e0 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 0000000000000..3e199b3931549 --- /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 diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 649a12a6ad633..d79542a083c39 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/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(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e670339e7b756..1c48a3e597382 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -110,6 +110,10 @@ extern AP_IOMCU iomcu; #endif +#if AP_AHRS_EXTERNAL_ENABLED +#include +#endif + #include 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; diff --git a/modules/mavlink b/modules/mavlink index 4c64b9522f9bb..2437a2dbdbb33 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 4c64b9522f9bb9815b089ab98cf2f33f11bded52 +Subproject commit 2437a2dbdbb33eca050d2e4889b2ec2be6700b5b