Merge pull request #1886 from DonLakeFlyer/ArmDisarm

Change arm/disarm mavlink message usage

src/comm/MockLink.cc

@@ -346,6 +346,10 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
             case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
                 _handleFTP(msg);
                 break;
+                
+            case MAVLINK_MSG_ID_COMMAND_LONG:
+                _handleCommandLong(msg);
+                break;
 
             default:
                 qDebug() << "MockLink: Unhandled mavlink message, id:" << msg.msgid;
@@ -724,3 +728,19 @@ void MockLink::_handleFTP(const mavlink_message_t& msg)
     Q_ASSERT(_fileServer);
     _fileServer->handleFTPMessage(msg);
 }
+
+void MockLink::_handleCommandLong(const mavlink_message_t& msg)
+{
+    mavlink_command_long_t request;
+    
+    mavlink_msg_command_long_decode(&msg, &request);
+
+    if (request.command == MAV_CMD_COMPONENT_ARM_DISARM) {
+        if (request.param1 == 0.0f) {
+            _mavBaseMode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+        } else {
+            _mavBaseMode |= MAV_MODE_FLAG_SAFETY_ARMED;
+        }
+    }
+}
+

src/comm/MockLink.h

@@ -124,6 +124,7 @@ private:
     void _handleMissionRequest(const mavlink_message_t& msg);
     void _handleMissionItem(const mavlink_message_t& msg);
     void _handleFTP(const mavlink_message_t& msg);
+    void _handleCommandLong(const mavlink_message_t& msg);
     float _floatUnionForParam(int componentId, const QString& paramName);
     void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
 

src/uas/UAS.cc

@@ -2446,7 +2446,10 @@ void UAS::launch()
  */
 void UAS::armSystem()
 {
-    setModeArm(base_mode | MAV_MODE_FLAG_SAFETY_ARMED, custom_mode);
+    // We specifically do not use setModeArm to change arming state. The APM flight stack does not support
+    // arm/disarm through the SET_MODE mavlink message. Instead we use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM
+    // which works on both PX4 and APM flight stack.
+    executeCommand(MAV_CMD_COMPONENT_ARM_DISARM, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
 }
 
 /**
@@ -2455,12 +2458,19 @@ void UAS::armSystem()
  */
 void UAS::disarmSystem()
 {
-    setModeArm(base_mode & ~(MAV_MODE_FLAG_SAFETY_ARMED), custom_mode);
+    // We specifically do not use setModeArm to change arming state. The APM flight stack does not support
+    // arm/disarm through the SET_MODE mavlink message. Instead we use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM
+    // which works on both PX4 and APM flight stack.
+    executeCommand(MAV_CMD_COMPONENT_ARM_DISARM, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
 }
 
 void UAS::toggleArmedState()
 {
-    setModeArm(base_mode ^ (MAV_MODE_FLAG_SAFETY_ARMED), custom_mode);
+    if (isArmed()) {
+        disarmSystem();
+    } else {
+        armSystem();
+    }
 }
 
 void UAS::goAutonomous()