diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index 0c0b708..4d7307e 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -662,12 +662,16 @@ SOURCES += \
DebugBuild|WindowsDebugAndRelease {
+DEFINES += UNITTEST_BUILD
+
INCLUDEPATH += \
src/qgcunittest
HEADERS += \
src/qgcunittest/AutoTest.h \
src/qgcunittest/UASUnitTest.h \
+ src/qgcunittest/MockLink.h \
+ src/qgcunittest/MockLinkMissionItemHandler.h \
src/qgcunittest/MockUASManager.h \
src/qgcunittest/MockUAS.h \
src/qgcunittest/MockQGCUASParamManager.h \
@@ -682,6 +686,8 @@ HEADERS += \
SOURCES += \
src/qgcunittest/UASUnitTest.cc \
+ src/qgcunittest/MockLink.cc \
+ src/qgcunittest/MockLinkMissionItemHandler.cc \
src/qgcunittest/MockUASManager.cc \
src/qgcunittest/MockUAS.cc \
src/qgcunittest/MockQGCUASParamManager.cc \
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index e93171f..68cf6a0 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -228,4 +228,7 @@
files/styles/Vera.ttf
+
+ src/qgcunittest/MockLink.param
+
diff --git a/src/qgcunittest/MockLink.cc b/src/qgcunittest/MockLink.cc
new file mode 100644
index 0000000..d8721dc
--- /dev/null
+++ b/src/qgcunittest/MockLink.cc
@@ -0,0 +1,516 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see .
+
+ ======================================================================*/
+
+#include "MockLink.h"
+
+#include
+#include
+#include
+
+#include "LinkManager.h"
+
+#include
+
+/// @file
+/// @brief Mock implementation of a Link.
+///
+/// @author Don Gagne
+
+MockLink::MockLink(void) :
+ _linkId(getNextLinkId()),
+ _name("MockLink"),
+ _connected(false),
+ _vehicleSystemId(128), // FIXME: Pull from eventual parameter manager
+ _vehicleComponentId(200), // FIXME: magic number?
+ _inNSH(false),
+ _mavlinkStarted(false),
+ _mavMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
+ _mavState(MAV_STATE_STANDBY)
+{
+ _missionItemHandler = new MockLinkMissionItemHandler(_vehicleSystemId, this);
+ Q_CHECK_PTR(_missionItemHandler);
+
+ moveToThread(this);
+ _loadParams();
+ QObject::connect(this, &MockLink::_incomingBytes, this, &MockLink::_handleIncomingBytes);
+ LinkManager::instance()->add(this);
+}
+
+MockLink::~MockLink(void)
+{
+ disconnect();
+ deleteLater();
+}
+
+void MockLink::readBytes(void)
+{
+ // FIXME: This is a bad virtual from LinkInterface?
+}
+
+bool MockLink::connect(void)
+{
+ _connected = true;
+
+ start();
+
+ emit connected();
+ emit connected(true);
+
+ return true;
+}
+
+bool MockLink::disconnect(void)
+{
+ _connected = false;
+ exit();
+
+ emit disconnected();
+ emit connected(false);
+
+ return true;
+}
+
+void MockLink::run(void)
+{
+ QTimer _timer1HzTasks;
+ QTimer _timer10HzTasks;
+ QTimer _timer50HzTasks;
+
+ QObject::connect(&_timer1HzTasks, &QTimer::timeout, this, &MockLink::_run1HzTasks);
+ QObject::connect(&_timer10HzTasks, &QTimer::timeout, this, &MockLink::_run10HzTasks);
+ QObject::connect(&_timer50HzTasks, &QTimer::timeout, this, &MockLink::_run50HzTasks);
+
+ _timer1HzTasks.start(1000);
+ _timer10HzTasks.start(100);
+ _timer50HzTasks.start(20);
+
+ exec();
+
+ emit disconnected();
+ emit connected(false);
+}
+
+void MockLink::_run1HzTasks(void)
+{
+ if (_mavlinkStarted) {
+ _sendHeartBeat();
+ }
+}
+
+void MockLink::_run10HzTasks(void)
+{
+ if (_mavlinkStarted) {
+ }
+}
+
+void MockLink::_run50HzTasks(void)
+{
+ if (_mavlinkStarted) {
+ }
+}
+
+void MockLink::_loadParams(void)
+{
+ QFile paramFile(":/unittest/MockLink.param");
+
+ bool success = paramFile.open(QFile::ReadOnly);
+ Q_UNUSED(success);
+ Q_ASSERT(success);
+
+ QTextStream paramStream(¶mFile);
+
+ while (!paramStream.atEnd()) {
+ QString line = paramStream.readLine();
+
+ if (line.startsWith("#")) {
+ continue;
+ }
+
+ QStringList paramData = line.split("\t");
+ Q_ASSERT(paramData.count() == 5);
+
+ QString paramName = paramData.at(2);
+ QString valStr = paramData.at(3);
+ uint paramType = paramData.at(4).toUInt();
+
+ QVariant paramValue;
+ switch (paramType) {
+ case MAV_PARAM_TYPE_REAL32:
+ paramValue = QVariant(valStr.toFloat());
+ break;
+ case MAV_PARAM_TYPE_UINT32:
+ paramValue = QVariant(valStr.toUInt());
+ break;
+ case MAV_PARAM_TYPE_INT32:
+ paramValue = QVariant(valStr.toInt());
+ break;
+ case MAV_PARAM_TYPE_INT8:
+ paramValue = QVariant((unsigned char)valStr.toUInt());
+ break;
+ default:
+ Q_ASSERT(false);
+ break;
+ }
+
+ _parameters[paramName] = paramValue;
+ }
+ _cParameters = _parameters.count();
+}
+
+void MockLink::_sendHeartBeat(void)
+{
+ mavlink_message_t msg;
+ uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
+
+ mavlink_msg_heartbeat_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &msg,
+ MAV_TYPE_QUADROTOR, // MAV_TYPE
+ MAV_AUTOPILOT_PX4, // MAV_AUTOPILOT
+ _mavMode, // MAV_MODE
+ 0, // custom mode
+ _mavState); // MAV_STATE
+
+ int cBuffer = mavlink_msg_to_send_buffer(buffer, &msg);
+ QByteArray bytes((char *)buffer, cBuffer);
+ emit bytesReceived(this, bytes);
+}
+
+/// @brief Called when QGC wants to write bytes to the MAV
+void MockLink::writeBytes(const char* bytes, qint64 cBytes)
+{
+ // Package up the data so we can signal it over to the right thread
+ QByteArray byteArray(bytes, cBytes);
+
+ emit _incomingBytes(byteArray);
+}
+
+/// @brief Handles bytes from QGC on the thread
+void MockLink::_handleIncomingBytes(const QByteArray bytes)
+{
+ if (_inNSH) {
+ _handleIncomingNSHBytes(bytes.constData(), bytes.count());
+ } else {
+ if (bytes.startsWith(QByteArray("\r\r\r"))) {
+ _inNSH = true;
+ _handleIncomingNSHBytes(&bytes.constData()[3], bytes.count() - 3);
+ }
+
+ _handleIncomingMavlinkBytes((uint8_t *)bytes.constData(), bytes.count());
+ }
+}
+
+/// @brief Handle incoming bytes which are meant to be interpreted by the NuttX shell
+void MockLink::_handleIncomingNSHBytes(const char* bytes, int cBytes)
+{
+ Q_UNUSED(cBytes);
+
+ // Drop back out of NSH
+ if (cBytes == 4 && bytes[0] == '\r' && bytes[1] == '\r' && bytes[2] == '\r') {
+ _inNSH = false;
+ return;
+ }
+
+ if (cBytes > 0) {
+ qDebug() << "NSH:" << (const char*)bytes;
+
+ if (strncmp(bytes, "sh /etc/init.d/rc.usb\n", cBytes) == 0) {
+ // This is the mavlink start command
+ _mavlinkStarted = true;
+ }
+ }
+}
+
+/// @brief Handle incoming bytes which are meant to be handled by the mavlink protocol
+void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
+{
+ mavlink_message_t msg;
+ mavlink_status_t comm;
+
+ for (qint64 i=0; ihandleMessage(msg);
+
+ switch (msg.msgid) {
+ case MAVLINK_MSG_ID_HEARTBEAT:
+ _handleHeartBeat(msg);
+ break;
+
+ case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+ _handleParamRequestList(msg);
+ break;
+
+ case MAVLINK_MSG_ID_SET_MODE:
+ _handleSetMode(msg);
+ break;
+
+ case MAVLINK_MSG_ID_PARAM_SET:
+ _handleParamSet(msg);
+ break;
+
+ case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
+ _handleParamRequestRead(msg);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+ _handleMissionRequestList(msg);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST:
+ _handleMissionRequest(msg);
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ITEM:
+ _handleMissionItem(msg);
+ break;
+
+#if 0
+ case MAVLINK_MSG_ID_MISSION_COUNT:
+ _handleMissionCount(msg);
+ break;
+#endif
+
+ default:
+ qDebug() << "MockLink: Unhandled mavlink message, id:" << msg.msgid;
+ break;
+ }
+ }
+}
+
+void MockLink::_emitMavlinkMessage(const mavlink_message_t& msg)
+{
+ uint8_t outputBuffer[MAVLINK_MAX_PACKET_LEN];
+
+ int cBuffer = mavlink_msg_to_send_buffer(outputBuffer, &msg);
+ QByteArray bytes((char *)outputBuffer, cBuffer);
+ emit bytesReceived(this, bytes);
+}
+
+void MockLink::_handleHeartBeat(const mavlink_message_t& msg)
+{
+ Q_UNUSED(msg);
+#if 0
+ mavlink_heartbeat_t heartbeat;
+ mavlink_msg_heartbeat_decode(&msg, &heartbeat);
+#endif
+}
+
+void MockLink::_handleSetMode(const mavlink_message_t& msg)
+{
+ mavlink_set_mode_t request;
+ mavlink_msg_set_mode_decode(&msg, &request);
+
+ if (request.target_system == _vehicleSystemId) {
+ _mavMode = request.base_mode;
+ } else {
+ _errorInvalidTargetSystem(request.target_system);
+ }
+}
+
+void MockLink::_errorInvalidTargetSystem(int targetId)
+{
+ QString errMsg("MSG_ID_SET_MODE received incorrect target system: actual(%1) expected(%2)");
+ emit error(errMsg.arg(targetId).arg(_vehicleSystemId));
+}
+
+void MockLink::_handleParamRequestList(const mavlink_message_t& msg)
+{
+ mavlink_param_request_list_t request;
+
+ mavlink_msg_param_request_list_decode(&msg, &request);
+
+ uint16_t paramIndex = 0;
+ if (request.target_system == _vehicleSystemId) {
+ ParamMap_t::iterator param;
+
+ for (param = _parameters.begin(); param != _parameters.end(); param++) {
+ mavlink_message_t responseMsg;
+
+ mavlink_msg_param_value_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ param.key().toLocal8Bit().constData(), // Parameter name
+ param.value().toFloat(), // Parameter vluae
+ MAV_PARAM_TYPE_REAL32, // FIXME: Pull from QVariant type
+ _cParameters, // Total number of parameters
+ paramIndex++); // Index of this parameter
+ _emitMavlinkMessage(responseMsg);
+ }
+ } else {
+ _errorInvalidTargetSystem(request.target_system);
+ }
+}
+
+void MockLink::_handleParamSet(const mavlink_message_t& msg)
+{
+ mavlink_param_set_t request;
+ mavlink_msg_param_set_decode(&msg, &request);
+
+ if (request.target_system == _vehicleSystemId) {
+ // Param may not be null terminated if exactly fits
+ char paramId[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
+ strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN);
+
+ if (_parameters.contains(paramId))
+ {
+ _parameters[paramId] = request.param_value;
+
+ mavlink_message_t responseMsg;
+
+ mavlink_msg_param_value_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ paramId, // Parameter name
+ request.param_value, // Parameter vluae
+ MAV_PARAM_TYPE_REAL32, // FIXME: Pull from QVariant type
+ _cParameters, // Total number of parameters
+ _parameters.keys().indexOf(paramId)); // Index of this parameter
+ _emitMavlinkMessage(responseMsg);
+ } else {
+ QString errMsg("MSG_ID_PARAM_SET requested unknown param id (%1)");
+ emit error(errMsg.arg(paramId));
+ }
+ } else {
+ _errorInvalidTargetSystem(request.target_system);
+ }
+}
+
+void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
+{
+ mavlink_param_request_read_t request;
+ mavlink_msg_param_request_read_decode(&msg, &request);
+
+ char paramId[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
+ paramId[0] = 0;
+
+ if (request.target_system == _vehicleSystemId) {
+ if (request.param_index == -1) {
+ // Request is by param name. Param may not be null terminated if exactly fits
+ strncpy(paramId, request.param_id, MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
+ } else {
+ if (request.param_index >= 0 && request.param_index < _cParameters) {
+ // Request is by index
+ QString key = _parameters.keys().at(request.param_index);
+ Q_ASSERT(key.length() <= MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN);
+ strcpy(paramId, key.toLocal8Bit().constData());
+ } else {
+ QString errMsg("MSG_ID_PARAM_REQUEST_READ requested unknown index: requested(%1) count(%2)");
+ emit error(errMsg.arg(request.param_index).arg(_cParameters));
+ }
+ }
+
+ if (paramId[0] && _parameters.contains(paramId)) {
+ float paramValue = _parameters[paramId].toFloat();
+
+ mavlink_message_t responseMsg;
+
+ mavlink_msg_param_value_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ paramId, // Parameter name
+ paramValue, // Parameter vluae
+ MAV_PARAM_TYPE_REAL32, // FIXME: Pull from QVariant type
+ _cParameters, // Total number of parameters
+ _parameters.keys().indexOf(paramId)); // Index of this parameter
+ _emitMavlinkMessage(responseMsg);
+ }
+ } else {
+ _errorInvalidTargetSystem(request.target_system);
+ }
+}
+
+void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
+{
+ mavlink_mission_request_list_t request;
+
+ mavlink_msg_mission_request_list_decode(&msg, &request);
+
+ if (request.target_system == _vehicleSystemId) {
+ mavlink_message_t responseMsg;
+
+ mavlink_msg_mission_count_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ msg.sysid, // Target is original sender
+ msg.compid, // Target is original sender
+ _missionItems.count()); // Number of mission items
+ _emitMavlinkMessage(responseMsg);
+ } else {
+ _errorInvalidTargetSystem(request.target_system);
+ }
+}
+
+void MockLink::_handleMissionRequest(const mavlink_message_t& msg)
+{
+ mavlink_mission_request_t request;
+
+ mavlink_msg_mission_request_decode(&msg, &request);
+
+ if (request.target_system == _vehicleSystemId) {
+ if (request.seq >= 0 && request.seq < _missionItems.count()) {
+ mavlink_message_t responseMsg;
+
+ mavlink_mission_item_t item = _missionItems[request.seq];
+
+ mavlink_msg_mission_item_pack(_vehicleSystemId,
+ _vehicleComponentId,
+ &responseMsg, // Outgoing message
+ msg.sysid, // Target is original sender
+ msg.compid, // Target is original sender
+ request.seq, // Index of mission item being sent
+ item.frame,
+ item.command,
+ item.current,
+ item.autocontinue,
+ item.param1, item.param2, item.param3, item.param4,
+ item.x, item.y, item.z);
+ _emitMavlinkMessage(responseMsg);
+ } else {
+ QString errMsg("MSG_ID_MISSION_REQUEST requested unknown sequence number: requested(%1) count(%2)");
+ emit error(errMsg.arg(request.seq).arg(_missionItems.count()));
+ }
+ } else {
+ _errorInvalidTargetSystem(request.target_system);
+ }
+}
+
+void MockLink::_handleMissionItem(const mavlink_message_t& msg)
+{
+ mavlink_mission_item_t request;
+
+ mavlink_msg_mission_item_decode(&msg, &request);
+
+ if (request.target_system == _vehicleSystemId) {
+ // FIXME: What do you do with duplication sequence numbers?
+ Q_ASSERT(!_missionItems.contains(request.seq));
+
+ _missionItems[request.seq] = request;
+ } else {
+ _errorInvalidTargetSystem(request.target_system);
+ }
+}
diff --git a/src/qgcunittest/MockLink.h b/src/qgcunittest/MockLink.h
new file mode 100644
index 0000000..0c8da26
--- /dev/null
+++ b/src/qgcunittest/MockLink.h
@@ -0,0 +1,118 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see .
+
+ ======================================================================*/
+
+#ifndef MOCKLINK_H
+#define MOCKLINK_H
+
+#include
+
+#include "MockLinkMissionItemHandler.h"
+#include "LinkInterface.h"
+#include "mavlink.h"
+
+/// @file
+/// @brief Mock implementation of a Link.
+///
+/// @author Don Gagne
+
+class MockLink : public LinkInterface
+{
+ Q_OBJECT
+
+public:
+ MockLink(void);
+ ~MockLink(void);
+
+ // Virtuals from LinkInterface
+ virtual int getId(void) const { return _linkId; }
+ virtual QString getName(void) const { return _name; }
+ virtual void requestReset(void){ }
+ virtual bool isConnected(void) const { return _connected; }
+ virtual qint64 getConnectionSpeed(void) const { return 100000000; }
+ virtual bool connect(void);
+ virtual bool disconnect(void);
+ virtual qint64 bytesAvailable(void) { return 0; }
+
+signals:
+ void error(const QString& errorMsg);
+
+ /// @brief Used internally to move data to the thread.
+ void _incomingBytes(const QByteArray bytes);
+
+public slots:
+ virtual void writeBytes(const char *bytes, qint64 cBytes);
+
+protected slots:
+ // FIXME: This should not be part of LinkInterface. It is an internal link implementation detail.
+ virtual void readBytes(void);
+
+private slots:
+ void _run1HzTasks(void);
+ void _run10HzTasks(void);
+ void _run50HzTasks(void);
+
+private:
+ // QThread override
+ virtual void run(void);
+
+ // MockLink methods
+ void _sendHeartBeat(void);
+ void _handleIncomingBytes(const QByteArray bytes);
+ void _handleIncomingNSHBytes(const char* bytes, int cBytes);
+ void _handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes);
+ void _loadParams(void);
+ void _errorInvalidTargetSystem(int targetId);
+ void _emitMavlinkMessage(const mavlink_message_t& msg);
+ void _handleHeartBeat(const mavlink_message_t& msg);
+ void _handleSetMode(const mavlink_message_t& msg);
+ void _handleParamRequestList(const mavlink_message_t& msg);
+ void _handleParamSet(const mavlink_message_t& msg);
+ void _handleParamRequestRead(const mavlink_message_t& msg);
+ void _handleMissionRequestList(const mavlink_message_t& msg);
+ void _handleMissionRequest(const mavlink_message_t& msg);
+ void _handleMissionItem(const mavlink_message_t& msg);
+
+ MockLinkMissionItemHandler* _missionItemHandler;
+
+ int _linkId;
+ QString _name;
+ bool _connected;
+
+ uint8_t _vehicleSystemId;
+ uint8_t _vehicleComponentId;
+
+ bool _inNSH;
+ bool _mavlinkStarted;
+
+ typedef QMap ParamMap_t;
+ ParamMap_t _parameters;
+ uint16_t _cParameters;
+
+ typedef QMap MissionList_t;
+ MissionList_t _missionItems;
+
+ uint8_t _mavMode;
+ uint8_t _mavState;
+};
+
+#endif
diff --git a/src/qgcunittest/MockLink.param b/src/qgcunittest/MockLink.param
new file mode 100644
index 0000000..920dda7
--- /dev/null
+++ b/src/qgcunittest/MockLink.param
@@ -0,0 +1,438 @@
+# Onboard parameters for system MAV 001
+#
+# MAV ID COMPONENT ID PARAM NAME VALUE (FLOAT)
+1 50 ATT_ACC_COMP 2 6
+1 50 ATT_MAG_DECL 0 9
+1 50 BAT_CAPACITY -1 9
+1 50 BAT_C_SCALING 0.0124 9
+1 50 BAT_N_CELLS 3 6
+1 50 BAT_V_CHARGED 4.2 9
+1 50 BAT_V_EMPTY 3.4 9
+1 50 BAT_V_LOAD_DROP 0.07 9
+1 50 BAT_V_SCALE_IO 10000 6
+1 50 BAT_V_SCALING 0.0082 9
+1 50 BD_GPROPERTIES 0.03 9
+1 50 BD_OBJ_CD 0.1 9
+1 50 BD_OBJ_MASS 0.6 9
+1 50 BD_OBJ_SURFACE 0.00311725 9
+1 50 BD_PRECISION 30 9
+1 50 BD_TURNRADIUS 120 9
+1 50 CBRK_AIRSPD_CHK 0 6
+1 50 CBRK_ENGINEFAIL 284953 6
+1 50 CBRK_FLIGHTTERM 121212 6
+1 50 CBRK_GPSFAIL 240024 6
+1 50 CBRK_IO_SAFETY 0 6
+1 50 CBRK_NO_VISION 0 6
+1 50 CBRK_RATE_CTRL 0 6
+1 50 CBRK_SUPPLY_CHK 0 6
+1 50 COM_DL_LOSS_EN 0 6
+1 50 COM_DL_LOSS_T 10 6
+1 50 COM_DL_REG_T 0 6
+1 50 COM_EF_C2T 5 9
+1 50 COM_EF_THROT 0.5 9
+1 50 COM_EF_TIME 10 9
+1 50 COM_RC_LOSS_T 0.5 9
+1 50 EKF_ATT_V3_Q0 0.0001 9
+1 50 EKF_ATT_V3_Q1 0.08 9
+1 50 EKF_ATT_V3_Q2 0.009 9
+1 50 EKF_ATT_V3_Q3 0.005 9
+1 50 EKF_ATT_V3_Q4 0 9
+1 50 EKF_ATT_V4_R0 0.0008 9
+1 50 EKF_ATT_V4_R1 10000 9
+1 50 EKF_ATT_V4_R2 100 9
+1 50 EKF_ATT_V4_R3 0 9
+1 50 FPE_DEBUG 0 6
+1 50 FPE_LO_THRUST 0.4 9
+1 50 FPE_SONAR_LP_L 0.2 9
+1 50 FPE_SONAR_LP_U 0.5 9
+1 50 FW_AIRSPD_MAX 50 9
+1 50 FW_AIRSPD_MIN 13 9
+1 50 FW_AIRSPD_TRIM 20 9
+1 50 FW_ATT_TC 0.5 9
+1 50 FW_CLMBOUT_DIFF 25 9
+1 50 FW_FLARE_PMAX 15 9
+1 50 FW_FLARE_PMIN 2.5 9
+1 50 FW_L1_DAMPING 0.75 9
+1 50 FW_L1_PERIOD 25 9
+1 50 FW_LND_ANG 5 9
+1 50 FW_LND_FLALT 8 9
+1 50 FW_LND_HHDIST 15 9
+1 50 FW_LND_HVIRT 10 9
+1 50 FW_LND_TLALT -1 9
+1 50 FW_LND_USETER 0 6
+1 50 FW_MAN_P_MAX 45 9
+1 50 FW_MAN_R_MAX 45 9
+1 50 FW_PR_FF 0.4 9
+1 50 FW_PR_I 0 9
+1 50 FW_PR_IMAX 0.2 9
+1 50 FW_PR_P 0.05 9
+1 50 FW_PSP_OFF 0 9
+1 50 FW_P_LIM_MAX 45 9
+1 50 FW_P_LIM_MIN -45 9
+1 50 FW_P_RMAX_NEG 0 9
+1 50 FW_P_RMAX_POS 0 9
+1 50 FW_P_ROLLFF 0 9
+1 50 FW_RR_FF 0.3 9
+1 50 FW_RR_I 0 9
+1 50 FW_RR_IMAX 0.2 9
+1 50 FW_RR_P 0.05 9
+1 50 FW_RSP_OFF 0 9
+1 50 FW_R_LIM 45 9
+1 50 FW_R_RMAX 0 9
+1 50 FW_THR_CRUISE 0.7 9
+1 50 FW_THR_LND_MAX 1 9
+1 50 FW_THR_MAX 1 9
+1 50 FW_THR_MIN 0 9
+1 50 FW_THR_SLEW_MAX 0 9
+1 50 FW_T_CLMB_MAX 5 9
+1 50 FW_T_HGT_OMEGA 3 9
+1 50 FW_T_HRATE_FF 0 9
+1 50 FW_T_HRATE_P 0.05 9
+1 50 FW_T_INTEG_GAIN 0.1 9
+1 50 FW_T_PTCH_DAMP 0 9
+1 50 FW_T_RLL2THR 10 9
+1 50 FW_T_SINK_MAX 5 9
+1 50 FW_T_SINK_MIN 2 9
+1 50 FW_T_SPDWEIGHT 1 9
+1 50 FW_T_SPD_OMEGA 2 9
+1 50 FW_T_SRATE_P 0.05 9
+1 50 FW_T_THRO_CONST 8 9
+1 50 FW_T_THR_DAMP 0.5 9
+1 50 FW_T_TIME_CONST 5 9
+1 50 FW_T_VERT_ACC 7 9
+1 50 FW_YCO_VMIN 1000 9
+1 50 FW_YR_FF 0.3 9
+1 50 FW_YR_I 0 9
+1 50 FW_YR_IMAX 0.2 9
+1 50 FW_YR_P 0.05 9
+1 50 FW_Y_RMAX 0 9
+1 50 GF_ALTMODE 0 6
+1 50 GF_COUNT -1 6
+1 50 GF_ON 1 6
+1 50 GF_SOURCE 0 6
+1 50 INAV_DELAY_GPS 0.2 9
+1 50 INAV_FLOW_K 0.15 9
+1 50 INAV_FLOW_Q_MIN 0.5 9
+1 50 INAV_LAND_DISP 0.7 9
+1 50 INAV_LAND_T 3 9
+1 50 INAV_LAND_THR 0.2 9
+1 50 INAV_SONAR_ERR 0.5 9
+1 50 INAV_SONAR_FILT 0.05 9
+1 50 INAV_W_ACC_BIAS 0.05 9
+1 50 INAV_W_GPS_FLOW 0.1 9
+1 50 INAV_W_XY_FLOW 5 9
+1 50 INAV_W_XY_GPS_P 1 9
+1 50 INAV_W_XY_GPS_V 2 9
+1 50 INAV_W_XY_RES_V 0.5 9
+1 50 INAV_W_XY_VIS_P 5 9
+1 50 INAV_W_XY_VIS_V 0 9
+1 50 INAV_W_Z_BARO 0.5 9
+1 50 INAV_W_Z_GPS_P 0.005 9
+1 50 INAV_W_Z_SONAR 3 9
+1 50 INAV_W_Z_VIS_P 0.5 9
+1 50 LAUN_ALL_ON 0 6
+1 50 LAUN_CAT_A 30 9
+1 50 LAUN_CAT_MDEL 0 9
+1 50 LAUN_CAT_PMAX 30 9
+1 50 LAUN_CAT_T 0.05 9
+1 50 LAUN_THR_PRE 0 9
+1 50 MAV_COMP_ID 50 6
+1 50 MAV_FWDEXTSP 1 6
+1 50 MAV_SYS_ID 128 6
+1 50 MAV_TYPE 2 6
+1 50 MAV_USEHILGPS 0 6
+1 50 MC_ACRO_P_MAX 90 9
+1 50 MC_ACRO_R_MAX 90 9
+1 50 MC_ACRO_Y_MAX 120 9
+1 50 MC_MAN_P_MAX 35 9
+1 50 MC_MAN_R_MAX 35 9
+1 50 MC_MAN_Y_MAX 120 9
+1 50 MC_PITCHRATE_D 0.003 9
+1 50 MC_PITCHRATE_I 0 9
+1 50 MC_PITCHRATE_P 0.1 9
+1 50 MC_PITCH_P 7 9
+1 50 MC_ROLLRATE_D 0.003 9
+1 50 MC_ROLLRATE_I 0 9
+1 50 MC_ROLLRATE_P 0.1 9
+1 50 MC_ROLL_P 7 9
+1 50 MC_YAWRATE_D 0 9
+1 50 MC_YAWRATE_I 0.1 9
+1 50 MC_YAWRATE_MAX 120 9
+1 50 MC_YAWRATE_P 0.2 9
+1 50 MC_YAW_FF 0.5 9
+1 50 MC_YAW_P 2.8 9
+1 50 MIS_ALTMODE 0 6
+1 50 MIS_DIST_1WP 500 9
+1 50 MIS_ONBOARD_EN 1 6
+1 50 MIS_TAKEOFF_ALT 10 9
+1 50 MPC_LAND_SPEED 1 9
+1 50 MPC_THR_MAX 1 9
+1 50 MPC_THR_MIN 0.1 9
+1 50 MPC_TILTMAX_AIR 45 9
+1 50 MPC_TILTMAX_LND 15 9
+1 50 MPC_XY_FF 0.5 9
+1 50 MPC_XY_P 1 9
+1 50 MPC_XY_VEL_D 0.01 9
+1 50 MPC_XY_VEL_I 0.02 9
+1 50 MPC_XY_VEL_MAX 5 9
+1 50 MPC_XY_VEL_P 0.1 9
+1 50 MPC_Z_FF 0.5 9
+1 50 MPC_Z_P 1 9
+1 50 MPC_Z_VEL_D 0 9
+1 50 MPC_Z_VEL_I 0.02 9
+1 50 MPC_Z_VEL_MAX 3 9
+1 50 MPC_Z_VEL_P 0.1 9
+1 50 MT_ACC_D 0 9
+1 50 MT_ACC_D_LP 0.5 9
+1 50 MT_ACC_MAX 40 9
+1 50 MT_ACC_MIN -40 9
+1 50 MT_ACC_P 0.3 9
+1 50 MT_AD_LP 0.5 9
+1 50 MT_ALT_LP 1 9
+1 50 MT_A_LP 0.5 9
+1 50 MT_ENABLED 0 6
+1 50 MT_FPA_D 0 9
+1 50 MT_FPA_D_LP 1 9
+1 50 MT_FPA_LP 1 9
+1 50 MT_FPA_MAX 30 9
+1 50 MT_FPA_MIN -20 9
+1 50 MT_FPA_P 0.3 9
+1 50 MT_LND_PIT_MAX 15 9
+1 50 MT_LND_PIT_MIN -5 9
+1 50 MT_LND_THR_MAX 0 9
+1 50 MT_LND_THR_MIN 0 9
+1 50 MT_PIT_FF 0.4 9
+1 50 MT_PIT_I 0.03 9
+1 50 MT_PIT_I_MAX 10 9
+1 50 MT_PIT_MAX 20 9
+1 50 MT_PIT_MIN -45 9
+1 50 MT_PIT_OFF 0 9
+1 50 MT_PIT_P 0.03 9
+1 50 MT_THR_FF 0.7 9
+1 50 MT_THR_I 0.25 9
+1 50 MT_THR_I_MAX 10 9
+1 50 MT_THR_MAX 1 9
+1 50 MT_THR_MIN 0 9
+1 50 MT_THR_OFF 0.7 9
+1 50 MT_THR_P 0.1 9
+1 50 MT_TKF_PIT_MAX 45 9
+1 50 MT_TKF_PIT_MIN 0 9
+1 50 MT_TKF_THR_MAX 1 9
+1 50 MT_TKF_THR_MIN 1 9
+1 50 MT_USP_PIT_MAX 0 9
+1 50 MT_USP_PIT_MIN -45 9
+1 50 MT_USP_THR_MAX 1 9
+1 50 MT_USP_THR_MIN 1 9
+1 50 NAV_ACC_RAD 25 9
+1 50 NAV_AH_ALT 600 9
+1 50 NAV_AH_LAT -265847810 6
+1 50 NAV_AH_LON 1518423250 6
+1 50 NAV_DLL_AH_T 120 9
+1 50 NAV_DLL_CHSK 0 6
+1 50 NAV_DLL_CH_ALT 600 9
+1 50 NAV_DLL_CH_LAT -266072120 6
+1 50 NAV_DLL_CH_LON 1518453890 6
+1 50 NAV_DLL_CH_T 120 9
+1 50 NAV_DLL_N 2 6
+1 50 NAV_DLL_OBC 0 6
+1 50 NAV_GPSF_LT 30 9
+1 50 NAV_GPSF_P 0 9
+1 50 NAV_GPSF_R 15 9
+1 50 NAV_GPSF_TR 0.7 9
+1 50 NAV_LOITER_RAD 50 9
+1 50 NAV_RCL_LT 120 9
+1 50 NAV_RCL_OBC 0 6
+1 50 PE_ABIAS_PNOISE 5e-05 9
+1 50 PE_ACC_PNOISE 0.25 9
+1 50 PE_EAS_NOISE 1.4 9
+1 50 PE_GBIAS_PNOISE 1e-07 9
+1 50 PE_GPS_ALT_WGT 0.9 9
+1 50 PE_GYRO_PNOISE 0.015 9
+1 50 PE_HGT_DELAY_MS 350 6
+1 50 PE_MAGB_PNOISE 0.0003 9
+1 50 PE_MAGE_PNOISE 0.0003 9
+1 50 PE_MAG_DELAY_MS 30 6
+1 50 PE_MAG_NOISE 0.05 9
+1 50 PE_POSDEV_INIT 5 9
+1 50 PE_POSD_NOISE 1 9
+1 50 PE_POSNE_NOISE 0.5 9
+1 50 PE_POS_DELAY_MS 210 6
+1 50 PE_TAS_DELAY_MS 210 6
+1 50 PE_VELD_NOISE 0.7 9
+1 50 PE_VELNE_NOISE 0.5 9
+1 50 PE_VEL_DELAY_MS 230 6
+1 50 RC10_DZ 0 9
+1 50 RC10_MAX 2000 9
+1 50 RC10_MIN 1000 9
+1 50 RC10_REV 1 9
+1 50 RC10_TRIM 1500 9
+1 50 RC11_DZ 0 9
+1 50 RC11_MAX 2000 9
+1 50 RC11_MIN 1000 9
+1 50 RC11_REV 1 9
+1 50 RC11_TRIM 1500 9
+1 50 RC12_DZ 0 9
+1 50 RC12_MAX 2000 9
+1 50 RC12_MIN 1000 9
+1 50 RC12_REV 1 9
+1 50 RC12_TRIM 1500 9
+1 50 RC13_DZ 0 9
+1 50 RC13_MAX 2000 9
+1 50 RC13_MIN 1000 9
+1 50 RC13_REV 1 9
+1 50 RC13_TRIM 1500 9
+1 50 RC14_DZ 0 9
+1 50 RC14_MAX 2000 9
+1 50 RC14_MIN 1000 9
+1 50 RC14_REV 1 9
+1 50 RC14_TRIM 1500 9
+1 50 RC15_DZ 0 9
+1 50 RC15_MAX 2000 9
+1 50 RC15_MIN 1000 9
+1 50 RC15_REV 1 9
+1 50 RC15_TRIM 1500 9
+1 50 RC16_DZ 0 9
+1 50 RC16_MAX 2000 9
+1 50 RC16_MIN 1000 9
+1 50 RC16_REV 1 9
+1 50 RC16_TRIM 1500 9
+1 50 RC17_DZ 0 9
+1 50 RC17_MAX 2000 9
+1 50 RC17_MIN 1000 9
+1 50 RC17_REV 1 9
+1 50 RC17_TRIM 1500 9
+1 50 RC18_DZ 0 9
+1 50 RC18_MAX 2000 9
+1 50 RC18_MIN 1000 9
+1 50 RC18_REV 1 9
+1 50 RC18_TRIM 1500 9
+1 50 RC1_DZ 10 9
+1 50 RC1_MAX 1900 9
+1 50 RC1_MIN 1100 9
+1 50 RC1_REV -1 9
+1 50 RC1_TRIM 1490 9
+1 50 RC2_DZ 10 9
+1 50 RC2_MAX 1900 9
+1 50 RC2_MIN 1100 9
+1 50 RC2_REV 1 9
+1 50 RC2_TRIM 1483 9
+1 50 RC3_DZ 10 9
+1 50 RC3_MAX 1901 9
+1 50 RC3_MIN 1099 9
+1 50 RC3_REV 1 9
+1 50 RC3_TRIM 1099 9
+1 50 RC4_DZ 10 9
+1 50 RC4_MAX 1900 9
+1 50 RC4_MIN 1100 9
+1 50 RC4_REV -1 9
+1 50 RC4_TRIM 1500 9
+1 50 RC5_DZ 10 9
+1 50 RC5_MAX 1901 9
+1 50 RC5_MIN 1099 9
+1 50 RC5_REV 1 9
+1 50 RC5_TRIM 1500 9
+1 50 RC6_DZ 10 9
+1 50 RC6_MAX 1901 9
+1 50 RC6_MIN 1099 9
+1 50 RC6_REV 1 9
+1 50 RC6_TRIM 1500 9
+1 50 RC7_DZ 10 9
+1 50 RC7_MAX 1901 9
+1 50 RC7_MIN 1099 9
+1 50 RC7_REV 1 9
+1 50 RC7_TRIM 1500 9
+1 50 RC8_DZ 10 9
+1 50 RC8_MAX 1901 9
+1 50 RC8_MIN 1099 9
+1 50 RC8_REV 1 9
+1 50 RC8_TRIM 1500 9
+1 50 RC9_DZ 0 9
+1 50 RC9_MAX 2000 9
+1 50 RC9_MIN 1000 9
+1 50 RC9_REV 1 9
+1 50 RC9_TRIM 1500 9
+1 50 RC_ACRO_TH 0.5 9
+1 50 RC_ASSIST_TH 0.25 9
+1 50 RC_AUTO_TH 0.75 9
+1 50 RC_DSM_BIND -1 6
+1 50 RC_FAILS_THR 0 6
+1 50 RC_LOITER_TH 0.5 9
+1 50 RC_MAP_ACRO_SW 0 6
+1 50 RC_MAP_AUX1 0 6
+1 50 RC_MAP_AUX2 0 6
+1 50 RC_MAP_AUX3 0 6
+1 50 RC_MAP_FAILSAFE 0 6
+1 50 RC_MAP_FLAPS 0 6
+1 50 RC_MAP_LOITER_SW 0 6
+1 50 RC_MAP_MODE_SW 0 6
+1 50 RC_MAP_OFFB_SW 0 6
+1 50 RC_MAP_PITCH 2 6
+1 50 RC_MAP_POSCTL_SW 0 6
+1 50 RC_MAP_RETURN_SW 0 6
+1 50 RC_MAP_ROLL 1 6
+1 50 RC_MAP_THROTTLE 3 6
+1 50 RC_MAP_YAW 4 6
+1 50 RC_OFFB_TH 0.5 9
+1 50 RC_POSCTL_TH 0.5 9
+1 50 RC_RETURN_TH 0.5 9
+1 50 RTL_DESCEND_ALT 20 9
+1 50 RTL_LAND_DELAY -1 9
+1 50 RTL_LOITER_RAD 50 9
+1 50 RTL_RETURN_ALT 100 9
+1 50 SDLOG_EXT -1 6
+1 50 SDLOG_RATE -1 6
+1 50 SENS_ACC_XOFF 0 9
+1 50 SENS_ACC_XSCALE 1 9
+1 50 SENS_ACC_YOFF 0 9
+1 50 SENS_ACC_YSCALE 1 9
+1 50 SENS_ACC_ZOFF 0 9
+1 50 SENS_ACC_ZSCALE 1 9
+1 50 SENS_BARO_QNH 1013.25 9
+1 50 SENS_BOARD_ROT 0 6
+1 50 SENS_BOARD_X_OFF 0 9
+1 50 SENS_BOARD_Y_OFF 0 9
+1 50 SENS_BOARD_Z_OFF 0 9
+1 50 SENS_DPRES_ANSC 0 9
+1 50 SENS_DPRES_OFF 0 9
+1 50 SENS_EXT_MAG 0 6
+1 50 SENS_EXT_MAG_ROT 0 6
+1 50 SENS_GYRO_XOFF 0 9
+1 50 SENS_GYRO_XSCALE 1 9
+1 50 SENS_GYRO_YOFF 0 9
+1 50 SENS_GYRO_YSCALE 1 9
+1 50 SENS_GYRO_ZOFF 0 9
+1 50 SENS_GYRO_ZSCALE 1 9
+1 50 SENS_MAG_XOFF 0 9
+1 50 SENS_MAG_XSCALE 1 9
+1 50 SENS_MAG_YOFF 0 9
+1 50 SENS_MAG_YSCALE 1 9
+1 50 SENS_MAG_ZOFF 0 9
+1 50 SENS_MAG_ZSCALE 1 9
+1 50 SO3_COMP_KI 0.05 9
+1 50 SO3_COMP_KP 1 9
+1 50 SO3_PITCH_OFFS 0 9
+1 50 SO3_ROLL_OFFS 0 9
+1 50 SO3_YAW_OFFS 0 9
+1 50 SYS_AUTOCONFIG 0 6
+1 50 SYS_AUTOSTART 4010 6
+1 50 SYS_RESTART_TYPE 0 6
+1 50 SYS_USE_IO 1 6
+1 50 TEST_D 0.01 9
+1 50 TEST_DEV 2 9
+1 50 TEST_D_LP 10 9
+1 50 TEST_HP 10 9
+1 50 TEST_I 0.1 9
+1 50 TEST_I_MAX 1 9
+1 50 TEST_LP 10 9
+1 50 TEST_MAX 1 9
+1 50 TEST_MEAN 1 9
+1 50 TEST_MIN -1 9
+1 50 TEST_P 0.2 9
+1 50 TEST_TRIM 0.5 9
+1 50 TRIM_PITCH 0 9
+1 50 TRIM_ROLL 0 9
+1 50 TRIM_YAW 0 9
+1 50 UAVCAN_BITRATE 1000000 6
+1 50 UAVCAN_ENABLE 0 6
+1 50 UAVCAN_NODE_ID 1 6
+1 50 test 305419896 6
diff --git a/src/qgcunittest/MockLinkMissionItemHandler.cc b/src/qgcunittest/MockLinkMissionItemHandler.cc
new file mode 100644
index 0000000..3591dd0
--- /dev/null
+++ b/src/qgcunittest/MockLinkMissionItemHandler.cc
@@ -0,0 +1,1178 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see .
+
+ ======================================================================*/
+
+/**
+* @file
+* @brief a program to manage waypoints and exchange them with the ground station
+*
+* @author Petri Tanskanen
+* @author Benjamin Knecht
+* @author Christian Schluchter
+*/
+
+// FIXME: This file is a work in progress
+
+#include "MockLinkMissionItemHandler.h"
+
+#include
+
+#include "QGC.h"
+#include
+
+
+MockLinkMissionItemHandler::MockLinkMissionItemHandler(uint16_t systemId, QObject* parent) :
+ QObject(parent),
+ _vehicleSystemId(systemId)
+{
+
+}
+
+void MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
+{
+ switch (msg.msgid) {
+ case MAVLINK_MSG_ID_MISSION_ACK:
+ // Acks are received back for each MISSION_ITEM message
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
+ // Sets the currently active mission item
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+ // Signals the start of requesting the full mission list. Subsequent MISSION_REQUEST message should be received for
+ // each mission item.
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST:
+ // Request the specified mission item. Requests should be in order.
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_COUNT:
+ // Return the current number of mission items
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_ITEM:
+ // FIXME: Figure out
+ break;
+
+ case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+ // Delete all mission items
+ break;
+ }
+}
+
+#if 0
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+class PxMatrix3x3;
+
+
+/**
+ * @brief Pixhawk 3D vector class, can be cast to a local OpenCV CvMat.
+ *
+ */
+class PxVector3
+{
+public:
+ /** @brief standard constructor */
+ PxVector3(void) {}
+ /** @brief copy constructor */
+ PxVector3(const PxVector3 &v) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = v.m_vec[i];
+ }
+ }
+ /** @brief x,y,z constructor */
+ PxVector3(const float _x, const float _y, const float _z) {
+ m_vec[0] = _x;
+ m_vec[1] = _y;
+ m_vec[2] = _z;
+ }
+ /** @brief broadcast constructor */
+ PxVector3(const float _f) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = _f;
+ }
+ }
+
+private:
+ /** @brief private constructor (not used here, for SSE compatibility) */
+ PxVector3(const float (&_vec)[3]) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = _vec[i];
+ }
+ }
+
+public:
+ /** @brief assignment operator */
+ void operator= (const PxVector3 &r) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = r.m_vec[i];
+ }
+ }
+ /** @brief const element access */
+ float operator[] (const int i) const {
+ return m_vec[i];
+ }
+ /** @brief element access */
+ float &operator[] (const int i) {
+ return m_vec[i];
+ }
+
+ // === arithmetic operators ===
+ /** @brief element-wise negation */
+ friend PxVector3 operator- (const PxVector3 &v) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = -v.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3 operator+ (const PxVector3 &l, const PxVector3 &r) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] + r.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3 operator- (const PxVector3 &l, const PxVector3 &r) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] - r.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3 operator* (const PxVector3 &l, const PxVector3 &r) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] * r.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3 operator/ (const PxVector3 &l, const PxVector3 &r) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] / r.m_vec[i];
+ }
+ return ret;
+ }
+
+ friend void operator+= (PxVector3 &l, const PxVector3 &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] + r.m_vec[i];
+ }
+ }
+ friend void operator-= (PxVector3 &l, const PxVector3 &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] - r.m_vec[i];
+ }
+ }
+ friend void operator*= (PxVector3 &l, const PxVector3 &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] * r.m_vec[i];
+ }
+ }
+ friend void operator/= (PxVector3 &l, const PxVector3 &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] / r.m_vec[i];
+ }
+ }
+
+ friend PxVector3 operator+ (const PxVector3 &l, float f) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] + f;
+ }
+ return ret;
+ }
+ friend PxVector3 operator- (const PxVector3 &l, float f) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] - f;
+ }
+ return ret;
+ }
+ friend PxVector3 operator* (const PxVector3 &l, float f) {
+ PxVector3 ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] * f;
+ }
+ return ret;
+ }
+ friend PxVector3 operator/ (const PxVector3 &l, float f) {
+ PxVector3 ret;
+ float inv = 1.f/f;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] * inv;
+ }
+ return ret;
+ }
+
+ friend void operator+= (PxVector3 &l, float f) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] + f;
+ }
+ }
+ friend void operator-= (PxVector3 &l, float f) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] - f;
+ }
+ }
+ friend void operator*= (PxVector3 &l, float f) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] * f;
+ }
+ }
+ friend void operator/= (PxVector3 &l, float f) {
+ float inv = 1.f/f;
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] * inv;
+ }
+ }
+
+ // === vector operators ===
+ /** @brief dot product */
+ float dot(const PxVector3 &v) const {
+ return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2];
+ }
+ /** @brief length squared of the vector */
+ float lengthSquared(void) const {
+ return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2];
+ }
+ /** @brief length of the vector */
+ float length(void) const {
+ return sqrt(lengthSquared());
+ }
+ /** @brief cross product */
+ PxVector3 cross(const PxVector3 &v) const {
+ return PxVector3(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]);
+ }
+ /** @brief normalizes the vector */
+ PxVector3 &normalize(void) {
+ const float l = 1.f / length();
+ for (int i=0; i < 3; i++) {
+ m_vec[i] *= l;
+ }
+ return *this;
+ }
+
+ friend class PxMatrix3x3;
+protected:
+ float m_vec[3];
+};
+
+/**
+ * @brief Pixhawk 3D vector class in double precision, can be cast to a local OpenCV CvMat.
+ *
+ */
+class PxVector3Double
+{
+public:
+ /** @brief standard constructor */
+ PxVector3Double(void) {}
+ /** @brief copy constructor */
+ PxVector3Double(const PxVector3Double &v) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = v.m_vec[i];
+ }
+ }
+ /** @brief x,y,z constructor */
+ PxVector3Double(const double _x, const double _y, const double _z) {
+ m_vec[0] = _x;
+ m_vec[1] = _y;
+ m_vec[2] = _z;
+ }
+ /** @brief broadcast constructor */
+ PxVector3Double(const double _f) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = _f;
+ }
+ }
+
+private:
+ /** @brief private constructor (not used here, for SSE compatibility) */
+ PxVector3Double(const double (&_vec)[3]) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = _vec[i];
+ }
+ }
+
+public:
+ /** @brief assignment operator */
+ void operator= (const PxVector3Double &r) {
+ for (int i=0; i < 3; i++) {
+ m_vec[i] = r.m_vec[i];
+ }
+ }
+ /** @brief const element access */
+ double operator[] (const int i) const {
+ return m_vec[i];
+ }
+ /** @brief element access */
+ double &operator[] (const int i) {
+ return m_vec[i];
+ }
+
+ // === arithmetic operators ===
+ /** @brief element-wise negation */
+ friend PxVector3Double operator- (const PxVector3Double &v) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = -v.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3Double operator+ (const PxVector3Double &l, const PxVector3Double &r) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] + r.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3Double operator- (const PxVector3Double &l, const PxVector3Double &r) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] - r.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3Double operator* (const PxVector3Double &l, const PxVector3Double &r) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] * r.m_vec[i];
+ }
+ return ret;
+ }
+ friend PxVector3Double operator/ (const PxVector3Double &l, const PxVector3Double &r) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] / r.m_vec[i];
+ }
+ return ret;
+ }
+
+ friend void operator+= (PxVector3Double &l, const PxVector3Double &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] + r.m_vec[i];
+ }
+ }
+ friend void operator-= (PxVector3Double &l, const PxVector3Double &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] - r.m_vec[i];
+ }
+ }
+ friend void operator*= (PxVector3Double &l, const PxVector3Double &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] * r.m_vec[i];
+ }
+ }
+ friend void operator/= (PxVector3Double &l, const PxVector3Double &r) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] / r.m_vec[i];
+ }
+ }
+
+ friend PxVector3Double operator+ (const PxVector3Double &l, double f) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] + f;
+ }
+ return ret;
+ }
+ friend PxVector3Double operator- (const PxVector3Double &l, double f) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] - f;
+ }
+ return ret;
+ }
+ friend PxVector3Double operator* (const PxVector3Double &l, double f) {
+ PxVector3Double ret;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] * f;
+ }
+ return ret;
+ }
+ friend PxVector3Double operator/ (const PxVector3Double &l, double f) {
+ PxVector3Double ret;
+ double inv = 1.f/f;
+ for (int i=0; i < 3; i++) {
+ ret.m_vec[i] = l.m_vec[i] * inv;
+ }
+ return ret;
+ }
+
+ friend void operator+= (PxVector3Double &l, double f) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] + f;
+ }
+ }
+ friend void operator-= (PxVector3Double &l, double f) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] - f;
+ }
+ }
+ friend void operator*= (PxVector3Double &l, double f) {
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] * f;
+ }
+ }
+ friend void operator/= (PxVector3Double &l, double f) {
+ double inv = 1.f/f;
+ for (int i=0; i < 3; i++) {
+ l.m_vec[i] = l.m_vec[i] * inv;
+ }
+ }
+
+ // === vector operators ===
+ /** @brief dot product */
+ double dot(const PxVector3Double &v) const {
+ return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2];
+ }
+ /** @brief length squared of the vector */
+ double lengthSquared(void) const {
+ return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2];
+ }
+ /** @brief length of the vector */
+ double length(void) const {
+ return sqrt(lengthSquared());
+ }
+ /** @brief cross product */
+ PxVector3Double cross(const PxVector3Double &v) const {
+ return PxVector3Double(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]);
+ }
+ /** @brief normalizes the vector */
+ PxVector3Double &normalize(void) {
+ const double l = 1.f / length();
+ for (int i=0; i < 3; i++) {
+ m_vec[i] *= l;
+ }
+ return *this;
+ }
+
+ friend class PxMatrix3x3;
+protected:
+ double m_vec[3];
+};
+
+
+ link(parent),
+ idle(false),
+ current_active_wp_id(-1),
+ yawReached(false),
+ posReached(false),
+ timestamp_lastoutside_orbit(0),
+ timestamp_firstinside_orbit(0),
+ waypoints(&waypoints1),
+ waypoints_receive_buffer(&waypoints2),
+ current_state(PX_WPP_IDLE),
+ protocol_current_wp_id(0),
+ protocol_current_count(0),
+ protocol_current_partner_systemid(0),
+ protocol_current_partner_compid(0),
+ protocol_timestamp_lastaction(0),
+ protocol_timeout(1000),
+ timestamp_last_send_setpoint(0),
+ systemid(sysid),
+ compid(MAV_COMP_ID_MISSIONPLANNER),
+ setpointDelay(10),
+ yawTolerance(0.4f),
+ verbose(true),
+ debug(false),
+ silent(false)
+{
+ connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
+ qDebug() << "PLANNER FOR SYSTEM" << systemid << "INITIALIZED";
+}
+
+
+
+/*
+* @brief Sends an waypoint ack message
+*/
+void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type)
+{
+ mavlink_message_t msg;
+ mavlink_mission_ack_t wpa;
+
+ wpa.target_system = target_systemid;
+ wpa.target_component = target_compid;
+ wpa.type = type;
+
+ mavlink_msg_mission_ack_encode(systemid, compid, &msg, &wpa);
+ link->sendMAVLinkMessage(&msg);
+
+
+
+ if (verbose) qDebug("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
+}
+
+/*
+* @brief Broadcasts the new target waypoint and directs the MAV to fly there
+*
+* This function broadcasts its new active waypoint sequence number and
+* sends a message to the controller, advising it to fly to the coordinates
+* of the waypoint with a given orientation
+*
+* @param seq The waypoint sequence number the MAV should fly to.
+*/
+void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq)
+{
+ if(seq < waypoints->size()) {
+ mavlink_mission_item_t *cur = waypoints->at(seq);
+
+ mavlink_message_t msg;
+ mavlink_mission_current_t wpc;
+
+ wpc.seq = cur->seq;
+
+ mavlink_msg_mission_current_encode(systemid, compid, &msg, &wpc);
+ link->sendMAVLinkMessage(&msg);
+
+
+
+ if (verbose) qDebug("Broadcasted new current waypoint %u\n", wpc.seq);
+ }
+}
+
+/*
+* @brief Directs the MAV to fly to a position
+*
+* Sends a message to the controller, advising it to fly to the coordinates
+* of the waypoint with a given orientation
+*
+* @param seq The waypoint sequence number the MAV should fly to.
+*/
+void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq)
+{
+ Q_UNUSED(seq);
+}
+
+void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count)
+{
+ mavlink_message_t msg;
+ mavlink_mission_count_t wpc;
+
+ wpc.target_system = target_systemid;
+ wpc.target_component = target_compid;
+ wpc.count = count;
+
+ mavlink_msg_mission_count_encode(systemid, compid, &msg, &wpc);
+ link->sendMAVLinkMessage(&msg);
+
+ if (verbose) qDebug("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
+
+
+}
+
+void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq)
+{
+ if (seq < waypoints->size()) {
+ mavlink_message_t msg;
+ mavlink_mission_item_t *wp = waypoints->at(seq);
+ wp->target_system = target_systemid;
+ wp->target_component = target_compid;
+
+ if (verbose) qDebug("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->command, wp->param3, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->param4, wp->autocontinue);
+
+ mavlink_msg_mission_item_encode(systemid, compid, &msg, wp);
+ link->sendMAVLinkMessage(&msg);
+ if (verbose) qDebug("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
+
+
+ } else {
+ if (verbose) qDebug("ERROR: index out of bounds\n");
+ }
+}
+
+void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq)
+{
+ mavlink_message_t msg;
+ mavlink_mission_request_t wpr;
+ wpr.target_system = target_systemid;
+ wpr.target_component = target_compid;
+ wpr.seq = seq;
+ mavlink_msg_mission_request_encode(systemid, compid, &msg, &wpr);
+ link->sendMAVLinkMessage(&msg);
+ if (verbose) qDebug("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
+
+
+}
+
+/*
+* @brief emits a message that a waypoint reached
+*
+* This function broadcasts a message that a waypoint is reached.
+*
+* @param seq The waypoint sequence number the MAV has reached.
+*/
+void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq)
+{
+ mavlink_message_t msg;
+ mavlink_mission_item_reached_t wp_reached;
+
+ wp_reached.seq = seq;
+
+ mavlink_msg_mission_item_reached_encode(systemid, compid, &msg, &wp_reached);
+ link->sendMAVLinkMessage(&msg);
+
+ if (verbose) qDebug("Sent waypoint %u reached message\n", wp_reached.seq);
+
+
+}
+
+float MAVLinkSimulationWaypointPlanner::distanceToSegment(uint16_t seq, float x, float y, float z)
+{
+ if (seq < waypoints->size()) {
+ mavlink_mission_item_t *cur = waypoints->at(seq);
+
+ const PxVector3 A(cur->x, cur->y, cur->z);
+ const PxVector3 C(x, y, z);
+
+ // seq not the second last waypoint
+ if ((uint16_t)(seq+1) < waypoints->size()) {
+ mavlink_mission_item_t *next = waypoints->at(seq+1);
+ const PxVector3 B(next->x, next->y, next->z);
+ const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
+ if (r >= 0 && r <= 1) {
+ const PxVector3 P(A + r*(B-A));
+ return (P-C).length();
+ } else if (r < 0.f) {
+ return (C-A).length();
+ } else {
+ return (C-B).length();
+ }
+ } else {
+ return (C-A).length();
+ }
+ }
+ return -1.f;
+}
+
+float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y, float z)
+{
+ if (seq < waypoints->size()) {
+ mavlink_mission_item_t *cur = waypoints->at(seq);
+
+ const PxVector3 A(cur->x, cur->y, cur->z);
+ const PxVector3 C(x, y, z);
+
+ return (C-A).length();
+ }
+ return -1.f;
+}
+
+float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y)
+{
+ if (seq < waypoints->size()) {
+ mavlink_mission_item_t *cur = waypoints->at(seq);
+
+ const PxVector3 A(cur->x, cur->y, 0);
+ const PxVector3 C(x, y, 0);
+
+ return (C-A).length();
+ }
+ return -1.f;
+}
+
+void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg)
+{
+ mavlink_handler(&msg);
+}
+
+void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* msg)
+{
+ // Handle param messages
+// paramClient->handleMAVLinkPacket(msg);
+
+ //check for timed-out operations
+
+ //qDebug() << "MAV: %d WAYPOINTPLANNER GOT MESSAGE" << systemid;
+
+ uint64_t now = QGC::groundTimeMilliseconds();
+ if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) {
+ if (verbose) qDebug() << "Last operation (state=%u) timed out, changing state to PX_WPP_IDLE" << current_state;
+ current_state = PX_WPP_IDLE;
+ protocol_current_count = 0;
+ protocol_current_partner_systemid = 0;
+ protocol_current_partner_compid = 0;
+ protocol_current_wp_id = -1;
+
+ if(waypoints->size() == 0) {
+ current_active_wp_id = -1;
+ }
+ }
+
+ if(now-timestamp_last_send_setpoint > setpointDelay) {
+ send_setpoint(current_active_wp_id);
+ }
+
+ switch(msg->msgid) {
+ case MAVLINK_MSG_ID_ATTITUDE: {
+ if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) {
+ mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id);
+ if(wp->frame == 1) {
+ mavlink_attitude_t att;
+ mavlink_msg_attitude_decode(msg, &att);
+ float yaw_tolerance = yawTolerance;
+ //compare current yaw
+ if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) {
+ if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
+ yawReached = true;
+ } else if(att.yaw - yaw_tolerance < 0.0f) {
+ float lowerBound = 360.0f + att.yaw - yaw_tolerance;
+ if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
+ yawReached = true;
+ } else {
+ float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
+ if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
+ yawReached = true;
+ }
+
+ // FIXME HACK: Ignore yaw:
+
+ yawReached = true;
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
+ if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) {
+ mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id);
+
+ if(wp->frame == 1) {
+ mavlink_local_position_ned_t pos;
+ mavlink_msg_local_position_ned_decode(msg, &pos);
+ //qDebug() << "Received new position: x:" << pos.x << "| y:" << pos.y << "| z:" << pos.z;
+
+ posReached = false;
+
+ // compare current position (given in message) with current waypoint
+ float orbit = wp->param1;
+
+ float dist;
+ if (wp->param2 == 0) {
+ dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z);
+ } else {
+ dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z);
+ }
+
+ if (dist >= 0.f && dist <= orbit && yawReached) {
+ posReached = true;
+ }
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+ if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) {
+ mavlink_mission_item_t *wp = waypoints->at(current_active_wp_id);
+
+ if(wp->frame == 0) {
+ mavlink_global_position_int_t pos;
+ mavlink_msg_global_position_int_decode(msg, &pos);
+
+ float x = static_cast(pos.lat)/1E7;
+ float y = static_cast(pos.lon)/1E7;
+ //float z = static_cast(pos.alt)/1000;
+
+ //qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
+
+ posReached = false;
+ yawReached = true;
+
+ // FIXME big hack for simulation!
+ //float oneDegreeOfLatMeters = 111131.745f;
+ float orbit = 0.00008f;
+
+ // compare current position (given in message) with current waypoint
+ //float orbit = wp->param1;
+
+ // Convert to degrees
+
+
+ float dist;
+ dist = distanceToPoint(current_active_wp_id, x, y);
+
+ if (dist >= 0.f && dist <= orbit && yawReached) {
+ posReached = true;
+ qDebug() << "WP PLANNER: REACHED POSITION";
+ }
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_COMMAND_LONG:
+ { // special action from ground station
+ mavlink_command_long_t action;
+ mavlink_msg_command_long_decode(msg, &action);
+ if(action.target_system == systemid) {
+ if (verbose) qDebug("Waypoint: received message with action %d\n", action.command);
+// switch (action.action) {
+// case MAV_ACTION_LAUNCH:
+// if (verbose) std::cerr << "Launch received" << std::endl;
+// current_active_wp_id = 0;
+// if (waypoints->size()>0)
+// {
+// setActive(waypoints[current_active_wp_id]);
+// }
+// else
+// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
+// break;
+
+// case MAV_ACTION_CONTINUE:
+// if (verbose) std::c
+// err << "Continue received" << std::endl;
+// idle = false;
+// setActive(waypoints[current_active_wp_id]);
+// break;
+
+// case MAV_ACTION_HALT:
+// if (verbose) std::cerr << "Halt received" << std::endl;
+// idle = true;
+// break;
+
+// default:
+// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
+// break;
+// }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MISSION_ACK: {
+ mavlink_mission_ack_t wpa;
+ mavlink_msg_mission_ack_decode(msg, &wpa);
+
+ if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) {
+ protocol_timestamp_lastaction = now;
+
+ if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) {
+ if (protocol_current_wp_id == waypoints->size()-1) {
+ if (verbose) qDebug("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
+ current_state = PX_WPP_IDLE;
+ protocol_current_wp_id = 0;
+ }
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
+ mavlink_mission_set_current_t wpc;
+ mavlink_msg_mission_set_current_decode(msg, &wpc);
+
+ if(wpc.target_system == systemid && wpc.target_component == compid) {
+ protocol_timestamp_lastaction = now;
+
+ if (current_state == PX_WPP_IDLE) {
+ if (wpc.seq < waypoints->size()) {
+ if (verbose) qDebug("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
+ current_active_wp_id = wpc.seq;
+ uint32_t i;
+ for(i = 0; i < waypoints->size(); i++) {
+ if (i == current_active_wp_id) {
+ waypoints->at(i)->current = true;
+ } else {
+ waypoints->at(i)->current = false;
+ }
+ }
+ if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
+ yawReached = false;
+ posReached = false;
+ send_waypoint_current(current_active_wp_id);
+ send_setpoint(current_active_wp_id);
+ timestamp_firstinside_orbit = 0;
+ } else {
+ if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
+ }
+ }
+ } else {
+ qDebug() << "SYSTEM / COMPONENT ID MISMATCH: target sys:" << wpc.target_system << "this system:" << systemid << "target comp:" << wpc.target_component << "this comp:" << compid;
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
+ mavlink_mission_request_list_t wprl;
+ mavlink_msg_mission_request_list_decode(msg, &wprl);
+ if(wprl.target_system == systemid && wprl.target_component == compid) {
+ protocol_timestamp_lastaction = now;
+
+ if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) {
+ if (waypoints->size() > 0) {
+ if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
+ if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
+ current_state = PX_WPP_SENDLIST;
+ protocol_current_wp_id = 0;
+ protocol_current_partner_systemid = msg->sysid;
+ protocol_current_partner_compid = msg->compid;
+ } else {
+ if (verbose) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
+ }
+ protocol_current_count = static_cast(waypoints->size());
+ send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
+ } else {
+ if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
+ }
+ } else {
+ if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because not my systemid or compid.\n");
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MISSION_REQUEST: {
+ mavlink_mission_request_t wpr;
+ mavlink_msg_mission_request_decode(msg, &wpr);
+ if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) {
+ protocol_timestamp_lastaction = now;
+
+ //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
+ if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) {
+ if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+ if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+ if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+
+ current_state = PX_WPP_SENDLIST_SENDWPS;
+ protocol_current_wp_id = wpr.seq;
+ send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq);
+ } else {
+ if (verbose) {
+ if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) {
+ qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", current_state);
+ break;
+ } else if (current_state == PX_WPP_SENDLIST) {
+ if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
+ } else if (current_state == PX_WPP_SENDLIST_SENDWPS) {
+ if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
+ else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
+ } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n");
+ }
+ }
+ } else {
+ //we we're target but already communicating with someone else
+ if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) {
+ if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MISSION_COUNT: {
+ mavlink_mission_count_t wpc;
+ mavlink_msg_mission_count_decode(msg, &wpc);
+ if(wpc.target_system == systemid && wpc.target_component == compid) {
+ protocol_timestamp_lastaction = now;
+
+ if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) {
+ if (wpc.count > 0) {
+ if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
+ if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
+
+ current_state = PX_WPP_GETLIST;
+ protocol_current_wp_id = 0;
+ protocol_current_partner_systemid = msg->sysid;
+ protocol_current_partner_compid = msg->compid;
+ protocol_current_count = wpc.count;
+
+ qDebug("clearing receive buffer and readying for receiving waypoints\n");
+ while(waypoints_receive_buffer->size() > 0) {
+ delete waypoints_receive_buffer->back();
+ waypoints_receive_buffer->pop_back();
+ }
+
+ send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
+ } else {
+ if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_MISSION_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
+ }
+ } else {
+ if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm doing something else already (state=%i).\n", current_state);
+ else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
+ else qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT - FIXME: missed error description\n");
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MISSION_ITEM: {
+ mavlink_mission_item_t wp;
+ mavlink_msg_mission_item_decode(msg, &wp);
+
+ if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) {
+ protocol_timestamp_lastaction = now;
+
+ //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
+ if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) {
+ if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
+ if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
+ if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
+
+ current_state = PX_WPP_GETLIST_GETWPS;
+ protocol_current_wp_id = wp.seq + 1;
+ mavlink_mission_item_t* newwp = new mavlink_mission_item_t;
+ memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
+ waypoints_receive_buffer->push_back(newwp);
+
+ if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) {
+ if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
+
+ send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
+
+ if (current_active_wp_id > waypoints_receive_buffer->size()-1) {
+ current_active_wp_id = static_cast(waypoints_receive_buffer->size()) - 1;
+ }
+
+ // switch the waypoints list
+ std::vector* waypoints_temp = waypoints;
+ waypoints = waypoints_receive_buffer;
+ waypoints_receive_buffer = waypoints_temp;
+
+ //get the new current waypoint
+ uint32_t i;
+ for(i = 0; i < waypoints->size(); i++) {
+ if (waypoints->at(i)->current == 1) {
+ current_active_wp_id = i;
+ //if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id);
+ yawReached = false;
+ posReached = false;
+ send_waypoint_current(current_active_wp_id);
+ send_setpoint(current_active_wp_id);
+ timestamp_firstinside_orbit = 0;
+ break;
+ }
+ }
+
+ if (i == waypoints->size()) {
+ current_active_wp_id = -1;
+ yawReached = false;
+ posReached = false;
+ timestamp_firstinside_orbit = 0;
+ }
+
+ current_state = PX_WPP_IDLE;
+ } else {
+ send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
+ }
+ } else {
+ if (current_state == PX_WPP_IDLE) {
+ //we're done receiving waypoints, answer with ack.
+ send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
+ qDebug("Received MAVLINK_MSG_ID_MISSION_ITEM while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
+ }
+ if (verbose) {
+ if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) {
+ qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, current_state);
+ break;
+ } else if (current_state == PX_WPP_GETLIST) {
+ if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
+ else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+ } else if (current_state == PX_WPP_GETLIST_GETWPS) {
+ if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
+ else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
+ else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+ } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
+ }
+ }
+ } else {
+ // We're target but already communicating with someone else
+ if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) {
+ if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
+ } else if(wp.target_system == systemid && wp.target_component == compid) {
+ if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
+ }
+ }
+ break;
+ }
+
+ case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
+ mavlink_mission_clear_all_t wpca;
+ mavlink_msg_mission_clear_all_decode(msg, &wpca);
+
+ if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) {
+ protocol_timestamp_lastaction = now;
+
+ if (verbose) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
+ while(waypoints->size() > 0) {
+ delete waypoints->back();
+ waypoints->pop_back();
+ }
+ current_active_wp_id = -1;
+ } else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) {
+ if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
+ }
+ break;
+ }
+
+ default: {
+ if (debug) qDebug("Waypoint: received message of unknown type\n");
+ break;
+ }
+ }
+
+ //check if the current waypoint was reached
+ if ((posReached && /*yawReached &&*/ !idle)) {
+ if (current_active_wp_id < waypoints->size()) {
+ mavlink_mission_item_t *cur_wp = waypoints->at(current_active_wp_id);
+
+ if (timestamp_firstinside_orbit == 0) {
+ // Announce that last waypoint was reached
+ if (verbose) qDebug("*** Reached waypoint %u ***\n", cur_wp->seq);
+ send_waypoint_reached(cur_wp->seq);
+ timestamp_firstinside_orbit = now;
+ }
+
+ // check if the MAV was long enough inside the waypoint orbit
+ //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
+ if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) {
+ if (cur_wp->autocontinue) {
+ cur_wp->current = 0;
+ if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 0) {
+ //the last waypoint was reached, if auto continue is
+ //activated restart the waypoint list from the beginning
+ current_active_wp_id = 0;
+ } else {
+ current_active_wp_id++;
+ }
+
+ // Fly to next waypoint
+ timestamp_firstinside_orbit = 0;
+ send_waypoint_current(current_active_wp_id);
+ send_setpoint(current_active_wp_id);
+ waypoints->at(current_active_wp_id)->current = true;
+ posReached = false;
+ //yawReached = false;
+ if (verbose) qDebug("Set new waypoint (%u)\n", current_active_wp_id);
+ }
+ }
+ }
+ } else {
+ timestamp_lastoutside_orbit = now;
+ }
+}
+
+#endif
\ No newline at end of file
diff --git a/src/qgcunittest/MockLinkMissionItemHandler.h b/src/qgcunittest/MockLinkMissionItemHandler.h
new file mode 100644
index 0000000..f1d9a7e
--- /dev/null
+++ b/src/qgcunittest/MockLinkMissionItemHandler.h
@@ -0,0 +1,109 @@
+/*=====================================================================
+
+ QGroundControl Open Source Ground Control Station
+
+ (c) 2009 - 2014 QGROUNDCONTROL PROJECT
+
+ This file is part of the QGROUNDCONTROL project
+
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see .
+
+ ======================================================================*/
+
+#ifndef MOCKLINKMISSIONITEMHANDLER_H
+#define MOCKLINKMISSIONITEMHANDLER_H
+
+// FIXME: This file is a work in progress
+
+#include
+#include
+
+#include "MAVLinkSimulationLink.h"
+#include "QGCMAVLink.h"
+
+enum PX_WAYPOINTPLANNER_STATES {
+ PX_WPP_IDLE = 0,
+ PX_WPP_SENDLIST,
+ PX_WPP_SENDLIST_SENDWPS,
+ PX_WPP_GETLIST,
+ PX_WPP_GETLIST_GETWPS,
+ PX_WPP_GETLIST_GOTALL
+};
+
+class MockLinkMissionItemHandler : public QObject
+{
+ Q_OBJECT
+
+public:
+ MockLinkMissionItemHandler(uint16_t systemId, QObject* parent = NULL);
+
+ /// @brief Called to handle mission item related messages. All messages should be passed to this method.
+ /// It will handle the appropriate set.
+ void handleMessage(const mavlink_message_t& msg);
+
+#if 0
+signals:
+ void messageSent(const mavlink_message_t& msg);
+
+protected:
+ MAVLinkSimulationLink* link;
+ bool idle; ///< indicates if the system is following the waypoints or is waiting
+ uint16_t current_active_wp_id; ///< id of current waypoint
+ bool yawReached; ///< boolean for yaw attitude reached
+ bool posReached; ///< boolean for position reached
+ uint64_t timestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
+ uint64_t timestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
+
+ std::vector waypoints1; ///< vector1 that holds the waypoints
+ std::vector waypoints2; ///< vector2 that holds the waypoints
+
+ std::vector* waypoints; ///< pointer to the currently active waypoint vector
+ std::vector* waypoints_receive_buffer; ///< pointer to the receive buffer waypoint vector
+ PX_WAYPOINTPLANNER_STATES current_state;
+ uint16_t protocol_current_wp_id;
+ uint16_t protocol_current_count;
+ uint8_t protocol_current_partner_systemid;
+ uint8_t protocol_current_partner_compid;
+ uint64_t protocol_timestamp_lastaction;
+ unsigned int protocol_timeout;
+ uint64_t timestamp_last_send_setpoint;
+ uint8_t systemid;
+ uint8_t compid;
+ unsigned int setpointDelay;
+ float yawTolerance;
+ bool verbose;
+ bool debug;
+ bool silent;
+
+ void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type);
+ void send_waypoint_current(uint16_t seq);
+ void send_setpoint(uint16_t seq);
+ void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count);
+ void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq);
+ void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq);
+ void send_waypoint_reached(uint16_t seq);
+ float distanceToSegment(uint16_t seq, float x, float y, float z);
+ float distanceToPoint(uint16_t seq, float x, float y, float z);
+ float distanceToPoint(uint16_t seq, float x, float y);
+ void mavlink_handler(const mavlink_message_t* msg);
+#endif
+
+private:
+ uint16_t _vehicleSystemId; ///< System id of this vehicle
+
+ QList _missionItems; ///< Current set of mission itemss
+
+};
+
+#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H
diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc
index 7b3d411..389d405 100644
--- a/src/ui/CommConfigurationWindow.cc
+++ b/src/ui/CommConfigurationWindow.cc
@@ -42,6 +42,9 @@ This file is part of the QGROUNDCONTROL project
#include "UDPLink.h"
#include "TCPLink.h"
#include "MAVLinkSimulationLink.h"
+#ifdef UNITTEST_BUILD
+#include "MockLink.h"
+#endif
#ifdef QGC_XBEE_ENABLED
#include "XbeeLink.h"
#include "XbeeConfigurationWindow.h"
@@ -85,10 +88,15 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
ui.linkType->addItem(tr("Serial"), QGC_LINK_SERIAL);
ui.linkType->addItem(tr("UDP"), QGC_LINK_UDP);
ui.linkType->addItem(tr("TCP"), QGC_LINK_TCP);
+
if(dynamic_cast(link)) {
//Only show simulation option if already setup elsewhere as a simulation
ui.linkType->addItem(tr("Simulation"), QGC_LINK_SIMULATION);
}
+
+#ifdef UNITTEST_BUILD
+ ui.linkType->addItem(tr("Mock"), QGC_LINK_MOCK);
+#endif
#ifdef QGC_RTLAB_ENABLED
ui.linkType->addItem(tr("Opal-RT Link"), QGC_LINK_OPAL);
@@ -144,6 +152,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
ui.linkGroupBox->setTitle(tr("Serial Link"));
ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_SERIAL));
}
+
UDPLink* udp = dynamic_cast(link);
if (udp != 0) {
QWidget* conf = new QGCUDPLinkConfiguration(udp, this);
@@ -151,6 +160,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
ui.linkGroupBox->setTitle(tr("UDP Link"));
ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_UDP));
}
+
TCPLink* tcp = dynamic_cast(link);
if (tcp != 0) {
QWidget* conf = new QGCTCPLinkConfiguration(tcp, this);
@@ -158,12 +168,22 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
ui.linkGroupBox->setTitle(tr("TCP Link"));
ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_TCP));
}
+
MAVLinkSimulationLink* sim = dynamic_cast(link);
if (sim != 0) {
ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_SIMULATION));
ui.linkType->setEnabled(false); //Don't allow the user to change to a non-simulation
ui.linkGroupBox->setTitle(tr("MAVLink Simulation Link"));
}
+
+#ifdef UNITTEST_BUILD
+ MockLink* mock = dynamic_cast(link);
+ if (mock != 0) {
+ ui.linkGroupBox->setTitle(tr("Mock Link"));
+ ui.linkType->setCurrentIndex(ui.linkType->findData(QGC_LINK_MOCK));
+ }
+#endif
+
#ifdef QGC_RTLAB_ENABLED
OpalLink* opal = dynamic_cast(link);
if (opal != 0) {
@@ -175,6 +195,7 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
ui.linkGroupBox->setTitle(tr("Opal-RT Link"));
}
#endif
+
#ifdef QGC_XBEE_ENABLED
XbeeLink* xbee = dynamic_cast(link); // new Konrad
if(xbee != 0)
@@ -187,7 +208,11 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn
connect(xbee,SIGNAL(tryConnectEnd(bool)),ui.actionConnect,SLOT(setEnabled(bool)));
}
#endif // QGC_XBEE_ENABLED
+
if (serial == 0 && udp == 0 && sim == 0 && tcp == 0
+#ifdef UNITTEST_BUILD
+ && mock == 0
+#endif
#ifdef QGC_RTLAB_ENABLED
&& opal == 0
#endif
@@ -258,6 +283,7 @@ void CommConfigurationWindow::setLinkType(qgc_link_t linktype)
break;
}
#endif // QGC_XBEE_ENABLED
+
case QGC_LINK_UDP:
{
UDPLink *udp = new UDPLink();
@@ -283,9 +309,18 @@ void CommConfigurationWindow::setLinkType(qgc_link_t linktype)
break;
}
#endif // QGC_RTLAB_ENABLED
+
+#ifdef UNITTEST_BUILD
+ case QGC_LINK_MOCK:
+ {
+ MockLink* mock = new MockLink;
+ tmpLink = mock;
+ MainWindow::instance()->addLink(tmpLink);
+ break;
+ }
+#endif
+
default:
- {
- }
case QGC_LINK_SERIAL:
{
SerialLink *serial = new SerialLink();
diff --git a/src/ui/CommConfigurationWindow.h b/src/ui/CommConfigurationWindow.h
index d506d0a..acb8e10 100644
--- a/src/ui/CommConfigurationWindow.h
+++ b/src/ui/CommConfigurationWindow.h
@@ -45,6 +45,9 @@ enum qgc_link_t {
QGC_LINK_TCP,
QGC_LINK_SIMULATION,
QGC_LINK_FORWARDING,
+#ifdef UNITTEST_BUILD
+ QGC_LINK_MOCK,
+#endif
#ifdef QGC_XBEE_ENABLED
QGC_LINK_XBEE,
#endif