diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 0c0b7088e623a341c164d3efed2af28583c471cf..4d7307ec61b6cc53db6c70bf9d27829f8755fe7c 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 e93171ffe4d42a48a92add501e8c656211761dbd..68cf6a0b067ac4185185f4e04cf4ef9618b58fd0 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 0000000000000000000000000000000000000000..d8721dca2d035f27668d28ca675ec33c900a8f34 --- /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 0000000000000000000000000000000000000000..0c8da2646033092d4103cea2df8d07646e54a007 --- /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 0000000000000000000000000000000000000000..920dda7144eca68ec01e2ede17035e28ae9c4baa --- /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 0000000000000000000000000000000000000000..3591dd055a55a984b23b6c7ab3528787dcfb5c86 --- /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 0000000000000000000000000000000000000000..f1d9a7e809feb354cb794f6819675666c9c7ea90 --- /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 7b3d411019d4f6963c6ad38a23c4e942e5adf13b..389d405e7ddf3fbe006282dd556900fb43ad39ee 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 d506d0a7158b7dfc0216bfc98c87e20b0b10dd35..acb8e101ad84f2b19c68a224da498f5ea8e2b5b9 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