Add support for multi-GCS operation

ArduCopter/GCS_MAVLink_Copter.cpp

@@ -317,10 +317,22 @@ uint8_t GCS_MAVLINK_Copter::sysid_my_gcs() const
 {
     return copter.g.sysid_my_gcs;
 }
+void GCS_MAVLINK_Copter::set_sysid_my_gcs(uint8_t sysid) const
+{
+    copter.g.sysid_my_gcs.set_and_save(sysid);
+}
 bool GCS_MAVLINK_Copter::sysid_enforce() const
 {
     return copter.g2.sysid_enforce;
 }
+bool GCS_MAVLINK_Copter::control_takeover_allowed() const
+{
+    return copter.g2.control_takeover_allowed;
+}
+void GCS_MAVLINK_Copter::set_control_takeover_allowed(bool takeoverAllowed) const
+{
+    return copter.g2.control_takeover_allowed.set_and_save(takeoverAllowed);
+}
 
 uint32_t GCS_MAVLINK_Copter::telem_delay() const
 {

ArduCopter/GCS_MAVLink_Copter.h

@@ -22,7 +22,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
     MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet) override;
 
     uint8_t sysid_my_gcs() const override;
+    void set_sysid_my_gcs(uint8_t sysid) const override;
     bool sysid_enforce() const override;
+    bool control_takeover_allowed() const override;
+    void set_control_takeover_allowed(bool takeoverAllowed) const override;
 
     bool params_ready() const override;
     void send_banner() override;

ArduCopter/Parameters.cpp

@@ -815,6 +815,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0),
 
+    // @Param: ALLOW_TAKEOVER
+    // @DisplayName: Allow takeover
+    // @Description: This controls if a GCS requesting control will be granted control automatically or the current GCS in control will need to be asked first
+    // @Values: 0:NotAllowed,1:Allowed
+    // @User: Advanced
+    AP_GROUPINFO("ALLOW_TAKEOVER", 12, ParametersG2, control_takeover_allowed, 1),
+
     // 12 was AP_Stats
 
     // 13 was AP_Gripper

ArduCopter/Parameters.h

@@ -536,6 +536,8 @@ class ParametersG2 {
     // whether to enforce acceptance of packets only from sysid_my_gcs
     AP_Int8 sysid_enforce;
 
+    AP_Int8 control_takeover_allowed;
+
 #if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
     // advanced failsafe library
     AP_AdvancedFailsafe_Copter afs;

ArduCopter/events.cpp

@@ -162,6 +162,9 @@ void Copter::failsafe_gcs_check()
 // failsafe_gcs_on_event - actions to take when GCS contact is lost
 void Copter::failsafe_gcs_on_event(void)
 {
+    // Set gcs allow_override true here, so other gcs can take control automatically, vehicle no longer has active controlling GCS
+    copter.g2.control_takeover_allowed.set_and_save(true);
+
     LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
     RC_Channels::clear_overrides();
 

libraries/GCS_MAVLink/GCS.h

@@ -271,7 +271,10 @@ class GCS_MAVLINK
     uint16_t cap_message_interval(uint16_t interval_ms) const;
 
     virtual uint8_t sysid_my_gcs() const = 0;
+    virtual void set_sysid_my_gcs(uint8_t sysid) const = 0;
     virtual bool sysid_enforce() const { return false; }
+    virtual bool control_takeover_allowed() const = 0;
+    virtual void set_control_takeover_allowed(bool takeoverAllowed) const = 0;
 
     // NOTE: param_name here must point to a 16+1 byte buffer - so do
     // NOT try to pass in a static-char-* unless it does have that
@@ -618,6 +621,8 @@ class GCS_MAVLINK
     bool get_ap_message_interval(ap_message id, uint16_t &interval_ms) const;
     MAV_RESULT handle_command_request_message(const mavlink_command_int_t &packet);
 
+    MAV_RESULT handle_request_operator_control(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
+
     MAV_RESULT handle_START_RX_PAIR(const mavlink_command_int_t &packet);
 
     virtual MAV_RESULT handle_flight_termination(const mavlink_command_int_t &packet);
@@ -806,10 +811,14 @@ class GCS_MAVLINK
     MAV_RESULT handle_servorelay_message(const mavlink_command_int_t &packet);
     bool send_relay_status() const;
 
+    void send_control_status() const;
+
     static bool command_long_stores_location(const MAV_CMD command);
 
     bool calibrate_gyros();
 
+    bool is_control_request_packet(const mavlink_message_t &msg) const;
+
     /// The stream we are communicating over
     AP_HAL::UARTDriver *_port;
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1151,6 +1151,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
 #endif
         { MAVLINK_MSG_ID_AVAILABLE_MODES, MSG_AVAILABLE_MODES},
         { MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, MSG_AVAILABLE_MODES_MONITOR},
+        { MAVLINK_MSG_ID_CONTROL_STATUS, MSG_CONTROL_STATUS},
             };
 
     for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@@ -1515,6 +1516,12 @@ void GCS_MAVLINK::update_send()
 #if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
         initialise_message_intervals_from_config_files();
 #endif
+        // We evaluate SYSID_ENFORCE parameter to send or not CONTROL_STATUS. This message is what
+        // populates GCS UI for multi GCS control
+        if (sysid_enforce()) {
+            set_mavlink_message_id_interval(MAVLINK_MSG_ID_CONTROL_STATUS, 5000);
+        }
+
         deferred_messages_initialised = true;
     }
 
@@ -3204,6 +3211,79 @@ MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int
     return MAV_RESULT_ACCEPTED;
 }
 
+MAV_RESULT GCS_MAVLINK::handle_request_operator_control(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
+{
+    // Param2 is 0: Release control, 1: Request control.
+    bool isRequestControl;
+    if (is_equal(packet.param2, 1.0f)) {
+        isRequestControl = true;
+    } else if (is_zero(packet.param2)) {
+        isRequestControl = false;
+    } else {
+        // Malformed packet, return denied
+        return MAV_RESULT_DENIED;
+    }
+
+    // Param3 is 0: Do not allow takeover, 1: Allow takeover.
+    bool setTakeoverAllowed;
+    if (is_equal(packet.param3, 1.0f)) {
+        setTakeoverAllowed = true;
+    } else if (is_zero(packet.param3)) {
+        setTakeoverAllowed = false;
+    } else {
+        // Malformed packet, return denied
+        return MAV_RESULT_DENIED;
+    }
+
+    if (isRequestControl) {
+        // This part would proceed the same if takeover is allowed, or if this is coming from the current GCS in control, 
+        // for example to modify takover allowed
+        if (control_takeover_allowed() || msg.sysid == sysid_my_gcs()) {
+            // Set takeover allowed, so other GCS can get control automatically or they need to ask first to current GCS in control
+            set_control_takeover_allowed(setTakeoverAllowed);
+            // Set current GCS in control if this is coming from a different GCS
+            if (msg.sysid != sysid_my_gcs()) {
+                set_sysid_my_gcs(msg.sysid);
+            }
+            // And send control status inmediatly to notify all GCS
+            send_control_status();
+            return MAV_RESULT_ACCEPTED;
+        // Else send a command to the current GCS in control, so current active operator can be prompted to allow takeover for the GCS asking
+        } else {
+            mavlink_msg_command_long_send(
+                chan,
+                sysid_my_gcs(),
+                0, // Should we retrieve here GCS compid and use it instead?
+                MAV_CMD_REQUEST_OPERATOR_CONTROL,
+                0,
+                msg.sysid, // Param1: Sysid of the GCS requesting control
+                1, // Param2: Release/request control. If we are here this should always be 1 (request). 0 would not make sense anyway 
+                packet.param3, // Param3: Allow takeover, this way the GCS in control can prompt the operator with the specific type of control request 
+                packet.param4, // Param4: Timeout in seconds before a request to a GCS to allow takeover is assumed to be rejected. This is used to display the timeout graphically on requestor and GCS in control.
+                0, 0, 0);
+            // We should answer result failed, see MAV_CMD_REQUEST_OPERATOR_CONTROL documentation for more information
+            return MAV_RESULT_FAILED;
+        }
+    // Release control
+    } else {
+        // Sanity check, double check that this is the GCS in control
+        if (msg.sysid == sysid_my_gcs()) {
+            // Set takeover allowed, so other GCS can get control automatically or they need to ask first to current GCS in control
+            set_control_takeover_allowed(true);
+            // As per Mavlink protocol, see MAV_CMD_REQUEST_OPERATOR_CONTROL, we should set sysid_my_gcs to 0, so CONTROL_STATUS emits 0
+            // (nobody in control), so other GCS can display properly lthe current control status of the vehicle. Should we be compliant here
+            // and set sysid_my_gcs to 0?
+            // set_sysid_my_gcs(0)
+            // Finally send a CONTROL_STATUS inmediately to notify the GCSs on the system
+            send_control_status();
+            return MAV_RESULT_ACCEPTED;
+        // A release control command should always be sent from the GCS in control. Return failed otherwise
+        } else {
+            return MAV_RESULT_FAILED;
+        }
+    }
+}
+
 bool GCS_MAVLINK::get_ap_message_interval(ap_message id, uint16_t &interval_ms) const
 {
     // check if it's a specially-handled message:
@@ -5610,6 +5690,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
     case MAV_CMD_REQUEST_MESSAGE:
         return handle_command_request_message(packet);
 
+    case MAV_CMD_REQUEST_OPERATOR_CONTROL:
+        return handle_request_operator_control(packet, msg);
     }
 
     return MAV_RESULT_UNSUPPORTED;
@@ -5961,6 +6043,18 @@ bool GCS_MAVLINK::send_relay_status() const
 }
 #endif  // AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
 
+void GCS_MAVLINK::send_control_status() const
+{
+    uint8_t flags = GCS_CONTROL_STATUS_FLAGS_SYSTEM_MANAGER;
+    flags |= control_takeover_allowed() ? GCS_CONTROL_STATUS_FLAGS_TAKEOVER_ALLOWED : 0;
+    mavlink_msg_control_status_send(
+        chan,
+        // System ID of the system currently in control of this MAV
+        sysid_my_gcs(),
+        // Flags: Automatic takeover allowed and this system controlling the rest of components of this system ID ( always true for an autopilot )
+        flags);
+}
+
 void GCS_MAVLINK::send_autopilot_state_for_gimbal_device() const
 {
 #if AP_AHRS_ENABLED
@@ -6490,6 +6584,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         ret = send_relay_status();
         break;
 #endif
+    case MSG_CONTROL_STATUS:
+        send_control_status();
+        break;
 
     case MSG_AVAILABLE_MODES:
         ret = send_available_modes();
@@ -6823,9 +6920,35 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status,
         return true;
     }
 
+    // Allow REQUEST_OPERATOR_CONTROL packets. This is needed for a GCS not in control
+    // to communicate with the vehicle, so it can forward the request to GCS in control.
+    if (is_control_request_packet(msg)) {
+        return true;
+    } 
+
     return false;
 }
 
+bool GCS_MAVLINK::is_control_request_packet(const mavlink_message_t &msg) const
+{
+    bool result = false;
+    if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
+        mavlink_command_long_t command;
+        mavlink_msg_command_long_decode(&msg, &command);
+        if (command.command == MAV_CMD_REQUEST_OPERATOR_CONTROL) {
+            result = true;
+        }
+    } 
+    if (msg.msgid == MAVLINK_MSG_ID_COMMAND_INT) {
+        mavlink_command_int_t command;
+        mavlink_msg_command_int_decode(&msg, &command);  
+        if (command.command == MAV_CMD_REQUEST_OPERATOR_CONTROL) {
+            result = true;
+        }        
+    }
+    return result;
+}
+
 /*
   update UART pass-thru, if enabled
  */

libraries/GCS_MAVLink/GCS_Dummy.h

@@ -26,6 +26,9 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK
     uint32_t telem_delay() const override { return 0; }
     bool try_send_message(enum ap_message id) override { return true; }
     uint8_t sysid_my_gcs() const override { return 1; }
+    void set_sysid_my_gcs(uint8_t sysid) const override {}
+    bool control_takeover_allowed() const override { return true; }
+    void set_control_takeover_allowed(bool takeoverAllowed) const override {}
 
 protected:
 

libraries/GCS_MAVLink/ap_message.h

@@ -105,5 +105,6 @@ enum ap_message : uint8_t {
     MSG_AIRSPEED,
     MSG_AVAILABLE_MODES,
     MSG_AVAILABLE_MODES_MONITOR,
+    MSG_CONTROL_STATUS,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };