provisional commit gimbal_manager_configure

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;
     }

libraries/AP_Mount/AP_Mount.h

@@ -221,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

@@ -184,6 +184,78 @@ void AP_Mount_Backend::send_gimbal_manager_status(mavlink_channel_t chan)
                                            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

@@ -105,6 +105,15 @@ class AP_Mount_Backend
     // 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) {}