AP_Mount: replace send-mount-status with send-gimbal-device-attitude-status

libraries/AP_Mount/AP_Mount.cpp

@@ -742,13 +742,13 @@ void AP_Mount::handle_mount_control(const mavlink_message_t &msg)
     _backends[_primary]->handle_mount_control(packet);
 }
 
-/// Return mount status information
-void AP_Mount::send_mount_status(mavlink_channel_t chan)
+// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
+void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan)
 {
-    // call send_mount_status for each instance
+    // call send_gimbal_device_attitude_status for each instance
     for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
-            _backends[instance]->send_mount_status(chan);
+            _backends[instance]->send_gimbal_device_attitude_status(chan);
         }
     }
 }

libraries/AP_Mount/AP_Mount.h

@@ -146,8 +146,8 @@ public:
     void handle_param_value(const mavlink_message_t &msg);
     void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
 
-    // send a MOUNT_STATUS message to GCS:
-    void send_mount_status(mavlink_channel_t chan);
+    // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
+    void send_gimbal_device_attitude_status(mavlink_channel_t chan);
 
     // run pre-arm check.  returns false on failure and fills in failure_msg
     // any failure_msg returned will not include a prefix

libraries/AP_Mount/AP_Mount_Alexmos.cpp

@@ -102,15 +102,19 @@ bool AP_Mount_Alexmos::has_pan_control() const
     return _gimbal_3axis;
 }
 
-// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_Alexmos::send_mount_status(mavlink_channel_t chan)
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_Alexmos::get_attitude_quaternion(Quaternion& att_quat)
 {
     if (!_initialised) {
-        return;
+        return false;
     }
 
+    // request attitude from gimbal
     get_angles();
-    mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y*100, _current_angle.x*100, _current_angle.z*100, _mode);
+
+    // construct quaternion
+    att_quat.from_euler(radians(_current_angle.x), radians(_current_angle.y), radians(_current_angle.z));
+    return true;
 }
 
 /*

libraries/AP_Mount/AP_Mount_Alexmos.h

@@ -81,8 +81,10 @@ public:
     // has_pan_control - returns true if this mount can control its pan (required for multicopters)
     bool has_pan_control() const override;
 
-    // send_mount_status - called to allow mounts to send their status to GCS via MAVLink
-    void send_mount_status(mavlink_channel_t chan) override;
+protected:
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
 
 private:
 

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -65,6 +65,34 @@ void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &p
     _state._stab_pan.set(packet.stab_yaw);
 }
 
+// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
+void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan)
+{
+    if (suppress_heartbeat()) {
+        // block heartbeat from transmitting to the GCS
+        GCS_MAVLINK::disable_channel_routing(chan);
+    }
+
+    Quaternion att_quat;
+    if (!get_attitude_quaternion(att_quat)) {
+        return;
+    }
+
+    // construct quaternion array
+    const float quat_array[4] = {att_quat.q1, att_quat.q2, att_quat.q3, att_quat.q4};
+
+    mavlink_msg_gimbal_device_attitude_status_send(chan,
+                                                   0,   // target system
+                                                   0,   // target component
+                                                   AP_HAL::millis(),    // autopilot system time
+                                                   get_gimbal_device_flags(),
+                                                   quat_array,    // attitude expressed as quaternion
+                                                   std::numeric_limits<double>::quiet_NaN(),    // roll axis angular velocity (NaN for unknown)
+                                                   std::numeric_limits<double>::quiet_NaN(),    // pitch axis angular velocity (NaN for unknown)
+                                                   std::numeric_limits<double>::quiet_NaN(),    // yaw axis angular velocity (NaN for unknown)
+                                                   0);  // failure flags (not supported)
+}
+
 // process MOUNT_CONTROL messages received from GCS. deprecated.
 void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet)
 {
@@ -310,6 +338,16 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad
     }
 }
 
+// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
+uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
+{
+    const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
+                           (get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
+                           GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
+                           GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // pitch angle is always earth-frame, yaw_angle is always body-frame
+    return flags;
+}
+
 // get angle targets (in radians) to home location
 // returns true on success, false on failure
 bool AP_Mount_Backend::get_angle_target_to_home(MountTarget& angle_rad) const

libraries/AP_Mount/AP_Mount_Backend.h

@@ -82,8 +82,8 @@ public:
     // process MOUNT_CONTROL messages received from GCS. deprecated.
     void handle_mount_control(const mavlink_mount_control_t &packet);
 
-    // send_mount_status - called to allow mounts to send their status to GCS via MAVLink
-    virtual void send_mount_status(mavlink_channel_t chan) = 0;
+    // send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
+    void send_gimbal_device_attitude_status(mavlink_channel_t chan);
 
     // handle a GIMBAL_REPORT message
     virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}
@@ -115,6 +115,12 @@ protected:
         bool yaw_is_ef;
     };
 
+    // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal)
+    virtual bool suppress_heartbeat() const { return false; }
+
+    // get attitude as a quaternion.  returns true on success
+    virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0;
+
     // get pilot input (in the range -1 to +1) received through RC
     void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;
 
@@ -153,6 +159,9 @@ protected:
     // assumes a 50hz update rate
     void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const;
 
+    // helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
+    uint16_t get_gimbal_device_flags() const;
+
     AP_Mount    &_frontend; // reference to the front end which holds parameters
     AP_Mount::mount_state &_state;    // references to the parameters and state for this backend
     uint8_t     _instance;  // this instance's number

libraries/AP_Mount/AP_Mount_Gremsy.cpp

@@ -121,31 +121,17 @@ bool AP_Mount_Gremsy::healthy() const
     return true;
 }
 
-// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_Gremsy::send_mount_status(mavlink_channel_t chan)
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_Gremsy::get_attitude_quaternion(Quaternion& att_quat)
 {
-    // check we have space for the message
-    if (!HAVE_PAYLOAD_SPACE(chan, GIMBAL_DEVICE_ATTITUDE_STATUS)) {
-        return;
-    }
-
     // check we have received an updated message
     if (_gimbal_device_attitude_status.time_boot_ms == _sent_gimbal_device_attitude_status_ms) {
-        return;
+        return false;
     }
     _sent_gimbal_device_attitude_status_ms = _gimbal_device_attitude_status.time_boot_ms;
 
-    // forward on message to GCS
-    mavlink_msg_gimbal_device_attitude_status_send(chan,
-                                                   0,    // target system
-                                                   0,    // target component
-                                                   AP_HAL::millis(),    // autopilot system time
-                                                   _gimbal_device_attitude_status.flags,
-                                                   _gimbal_device_attitude_status.q,
-                                                   _gimbal_device_attitude_status.angular_velocity_x,
-                                                   _gimbal_device_attitude_status.angular_velocity_y,
-                                                   _gimbal_device_attitude_status.angular_velocity_z,
-                                                   _gimbal_device_attitude_status.failure_flags);
+    att_quat = _gimbal_device_attitude_status.q;
+    return true;
 }
 
 // search for gimbal in GCS_MAVLink routing table

libraries/AP_Mount/AP_Mount_Gremsy.h

@@ -36,15 +36,17 @@ public:
     // has_pan_control
     bool has_pan_control() const override { return true; }
 
-    // send_mount_status
-    void send_mount_status(mavlink_channel_t chan) override;
-
     // handle GIMBAL_DEVICE_INFORMATION message
     void handle_gimbal_device_information(const mavlink_message_t &msg) override;
 
     // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
     void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) override;
 
+protected:
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
+
 private:
 
     // search for gimbal in GCS_MAVLink routing table

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -123,11 +123,11 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode)
     _mode = mode;
 }
 
-// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_SToRM32::send_mount_status(mavlink_channel_t chan)
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_SToRM32::get_attitude_quaternion(Quaternion& att_quat)
 {
-    // return target angles as gimbal's actual attitude.  To-Do: retrieve actual gimbal attitude and send these instead
-    mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.pitch)*100, degrees(_angle_rad.roll)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode);
+    att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad));
+    return true;
 }
 
 // search for gimbal in GCS_MAVLink routing table

libraries/AP_Mount/AP_Mount_SToRM32.h

@@ -35,8 +35,10 @@ public:
     // set_mode - sets mount's mode
     void set_mode(enum MAV_MOUNT_MODE mode) override;
 
-    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    void send_mount_status(mavlink_channel_t chan) override;
+protected:
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
 
 private:
 

libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp

@@ -152,11 +152,11 @@ void AP_Mount_SToRM32_serial::set_mode(enum MAV_MOUNT_MODE mode)
     _mode = mode;
 }
 
-// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_SToRM32_serial::send_mount_status(mavlink_channel_t chan)
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_SToRM32_serial::get_attitude_quaternion(Quaternion& att_quat)
 {
-    // return target angles as gimbal's actual attitude.
-    mavlink_msg_mount_status_send(chan, 0, 0, _current_angle.y, _current_angle.x, _current_angle.z, _mode);
+    att_quat.from_euler(radians(_current_angle.x * 0.01f), radians(_current_angle.y * 0.01f), radians(_current_angle.z * 0.01f));
+    return true;
 }
 
 bool AP_Mount_SToRM32_serial::can_send(bool with_control) {

libraries/AP_Mount/AP_Mount_SToRM32_serial.h

@@ -38,8 +38,10 @@ public:
     // set_mode - sets mount's mode
     void set_mode(enum MAV_MOUNT_MODE mode) override;
 
-    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    void send_mount_status(mavlink_channel_t chan) override;
+protected:
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
 
 private:
 

libraries/AP_Mount/AP_Mount_Servo.cpp

@@ -120,14 +120,15 @@ bool AP_Mount_Servo::has_pan_control() const
     return SRV_Channels::function_assigned(_pan_idx);
 }
 
-// private methods
-
-// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_Servo::send_mount_status(mavlink_channel_t chan)
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat)
 {
-    mavlink_msg_mount_status_send(chan, 0, 0, _angle_bf_output_deg.y*100, _angle_bf_output_deg.x*100, _angle_bf_output_deg.z*100, _mode);
+    att_quat.from_euler(radians(_angle_bf_output_deg.x), radians(_angle_bf_output_deg.y), radians(_angle_bf_output_deg.z));
+    return true;
 }
 
+// private methods
+
 // update body-frame angle outputs from earth-frame angle targets
 void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad)
 {

libraries/AP_Mount/AP_Mount_Servo.h

@@ -39,8 +39,10 @@ public:
     // returns true if this mount can control its pan (required for multicopters)
     bool has_pan_control() const override;
 
-    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    void send_mount_status(mavlink_channel_t chan) override;
+protected:
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
 
 private:
 

libraries/AP_Mount/AP_Mount_SoloGimbal.cpp

@@ -115,15 +115,14 @@ void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode)
     _mode = mode;
 }
 
-// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-void AP_Mount_SoloGimbal::send_mount_status(mavlink_channel_t chan)
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_SoloGimbal::get_attitude_quaternion(Quaternion& att_quat)
 {
-    if (_gimbal.aligned()) {
-        mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.roll)*100, degrees(_angle_rad.pitch)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode);
+    if (!_gimbal.aligned()) {
+        return false;
     }
-
-    // block heartbeat from transmitting to the GCS
-    GCS_MAVLINK::disable_channel_routing(chan);
+    att_quat.from_euler(_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad));
+    return true;
 }
 
 /*

libraries/AP_Mount/AP_Mount_SoloGimbal.h

@@ -35,9 +35,6 @@ public:
     // set_mode - sets mount's mode
     void set_mode(enum MAV_MOUNT_MODE mode) override;
 
-    // send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
-    void send_mount_status(mavlink_channel_t chan) override;
-
     // handle a GIMBAL_REPORT message
     void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) override;
     void handle_gimbal_torque_report(mavlink_channel_t chan, const mavlink_message_t &msg);
@@ -45,6 +42,14 @@ public:
 
     void update_fast() override;
 
+protected:
+
+    // returns true if heart beat should be suppressed for this gimbal
+    bool suppress_heartbeat() const override { return true; }
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
+
 private:
     // internal variables
     bool _initialised;              // true once the driver has been initialised