AP_Mount: add Gremsy driver

Co-authored-by: bugobliterator <siddharthbharatpurohit@gmail.com>

this drivers special features include
sends ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL
support of RC rate targets
captures and re-forwards gimbal_device_attitude_status

libraries/AP_Mount/AP_Mount.cpp

@@ -10,6 +10,7 @@
 #include "AP_Mount_Alexmos.h"
 #include "AP_Mount_SToRM32.h"
 #include "AP_Mount_SToRM32_serial.h"
+#include "AP_Mount_Gremsy.h"
 #include <AP_Math/location.h>
 
 const AP_Param::GroupInfo AP_Mount::var_info[] = {
@@ -17,7 +18,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Mount Type
     // @Description: Mount Type (None, Servo or MAVLink)
-    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial
+    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 19, AP_Mount, state[0]._type, 0, AP_PARAM_FLAG_ENABLE),
@@ -392,7 +393,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
     // @Param: 2_TYPE
     // @DisplayName: Mount2 Type
     // @Description: Mount Type (None, Servo or MAVLink)
-    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial
+    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy
     // @User: Standard
     AP_GROUPINFO("2_TYPE",           42, AP_Mount, state[1]._type, 0),
 #endif // AP_MOUNT_MAX_INSTANCES > 1
@@ -466,6 +467,11 @@ void AP_Mount::init()
         } else if (mount_type == Mount_Type_SToRM32_serial) {
             _backends[instance] = new AP_Mount_SToRM32_serial(*this, state[instance], instance);
             _num_instances++;
+
+        // check for Gremsy mounts
+        } else if (mount_type == Mount_Type_Gremsy) {
+            _backends[instance] = new AP_Mount_Gremsy(*this, state[instance], instance);
+            _num_instances++;
         }
 
         // init new instance
@@ -766,6 +772,9 @@ void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &m
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
         handle_global_position_int(msg);
         break;
+    case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION:
+        handle_gimbal_device_information(msg);
+        break;
     case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS:
         handle_gimbal_device_attitude_status(msg);
         break;
@@ -788,6 +797,16 @@ void AP_Mount::handle_param_value(const mavlink_message_t &msg)
 }
 
 
+// handle GIMBAL_DEVICE_INFORMATION message
+void AP_Mount::handle_gimbal_device_information(const mavlink_message_t &msg)
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->handle_gimbal_device_information(msg);
+        }
+    }
+}
+
 // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
 void AP_Mount::handle_gimbal_device_attitude_status(const mavlink_message_t &msg)
 {

libraries/AP_Mount/AP_Mount.h

@@ -46,6 +46,7 @@ class AP_Mount_SoloGimbal;
 class AP_Mount_Alexmos;
 class AP_Mount_SToRM32;
 class AP_Mount_SToRM32_serial;
+class AP_Mount_Gremsy;
 
 /*
   This is a workaround to allow the MAVLink backend access to the
@@ -61,6 +62,7 @@ class AP_Mount
     friend class AP_Mount_Alexmos;
     friend class AP_Mount_SToRM32;
     friend class AP_Mount_SToRM32_serial;
+    friend class AP_Mount_Gremsy;
 
 public:
     AP_Mount();
@@ -81,7 +83,8 @@ public:
         Mount_Type_SoloGimbal = 2,      /// Solo's gimbal
         Mount_Type_Alexmos = 3,         /// Alexmos mount
         Mount_Type_SToRM32 = 4,         /// SToRM32 mount using MAVLink protocol
-        Mount_Type_SToRM32_serial = 5   /// SToRM32 mount using custom serial protocol
+        Mount_Type_SToRM32_serial = 5,  /// SToRM32 mount using custom serial protocol
+        Mount_Type_Gremsy = 6           /// Gremsy gimbal using MAVLink v2 Gimbal protocol
     };
 
     // init - detect and initialise all mounts
@@ -207,6 +210,7 @@ private:
     MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet);
     void handle_global_position_int(const mavlink_message_t &msg);
+    void handle_gimbal_device_information(const mavlink_message_t &msg);
     void handle_gimbal_device_attitude_status(const mavlink_message_t &msg);
 };
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -79,6 +79,9 @@ public:
     // handle a GLOBAL_POSITION_INT message
     bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet);
 
+    // handle GIMBAL_DEVICE_INFORMATION message
+    virtual void handle_gimbal_device_information(const mavlink_message_t &msg) {}
+
     // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
     virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {}
 

libraries/AP_Mount/AP_Mount_Gremsy.cpp

@@ -0,0 +1,311 @@
+#include "AP_Mount_Gremsy.h"
+#if HAL_MOUNT_ENABLED
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define AP_MOUNT_GREMSY_RESEND_MS  1000     // resend angle targets to gimbal at least once per second
+#define AP_MOUNT_GREMSY_SEARCH_MS  60000    // search for gimbal for 1 minute after startup
+#define AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US    10000  // send ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE at 100hz
+
+AP_Mount_Gremsy::AP_Mount_Gremsy(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
+    AP_Mount_Backend(frontend, state, instance)
+{}
+
+// update mount position
+void AP_Mount_Gremsy::update()
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        find_gimbal();
+        return;
+    }
+
+    // update based on mount mode
+    switch (get_mode()) {
+
+        // move mount to a "retracted" position.  We disable motors
+        case MAV_MOUNT_MODE_RETRACT:
+            // handled below
+            enable_motors(false);
+            break;
+
+        // move mount to a neutral position, typically pointing forward
+        case MAV_MOUNT_MODE_NEUTRAL: {
+            const Vector3f &target = _state._neutral_angles.get();
+            _angle_ef_target_rad.x = ToRad(target.x);
+            _angle_ef_target_rad.y = ToRad(target.y);
+            _angle_ef_target_rad.z = ToRad(target.z);
+            send_gimbal_device_set_attitude(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z, false);
+            }
+            break;
+
+        // point to the angles given by a mavlink message
+        case MAV_MOUNT_MODE_MAVLINK_TARGETING:
+            // angle targets should have been set by a MOUNT_CONTROL message from GCS
+            send_gimbal_device_set_attitude(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z, true);
+            break;
+
+        // RC radio manual angle control, but with stabilization from the AHRS
+        case MAV_MOUNT_MODE_RC_TARGETING:
+            // update targets using pilot's rc inputs
+            update_targets_from_rc();
+            if (_rate_target_rads_valid) {
+                send_gimbal_device_set_rate(_rate_target_rads.x, _rate_target_rads.y, _rate_target_rads.z, _state._yaw_lock);
+            } else {
+                send_gimbal_device_set_attitude(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z, _state._yaw_lock);
+            }
+            break;
+
+        // point mount to a GPS point given by the mission planner
+        case MAV_MOUNT_MODE_GPS_POINT:
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, false)) {
+                send_gimbal_device_set_attitude(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z, true);
+            }
+            break;
+
+        case MAV_MOUNT_MODE_HOME_LOCATION:
+            // constantly update the home location:
+            if (!AP::ahrs().home_is_set()) {
+                break;
+            }
+            _state._roi_target = AP::ahrs().get_home();
+            _state._roi_target_set = true;
+            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, false)) {
+                send_gimbal_device_set_attitude(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z, true);
+            }
+            break;
+
+        case MAV_MOUNT_MODE_SYSID_TARGET:
+            if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true, false)) {
+                send_gimbal_device_set_attitude(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z, true);
+            }
+            break;
+
+        default:
+            // unknown mode so do nothing
+            break;
+    }
+}
+
+// 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)
+{
+    // 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;
+    }
+    _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);
+}
+
+// search for gimbal in GCS_MAVLink routing table
+void AP_Mount_Gremsy::find_gimbal()
+{
+    // do not look for gimbal for first 10 seconds so user may see banner
+    if (AP_HAL::millis() < 10000) {
+        return;
+    }
+
+    // return if search time has has passed
+    if (AP_HAL::millis() > AP_MOUNT_GREMSY_SEARCH_MS) {
+        return;
+    }
+
+    // search for a mavlink enabled gimbal
+    if (!_found_gimbal) {
+        mavlink_channel_t chan;
+        uint8_t sysid, compid;
+        if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_GIMBAL, sysid, compid, chan)) {
+            if (((_instance == 0) && (compid == MAV_COMP_ID_GIMBAL)) ||
+                ((_instance == 1) && (compid == MAV_COMP_ID_GIMBAL2))) {
+                _found_gimbal = true;
+                _sysid = sysid;
+                _compid = compid;
+                _chan = chan;
+            }
+        } else {
+            // have not yet found a gimbal so return
+            return;
+        }
+    }
+
+    // request GIMBAL_DEVICE_INFORMATION
+    if (!_got_device_info) {
+        uint32_t now_ms = AP_HAL::millis();
+        if (now_ms - _last_devinfo_req_ms > 1000) {
+            _last_devinfo_req_ms = now_ms;
+            request_gimbal_device_information();
+        }
+        return;
+    }
+
+    // start sending autopilot attitude to gimbal
+    if (start_sending_attitude_to_gimbal()) {
+        _initialised = true;
+    }
+}
+
+// handle GIMBAL_DEVICE_INFORMATION message
+void AP_Mount_Gremsy::handle_gimbal_device_information(const mavlink_message_t &msg)
+{
+    // exit immediately if this is not our message
+    if (msg.sysid != _sysid || msg.compid != _compid) {
+        return;
+    }
+
+    mavlink_gimbal_device_information_t info;
+    mavlink_msg_gimbal_device_information_decode(&msg, &info);
+
+    // set parameter defaults from gimbal information
+    _state._roll_angle_min.set_default(degrees(info.roll_min) * 100);
+    _state._roll_angle_max.set_default(degrees(info.roll_max) * 100);
+    _state._tilt_angle_min.set_default(degrees(info.pitch_min) * 100);
+    _state._tilt_angle_max.set_default(degrees(info.pitch_max) * 100);
+    _state._pan_angle_min.set_default(degrees(info.yaw_min) * 100);
+    _state._pan_angle_max.set_default(degrees(info.yaw_max) * 100);
+
+    const uint8_t fw_ver_major = info.firmware_version & 0x000000FF;
+    const uint8_t fw_ver_minor = (info.firmware_version & 0x0000FF00) >> 8;
+    const uint8_t fw_ver_revision = (info.firmware_version & 0x00FF0000) >> 16;
+    const uint8_t fw_ver_build = (info.firmware_version & 0xFF000000) >> 24;
+
+    // display gimbal info to user
+    gcs().send_text(MAV_SEVERITY_INFO, "Mount: %s %s fw:%u.%u.%u.%u",
+            info.vendor_name,
+            info.model_name,
+            (unsigned)fw_ver_major,
+            (unsigned)fw_ver_minor,
+            (unsigned)fw_ver_revision,
+            (unsigned)fw_ver_build);
+
+    _got_device_info = true;
+}
+
+// handle GIMBAL_DEVICE_ATTITUDE_STATUS message
+void AP_Mount_Gremsy::handle_gimbal_device_attitude_status(const mavlink_message_t &msg)
+{
+    // exit immediately if this is not our message
+    if (msg.sysid != _sysid || msg.compid != _compid) {
+        return;
+    }
+
+    // take copy of message so it can be forwarded onto GCS later
+    mavlink_msg_gimbal_device_attitude_status_decode(&msg, &_gimbal_device_attitude_status);
+}
+
+// request GIMBAL_DEVICE_INFORMATION message
+void AP_Mount_Gremsy::request_gimbal_device_information() const
+{
+    // check we have space for the message
+    if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) {
+        return;
+    }
+
+    mavlink_msg_command_long_send(
+        _chan,
+        _sysid,
+        _compid,
+        MAV_CMD_REQUEST_MESSAGE,
+        0, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, 0, 0, 0, 0, 0, 0);
+}
+
+// start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal
+bool AP_Mount_Gremsy::start_sending_attitude_to_gimbal()
+{
+    // send AUTOPILOT_STATE_FOR_GIMBAL_DEVICE
+    const MAV_RESULT res = gcs().set_message_interval(_chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, AP_MOUNT_GREMSY_ATTITUDE_INTERVAL_US);
+
+    // return true on success
+    return (res == MAV_RESULT_ACCEPTED);
+}
+
+// send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate
+// earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame
+void AP_Mount_Gremsy::send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const
+{
+    // check we have space for the message
+    if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
+        return;
+    }
+
+    // prepare flags
+    const uint16_t flags = earth_frame ? (GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK | GIMBAL_DEVICE_FLAGS_YAW_LOCK) : 0;
+    const float quat_array[4] = {NAN, NAN, NAN, NAN};
+
+    // send command_long command containing a do_mount_control command
+    mavlink_msg_gimbal_device_set_attitude_send(_chan,
+                                                _sysid,     // target system
+                                                _compid,    // target component
+                                                flags,      // gimbal device flags
+                                                quat_array, // attitude as a quaternion
+                                                roll_rads, pitch_rads, yaw_rads);   // angular velocities
+}
+
+// send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control attitude
+// earth_frame should be true if yaw_rad target is in earth frame angle, false if body_frame
+void AP_Mount_Gremsy::send_gimbal_device_set_attitude(float roll_rad, float pitch_rad, float yaw_rad, bool earth_frame) const
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    // check we have space for the message
+    if (!HAVE_PAYLOAD_SPACE(_chan, GIMBAL_DEVICE_SET_ATTITUDE)) {
+        return;
+    }
+
+    // prepare flags
+    const uint16_t flags = earth_frame ? (GIMBAL_DEVICE_FLAGS_ROLL_LOCK | GIMBAL_DEVICE_FLAGS_PITCH_LOCK | GIMBAL_DEVICE_FLAGS_YAW_LOCK) : 0;
+
+    // convert euler angles to quaternion
+    Quaternion q;
+    q.from_euler(roll_rad, pitch_rad, yaw_rad);
+    const float quat_array[4] = {q.q1, q.q2, q.q3, q.q4};
+
+    // send command_long command containing a do_mount_control command
+    mavlink_msg_gimbal_device_set_attitude_send(_chan,
+                                                _sysid,     // target system
+                                                _compid,    // target component
+                                                flags,      // gimbal device flags
+                                                quat_array, // attitude as a quaternion
+                                                NAN, NAN, NAN);   // angular velocities
+}
+
+// turn motors on/off
+void AP_Mount_Gremsy::enable_motors(bool on) const
+{
+    // check we have space for the message
+    if (!HAVE_PAYLOAD_SPACE(_chan, COMMAND_LONG)) {
+        return;
+    }
+
+    // send COMPMAND_LONG with MAV_CMD_USER_1 command with Param7 populated to turn on/off motor
+    mavlink_msg_command_long_send(_chan,
+                                  _sysid,
+                                  _compid,
+                                  MAV_CMD_USER_1,
+                                  0,                // confirmation of zero means this is the first time this message has been sent
+                                  0, 0, 0, 0, 0, 0, // param1 to param6 unused
+                                  on ? 0x01 : 0x00);// 0=OFF, 1=ON
+}
+
+#endif // HAL_MOUNT_ENABLED

libraries/AP_Mount/AP_Mount_Gremsy.h

@@ -0,0 +1,79 @@
+/*
+  SToRM32 mount using serial protocol backend class
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+#include <AP_Math/AP_Math.h>
+#include <AP_Common/AP_Common.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include "AP_Mount_Backend.h"
+
+#if HAL_MOUNT_ENABLED
+
+
+class AP_Mount_Gremsy : public AP_Mount_Backend
+{
+
+public:
+    // Constructor
+    AP_Mount_Gremsy(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
+
+    // init
+    void init() override {}
+
+    // update mount position
+    void update() override;
+
+    // has_pan_control
+    bool has_pan_control() const override { return true; }
+
+    // set_mode
+    void set_mode(enum MAV_MOUNT_MODE mode) override { _state._mode = mode; }
+
+    // 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;
+
+private:
+
+    // search for gimbal in GCS_MAVLink routing table
+    void find_gimbal();
+
+    // request GIMBAL_DEVICE_INFORMATION from gimbal (holds vendor and model name, max lean angles)
+    void request_gimbal_device_information() const;
+
+    // start sending ATTITUDE and AUTOPILOT_STATE_FOR_GIMBAL_DEVICE to gimbal
+    // returns true on success, false on failure to start sending
+    bool start_sending_attitude_to_gimbal();
+
+    // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control rate
+    // earth_frame should be true if yaw_rads target is an earth frame rate, false if body_frame
+    void send_gimbal_device_set_rate(float roll_rads, float pitch_rads, float yaw_rads, bool earth_frame) const;
+
+    // send GIMBAL_DEVICE_SET_ATTITUDE to gimbal to control attitude
+    // earth_frame should be true if yaw_rad target is an earth frame angle, false if body_frame
+    void send_gimbal_device_set_attitude(float roll_rad, float pitch_rad, float yaw_rad, bool earth_frame) const;
+
+    // turn motors on/off
+    void enable_motors(bool on) const;
+
+    // internal variables
+    bool _found_gimbal;             // true once a MAVLink enabled gimbal has been found
+    bool _got_device_info;          // true once gimbal has provided device info
+    bool _initialised;              // true once the gimbal has provided a GIMBAL_DEVICE_INFORMATION
+    uint32_t _last_devinfo_req_ms;  // system time that GIMBAL_DEVICE_INFORMATION was last requested (used to throttle requests)
+    mavlink_channel_t _chan;        // mavlink channel used to communicate with gimbal
+    uint8_t _sysid;                 // sysid of gimbal
+    uint8_t _compid;                // component id of gimbal
+    mavlink_gimbal_device_attitude_status_t _gimbal_device_attitude_status;  // copy of most recently received gimbal status
+    uint32_t _sent_gimbal_device_attitude_status_ms;    // time_boot_ms field of gimbal_device_status message last forwarded to the GCS (used to prevent sending duplicates)
+};
+#endif // HAL_MOUNT_ENABLED