From c64457ec1897c794c79e001a52f5f597dfddbc40 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Fri, 25 Sep 2015 19:33:16 -0700
Subject: [PATCH] Add COMMAND_LONG arm/disarm support
---
src/comm/MockLink.cc | 20 ++++++++++++++++++++
src/comm/MockLink.h | 1 +
2 files changed, 21 insertions(+)
diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index ed7494e0da..ac4c069f8a 100644
--- a/src/comm/MockLink.cc
+++ b/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;
+ }
+ }
+}
+
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index f55a2e2164..7ca8f179d7 100644
--- a/src/comm/MockLink.h
+++ b/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);
--
GitLab