From aa7844e4a7062680d7c2d572652638d17861da6f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 18 Dec 2018 22:55:24 +1100 Subject: [PATCH] GCS_MAVLink: split SIMSTATE, AHRS2 and AHRS3 onto their own ap_message ids --- libraries/GCS_MAVLink/GCS.h | 3 +++ libraries/GCS_MAVLink/GCS_Common.cpp | 24 +++++++++++++++++++++--- 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 61c2aac8ac..c51c43b41e 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -68,6 +68,8 @@ enum ap_message : uint8_t { MSG_FENCE_STATUS, MSG_AHRS, MSG_SIMSTATE, + MSG_AHRS2, + MSG_AHRS3, MSG_HWSTATUS, MSG_WIND, MSG_RANGEFINDER, @@ -190,6 +192,7 @@ public: virtual void send_rangefinder() const; void send_proximity() const; void send_ahrs2(); + void send_ahrs3(); void send_system_time(); void send_radio_in(); void send_raw_imu(); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b488b1f018..e48a2ed8cc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -387,7 +387,8 @@ void GCS_MAVLINK::send_ahrs2() const AP_AHRS &ahrs = AP::ahrs(); Vector3f euler; struct Location loc {}; - if (ahrs.get_secondary_attitude(euler)) { + if (ahrs.get_secondary_attitude(euler) || + ahrs.get_secondary_position(loc)) { mavlink_msg_ahrs2_send(chan, euler.x, euler.y, @@ -396,11 +397,18 @@ void GCS_MAVLINK::send_ahrs2() loc.lat, loc.lng); } - const AP_AHRS_NavEKF &_ahrs = reinterpret_cast(ahrs); - const NavEKF2 &ekf2 = _ahrs.get_NavEKF2_const(); +#endif +} + +void GCS_MAVLINK::send_ahrs3() +{ +#if AP_AHRS_NAVEKF_AVAILABLE + const NavEKF2 &ekf2 = AP::ahrs_navekf().get_NavEKF2_const(); if (ekf2.activeCores() > 0 && HAVE_PAYLOAD_SPACE(chan, AHRS3)) { + struct Location loc {}; ekf2.getLLH(loc); + Vector3f euler; ekf2.getEulerAngles(-1,euler); mavlink_msg_ahrs3_send(chan, euler.x, @@ -899,6 +907,8 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_FENCE_STATUS, MSG_FENCE_STATUS}, { MAVLINK_MSG_ID_AHRS, MSG_AHRS}, { MAVLINK_MSG_ID_SIMSTATE, MSG_SIMSTATE}, + { MAVLINK_MSG_ID_AHRS2, MSG_AHRS2}, + { MAVLINK_MSG_ID_AHRS3, MSG_AHRS3}, { MAVLINK_MSG_ID_HWSTATUS, MSG_HWSTATUS}, { MAVLINK_MSG_ID_WIND, MSG_WIND}, { MAVLINK_MSG_ID_RANGEFINDER, MSG_RANGEFINDER}, @@ -3835,10 +3845,18 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) case MSG_SIMSTATE: CHECK_PAYLOAD_SIZE(SIMSTATE); send_simstate(); + break; + + case MSG_AHRS2: CHECK_PAYLOAD_SIZE(AHRS2); send_ahrs2(); break; + case MSG_AHRS3: + CHECK_PAYLOAD_SIZE(AHRS3); + send_ahrs3(); + break; + case MSG_AHRS: CHECK_PAYLOAD_SIZE(AHRS); send_ahrs();