WIP - gimbal_manager_status and mav_cmd_do_gimbal_manager_configure support

libraries/AP_Mount/AP_Mount.cpp

@@ -337,6 +337,26 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
     return MAV_RESULT_FAILED;
 }
 
+MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet)
+{
+    AP_Mount_Backend *backend;
+
+    // check gimbal device id.  0 is primary, 1 is 1st gimbal, 2 is
+    // 2nd gimbal, etc
+    const uint8_t instance = packet.param7;
+    if (instance == 0) {
+        backend = get_primary();
+    } else {
+        backend = get_instance(instance - 1);
+    }
+
+    if (backend == nullptr) {
+        return MAV_RESULT_FAILED;
+    }
+
+    return backend->handle_command_do_gimbal_manager_configure(packet);    
+}
+
 void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){
     mavlink_gimbal_manager_set_attitude_t packet;
     mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet);
@@ -411,6 +431,8 @@ MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet)
         return handle_command_do_mount_control(packet);
     case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
         return handle_command_do_gimbal_manager_pitchyaw(packet);
+    case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
+        return handle_command_do_gimbal_manager_configure(packet);
     default:
         return MAV_RESULT_UNSUPPORTED;
     }
@@ -485,6 +507,17 @@ void AP_Mount::send_gimbal_manager_information(mavlink_channel_t chan)
     }
 }
 
+// send a GIMBAL_MANAGER_STATUS message to GCS
+void AP_Mount::send_gimbal_manager_status(mavlink_channel_t chan)
+{
+    // 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_gimbal_manager_status(chan);
+        }
+    }
+}
+
 // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
 // returns true on success
 bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)

libraries/AP_Mount/AP_Mount.h

@@ -159,6 +159,9 @@ class AP_Mount
     // send a GIMBAL_MANAGER_INFORMATION message to GCS
     void send_gimbal_manager_information(mavlink_channel_t chan);
 
+    // send a GIMBAL_MANAGER_STATUS message to GCS
+    void send_gimbal_manager_status(mavlink_channel_t chan);
+
     // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
     // returns true on success
     bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);
@@ -218,6 +221,7 @@ class AP_Mount
     MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
     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);
+    MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet);
     void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg);
     void handle_global_position_int(const mavlink_message_t &msg);
     void handle_gimbal_device_information(const mavlink_message_t &msg);

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -171,6 +171,91 @@ void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan)
                                                 radians(_params.yaw_angle_max));        // yaw_max in radians
 }
 
+// send a GIMBAL_MANAGER_STATUS message to GCS
+void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan)
+{
+    mavlink_msg_gimbal_manager_status_send(chan,
+                                           AP_HAL::millis(),                       // autopilot system time
+                                           get_gimbal_manager_capability_flags(),  // bitmap of gimbal manager capability flags
+                                           _instance,                              // gimbal device id
+                                           _gimbal_manager_primary_control_sysid,  // gimbal manager primary control sysid
+                                           _gimbal_manager_primary_control_compid, // gimbal manager primary control compid
+                                           0,                                      // gimbal manager secondary control sysid ( unsupported for the moment )
+                                           0);                                     // gimbal manager secondary control compid ( unsupported for the moment )
+}
+
+// handle gimbal_manager_configure command.  Returns MAV_RESULT_ACCEPTED on success
+MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet)
+{
+    // For the moment we only take into account primary control, ignoring secondary control.
+    // If a particular sysid and compid is specified, grant control to it
+    if (packet.param1 > 0 && packet.param2 > 0) {
+        _gimbal_manager_primary_control_sysid = packet.param1;
+        _gimbal_manager_primary_control_compid = packet.param2;
+        return MAV_RESULT_ACCEPTED;
+    }
+
+    // if 0 or less, we have magic numbers to manipulate control as follows:
+    //      0: no one in control
+    //      -1: leave unchanged, 
+    //      -2: set itself in control (for missions where the own sysid is still unknown), 
+    //      -3: remove control if currently in control
+
+    // At this point, both param1 and param2 should be equal, it doesn't make sense otherwise
+    if (!is_equal(packet.param1,packet.param2)) {
+        return MAV_RESULT_FAILED;
+    }
+
+    int8_t control_action = packet.param1;
+
+    switch(control_action) {
+        case 0:
+            _gimbal_manager_primary_control_sysid = 0;
+            _gimbal_manager_primary_control_compid = 0;
+            return MAV_RESULT_ACCEPTED;
+        case -1:
+            return MAV_RESULT_ACCEPTED;
+        case -2:
+            // Unsuported, we should get the sysid and compid from the packet and set it to control, but we can not retrieve it on the current architecture
+        case -3:
+            // Unsuported, we should get the sysid and compid from the packet and check if it is in control, but we can not retrieve it on the current architecture
+        default:        
+            return MAV_RESULT_FAILED;            
+    }
+}
+
+bool AP_Mount_Backend::gimbal_manager_have_control() const
+{
+    return _gimbal_manager_primary_control_sysid == mavlink_system.sysid &&
+           _gimbal_manager_primary_control_compid == mavlink_system.compid; 
+}
+
+void AP_Mount_Backend::gimbal_manager_set_control(uint8_t sysid, uint8_t compid)
+{
+    // If no sysid and compid is specified we set control to this autopilot
+    if (!sysid && !compid) {
+        _gimbal_manager_primary_control_sysid = mavlink_system.sysid;
+        _gimbal_manager_primary_control_compid = mavlink_system.compid;
+
+    // Otherwise we set the sysid and compid specified
+    } else {
+        _gimbal_manager_primary_control_sysid = sysid;
+        _gimbal_manager_primary_control_compid = compid;
+    }
+
+    // send gimbal_manager_status so the rest of the system knows who is in control
+    const mavlink_gimbal_manager_status_t packet{
+        AP_HAL::millis(),
+        get_gimbal_manager_capability_flags(),
+        _instance,
+        _gimbal_manager_primary_control_sysid,
+        _gimbal_manager_primary_control_compid,
+        0,
+        0
+    };
+    gcs().send_to_active_channels(MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (const char *)&packet);
+}
+
 // process MOUNT_CONTROL messages received from GCS. deprecated.
 void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet)
 {

libraries/AP_Mount/AP_Mount_Backend.h

@@ -102,6 +102,18 @@ class AP_Mount_Backend
     // send a GIMBAL_MANAGER_INFORMATION message to GCS
     void send_gimbal_manager_information(mavlink_channel_t chan);
 
+    // send a GIMBAL_MANAGER_STATUS message to GCS
+    void send_gimbal_manager_status(mavlink_channel_t chan);
+
+    // handle GIMBAL_MANAGER_CONFIGURE command
+    MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_long_t &packet);
+
+    // true if this autopilot is in control of gimbal manager
+    bool gimbal_manager_have_control() const;
+
+    // take control of gimbal manager by this autopilot
+    void gimbal_manager_set_control(uint8_t sysid = -1, uint8_t compid = -1);
+
     // handle a GIMBAL_REPORT message
     virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}
 
@@ -229,6 +241,10 @@ class AP_Mount_Backend
     bool _target_sysid_location_set;// true if _target_sysid has been set
 
     uint32_t _last_warning_ms;      // system time of last warning sent to GCS
+
+    // Gimbal manager status fields
+    uint8_t _gimbal_manager_primary_control_sysid = 0;
+    uint8_t _gimbal_manager_primary_control_compid = 0;
 };
 
 #endif // HAL_MOUNT_ENABLED

libraries/GCS_MAVLink/GCS.h

@@ -372,6 +372,7 @@ class GCS_MAVLINK
     void send_vibration() const;
     void send_gimbal_device_attitude_status() const;
     void send_gimbal_manager_information() const;
+    void send_gimbal_manager_status() const;
     void send_named_float(const char *name, float value) const;
     void send_home_position() const;
     void send_gps_global_origin() const;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -987,9 +987,10 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
 #endif
         { MAVLINK_MSG_ID_CAMERA_FEEDBACK,       MSG_CAMERA_FEEDBACK},
 #if HAL_MOUNT_ENABLED
-        { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
+        { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS,     MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
         { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE},
-        { MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_INFORMATION},
+        { MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION,        MSG_GIMBAL_MANAGER_INFORMATION},
+        { MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS,             MSG_GIMBAL_MANAGER_STATUS},
 #endif
 #if AP_OPTICALFLOW_ENABLED
         { MAVLINK_MSG_ID_OPTICAL_FLOW,          MSG_OPTICAL_FLOW},
@@ -5344,6 +5345,17 @@ void GCS_MAVLINK::send_gimbal_manager_information() const
 }
 #endif
 
+#if HAL_MOUNT_ENABLED
+void GCS_MAVLINK::send_gimbal_manager_status() const
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    mount->send_gimbal_manager_status(chan);
+}
+#endif
+
 void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc)
 {
 
@@ -5672,6 +5684,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 #if HAL_MOUNT_ENABLED
         CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION);
         send_gimbal_manager_information();
+#endif
+        break;
+    case MSG_GIMBAL_MANAGER_STATUS:
+#if HAL_MOUNT_ENABLED
+        CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_STATUS);
+        send_gimbal_manager_status();
 #endif
         break;
     case MSG_OPTICAL_FLOW:

libraries/GCS_MAVLink/ap_message.h

@@ -51,6 +51,7 @@ enum ap_message : uint8_t {
     MSG_CAMERA_FEEDBACK,
     MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
     MSG_GIMBAL_MANAGER_INFORMATION,
+    MSG_GIMBAL_MANAGER_STATUS,
     MSG_OPTICAL_FLOW,
     MSG_MAG_CAL_PROGRESS,
     MSG_MAG_CAL_REPORT,