Skip to content

Q
qgroundcontrol
Commit 765222eb authored by Don Gagne's avatar Don Gagne
Browse files
Options

Mock implementation of a Link 

Handy for testing without a real board
parent 1a182ddd
Show whitespace changes
Inline Side-by-side
Showing with 2408 additions and 2 deletions
qgroundcontrol.pro
View file @ 765222eb
......@@ -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 \
......
qgroundcontrol.qrc
View file @ 765222eb
......@@ -228,4 +228,7 @@
<qresource prefix="/general">
<file alias="vera.ttf">files/styles/Vera.ttf</file>
</qresource>
<qresource prefix="/unittest">
<file alias="MockLink.param">src/qgcunittest/MockLink.param</file>
</qresource>
</RCC>
src/qgcunittest/MockLink.cc 0 → 100644
View file @ 765222eb
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
#include "MockLink.h"
#include <QTimer>
#include <QDebug>
#include <QFile>
#include "LinkManager.h"
#include <string.h>
/// @file
/// @brief Mock implementation of a Link.
///
/// @author Don Gagne <don@thegagnes.com>
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(&paramFile);
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; i<cBytes; i++)
{
if (!mavlink_parse_char(_linkId, bytes[i], &msg, &comm)) {
continue;
}
Q_ASSERT(_missionItemHandler);
_missionItemHandler->handleMessage(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);
}
}
src/qgcunittest/MockLink.h 0 → 100644
View file @ 765222eb
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef MOCKLINK_H
#define MOCKLINK_H
#include <QMap>
#include "MockLinkMissionItemHandler.h"
#include "LinkInterface.h"
#include "mavlink.h"
/// @file
/// @brief Mock implementation of a Link.
///
/// @author Don Gagne <don@thegagnes.com>
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<QString, QVariant> ParamMap_t;
ParamMap_t _parameters;
uint16_t _cParameters;
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
MissionList_t _missionItems;
uint8_t _mavMode;
uint8_t _mavState;
};
#endif
src/qgcunittest/MockLink.param 0 → 100644
View file @ 765222eb
# 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
src/qgcunittest/MockLinkMissionItemHandler.cc 0 → 100644
View file @ 765222eb
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief a program to manage waypoints and exchange them with the ground station
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Benjamin Knecht <bknecht@student.ethz.ch>
* @author Christian Schluchter <schluchc@ee.ethz.ch>
*/
// FIXME: This file is a work in progress
#include "MockLinkMissionItemHandler.h"
#include <cmath>
#include "QGC.h"
#include <QDebug>
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<double>(pos.lat)/1E7;
float y = static_cast<double>(pos.lon)/1E7;
//float z = static_cast<double>(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<uint16_t>(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<uint16_t>(waypoints_receive_buffer->size()) - 1;
}
// switch the waypoints list
std::vector<mavlink_mission_item_t*>* 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
src/qgcunittest/MockLinkMissionItemHandler.h 0 → 100644
View file @ 765222eb
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
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 <http://www.gnu.org/licenses/>.
======================================================================*/
#ifndef MOCKLINKMISSIONITEMHANDLER_H
#define MOCKLINKMISSIONITEMHANDLER_H
// FIXME: This file is a work in progress
#include <QObject>
#include <vector>
#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<mavlink_mission_item_t*> waypoints1; ///< vector1 that holds the waypoints
std::vector<mavlink_mission_item_t*> waypoints2; ///< vector2 that holds the waypoints
std::vector<mavlink_mission_item_t*>* waypoints; ///< pointer to the currently active waypoint vector
std::vector<mavlink_mission_item_t*>* 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<mavlink_mission_item_t> _missionItems; ///< Current set of mission itemss
};
#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H
src/ui/CommConfigurationWindow.cc
View file @ 765222eb
......@@ -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,11 +88,16 @@ 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<MAVLinkSimulationLink*>(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);
#endif
......@@ -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<UDPLink*>(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<TCPLink*>(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<MAVLinkSimulationLink*>(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<MockLink*>(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<OpalLink*>(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<XbeeLink*>(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
default:
#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();
......
src/ui/CommConfigurationWindow.h
View file @ 765222eb
......@@ -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
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment