GCS_MAVLink: add support for ATTITUDE_QUATERNION

libraries/GCS_MAVLink/GCS.h

@@ -247,6 +247,7 @@ public:
     void send_battery2();
     void send_opticalflow();
     virtual void send_attitude() const;
+    virtual void send_attitude_quaternion() const;
     void send_autopilot_version() const;
     void send_extended_sys_state() const;
     void send_local_position() const;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -790,6 +790,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
     } map[] {
         { MAVLINK_MSG_ID_HEARTBEAT,             MSG_HEARTBEAT},
         { MAVLINK_MSG_ID_ATTITUDE,              MSG_ATTITUDE},
+        { MAVLINK_MSG_ID_ATTITUDE_QUATERNION,   MSG_ATTITUDE_QUATERNION},
         { MAVLINK_MSG_ID_GLOBAL_POSITION_INT,   MSG_LOCATION},
         { MAVLINK_MSG_ID_HOME_POSITION,         MSG_HOME},
         { MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN,     MSG_ORIGIN},
@@ -4541,6 +4542,29 @@ void GCS_MAVLINK::send_attitude() const
         omega.z);
 }
 
+void GCS_MAVLINK::send_attitude_quaternion() const
+{
+    const AP_AHRS &ahrs = AP::ahrs();
+    Quaternion quat;
+    if (!ahrs.get_secondary_quaternion(quat)) {
+        return;
+    }
+    const Vector3f omega = ahrs.get_gyro();
+    const float repr_offseq_q[] {0,0,0,0};  // unused, but probably should correspond to the AHRS view?
+    mavlink_msg_attitude_quaternion_send(
+        chan,
+        AP_HAL::millis(),
+        quat.q1,
+        quat.q2,
+        quat.q3,
+        quat.q4,
+        omega.x, // rollspeed
+        omega.y, // pitchspeed
+        omega.z, // yawspeed
+        repr_offseq_q
+        );
+}
+
 int32_t GCS_MAVLINK::global_position_int_alt() const {
     return global_position_current_loc.alt * 10UL;
 }
@@ -4646,6 +4670,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_attitude();
         break;
 
+    case MSG_ATTITUDE_QUATERNION:
+        CHECK_PAYLOAD_SIZE(ATTITUDE_QUATERNION);
+        send_attitude_quaternion();
+        break;
+
     case MSG_NEXT_PARAM:
         CHECK_PAYLOAD_SIZE(PARAM_VALUE);
         queued_param_send();

libraries/GCS_MAVLink/ap_message.h

@@ -9,6 +9,7 @@
 enum ap_message : uint8_t {
     MSG_HEARTBEAT,
     MSG_ATTITUDE,
+    MSG_ATTITUDE_QUATERNION,
     MSG_LOCATION,
     MSG_SYS_STATUS,
     MSG_POWER_STATUS,