Skip to content

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

Add MissionManager unit test 

- fix bugs found by test
- Implemented Mock mission item protocol in
parent 5357b349
Hide whitespace changes
Inline Side-by-side
Showing with 484 additions and 1270 deletions
QGCApplication.pro
View file @ e1ed35c3
......@@ -515,6 +515,7 @@ HEADERS += \
src/FactSystem/FactSystemTestGeneric.h \
src/FactSystem/FactSystemTestPX4.h \
src/MissionItemTest.h \
src/MissionManager/MissionManagerTest.h \
src/qgcunittest/FileDialogTest.h \
src/qgcunittest/FileManagerTest.h \
src/qgcunittest/FlightGearTest.h \
......@@ -534,6 +535,7 @@ SOURCES += \
src/FactSystem/FactSystemTestGeneric.cc \
src/FactSystem/FactSystemTestPX4.cc \
src/MissionItemTest.cc \
src/MissionManager/MissionManagerTest.cc \
src/qgcunittest/FileDialogTest.cc \
src/qgcunittest/FileManagerTest.cc \
src/qgcunittest/FlightGearTest.cc \
......
src/MissionManager/MissionManager.cc
View file @ e1ed35c3
......@@ -79,11 +79,12 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionRequest, message);
emit inProgressChanged(true);
}
void MissionManager::requestMissionItems(void)
{
qCDebug(MissionManagerLog) << "_requestMissionItems";
qCDebug(MissionManagerLog) << "requestMissionItems read sequence";
mavlink_message_t message;
mavlink_mission_request_list_t request;
......@@ -97,6 +98,7 @@ void MissionManager::requestMissionItems(void)
_vehicle->sendMessage(message);
_startAckTimeout(AckMissionCount, message);
emit inProgressChanged(true);
}
void MissionManager::_ackTimeout(void)
......@@ -157,6 +159,7 @@ void MissionManager::_sendTransactionComplete(void)
_vehicle->sendMessage(message);
emit newMissionItemsAvailable();
emit inProgressChanged(false);
}
void MissionManager::_handleMissionCount(const mavlink_message_t& message)
......@@ -174,6 +177,7 @@ void MissionManager::_handleMissionCount(const mavlink_message_t& message)
if (_cMissionItems == 0) {
emit newMissionItemsAvailable();
emit inProgressChanged(false);
} else {
_requestNextMissionItem(0);
}
......@@ -226,7 +230,7 @@ void MissionManager::_handleMissionItem(const mavlink_message_t& message)
missionItem.param1,
missionItem.param2,
missionItem.param3,
missionItem.param3,
missionItem.param4,
missionItem.autocontinue,
missionItem.current,
missionItem.frame);
......@@ -308,8 +312,11 @@ void MissionManager::_handleMissionAck(const mavlink_message_t& message)
if (missionAck.type == MAV_MISSION_ACCEPTED) {
qCDebug(MissionManagerLog) << "_handleMissionAck write sequence complete";
} else {
_missionItems.clear();
emit newMissionItemsAvailable();
qCDebug(MissionManagerLog) << "_handleMissionAck ack error:" << missionAck.type;
}
emit inProgressChanged(false);
}
/// Called when a new mavlink message for out vehicle is received
......
src/MissionManager/MissionManager.h
View file @ e1ed35c3
......@@ -47,7 +47,7 @@ public:
MissionManager(Vehicle* vehicle);
~MissionManager();
Q_PROPERTY(bool inProgress READ inProgress CONSTANT)
Q_PROPERTY(bool inProgress READ inProgress NOTIFY inProgressChanged)
Q_PROPERTY(QmlObjectListModel* missionItems READ missionItems CONSTANT)
Q_PROPERTY(bool canEdit READ canEdit NOTIFY canEditChanged)
......@@ -72,6 +72,7 @@ signals:
// Public signals
void canEditChanged(bool canEdit);
void newMissionItemsAvailable(void);
void inProgressChanged(bool inProgress);
private slots:
void _mavlinkMessageReceived(const mavlink_message_t& message);
......
src/MissionManager/MissionManagerTest.cc 0 → 100644
View file @ e1ed35c3
/*=====================================================================
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 "MissionManagerTest.h"
#include "LinkManager.h"
#include "MultiVehicleManager.h"
UT_REGISTER_TEST(MissionManagerTest)
const MissionManagerTest::TestCase_t MissionManagerTest::_rgTestCases[] = {
{ "1\t0\t3\t16\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_WAYPOINT, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t17\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_UNLIM, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t18\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TURNS, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t19\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LOITER_TIME, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t21\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_LAND, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t3\t22\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_NAV_TAKEOFF, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT } },
{ "1\t0\t2\t112\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_CONDITION_DELAY, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } },
{ "1\t0\t2\t177\t10\t20\t30\t40\t-10\t-20\t-30\t1\r\n", { 1, QGeoCoordinate(-10.0, -20.0, -30.0), MAV_CMD_DO_JUMP, 10.0, 20.0, 30.0, 40.0, true, false, MAV_FRAME_MISSION } },
};
MissionManagerTest::MissionManagerTest(void)
: _mockLink(NULL)
{
}
void MissionManagerTest::init(void)
{
UnitTest::init();
LinkManager* linkMgr = LinkManager::instance();
Q_CHECK_PTR(linkMgr);
_mockLink = new MockLink();
Q_CHECK_PTR(_mockLink);
LinkManager::instance()->_addLink(_mockLink);
linkMgr->connectLink(_mockLink);
// Wait for the Vehicle to work it's way through the various threads
QSignalSpy spyVehicle(MultiVehicleManager::instance(), SIGNAL(activeVehicleChanged(Vehicle*)));
QCOMPARE(spyVehicle.wait(5000), true);
// Wait for the Mission Manager to finish it's initial load
_missionManager = MultiVehicleManager::instance()->activeVehicle()->missionManager();
QVERIFY(_missionManager);
_rgSignals[canEditChangedSignalIndex] = SIGNAL(canEditChanged(bool));
_rgSignals[newMissionItemsAvailableSignalIndex] = SIGNAL(newMissionItemsAvailable(void));
_rgSignals[inProgressChangedSignalIndex] = SIGNAL(inProgressChanged(bool));
_multiSpy = new MultiSignalSpy();
Q_CHECK_PTR(_multiSpy);
QCOMPARE(_multiSpy->init(_missionManager, _rgSignals, _cSignals), true);
if (_missionManager->inProgress()) {
_multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000);
_multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000);
QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalIndex), true);
}
QVERIFY(!_missionManager->inProgress());
QCOMPARE(_missionManager->missionItems()->count(), 0);
_multiSpy->clearAllSignals();
}
void MissionManagerTest::cleanup(void)
{
delete _multiSpy;
_multiSpy = NULL;
LinkManager::instance()->disconnectLink(_mockLink);
_mockLink = NULL;
QTest::qWait(1000); // Need to allow signals to move between threads
UnitTest::cleanup();
}
/// Checks the state of the inProgress value and signal to match the specified value
void MissionManagerTest::_checkInProgressValues(bool inProgress)
{
QCOMPARE(_missionManager->inProgress(), inProgress);
QSignalSpy* spy = _multiSpy->getSpyByIndex(inProgressChangedSignalIndex);
QList<QVariant> signalArgs = spy->takeFirst();
QCOMPARE(signalArgs.count(), 1);
QCOMPARE(signalArgs[0].toBool(), inProgress);
}
void MissionManagerTest::_readEmptyVehicle(void)
{
_missionManager->requestMissionItems();
// requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress());
QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
_checkInProgressValues(true);
_multiSpy->clearAllSignals();
// Now wait for read sequence to complete. We should get both a newMissionItemsAvailable and a
// inProgressChanged signal to signal completion.
_multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000);
_multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000);
QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true);
_checkInProgressValues(false);
// Vehicle should have no items at this point
QCOMPARE(_missionManager->missionItems()->count(), 0);
QCOMPARE(_missionManager->canEdit(), true);
}
void MissionManagerTest::_roundTripItems(void)
{
// Setup our test case data
const size_t cTestCases = sizeof(_rgTestCases)/sizeof(_rgTestCases[0]);
QmlObjectListModel* list = new QmlObjectListModel();
for (size_t i=0; i<cTestCases; i++) {
const TestCase_t* testCase = &_rgTestCases[i];
MissionItem* item = new MissionItem(list);
QTextStream loadStream(testCase->itemStream, QIODevice::ReadOnly);
QVERIFY(item->load(loadStream));
list->append(item);
}
// Send the items to the vehicle
_missionManager->writeMissionItems(*list);
// writeMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress());
QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
_checkInProgressValues(true);
_multiSpy->clearAllSignals();
// Now wait for write sequence to complete. We should only get an inProgressChanged signal to signal completion.
_multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000);
QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
_checkInProgressValues(false);
QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
QCOMPARE(_missionManager->canEdit(), true);
delete list;
list = NULL;
_multiSpy->clearAllSignals();
// Read the items back from the vehicle
_missionManager->requestMissionItems();
// requestMissionItems should emit inProgressChanged signal before returning so no need to wait for it
QVERIFY(_missionManager->inProgress());
QCOMPARE(_multiSpy->checkOnlySignalByMask(inProgressChangedSignalMask), true);
_checkInProgressValues(true);
_multiSpy->clearAllSignals();
// Now wait for read sequence to complete. We should get both a newMissionItemsAvailable and a
// inProgressChanged signal to signal completion.
_multiSpy->waitForSignalByIndex(newMissionItemsAvailableSignalIndex, 1000);
_multiSpy->waitForSignalByIndex(inProgressChangedSignalIndex, 1000);
QCOMPARE(_multiSpy->checkSignalByMask(newMissionItemsAvailableSignalMask | inProgressChangedSignalMask), true);
QCOMPARE(_multiSpy->checkNoSignalByMask(canEditChangedSignalMask), true);
_checkInProgressValues(false);
QCOMPARE(_missionManager->missionItems()->count(), (int)cTestCases);
QCOMPARE(_missionManager->canEdit(), true);
// Validate the returned items against our test data
for (size_t i=0; i<cTestCases; i++) {
const TestCase_t* testCase = &_rgTestCases[i];
MissionItem* actual = qobject_cast<MissionItem*>(_missionManager->missionItems()->get(i));
qDebug() << "Test case" << i;
QCOMPARE(actual->coordinate().latitude(), testCase->expectedItem.coordinate.latitude());
QCOMPARE(actual->coordinate().longitude(), testCase->expectedItem.coordinate.longitude());
QCOMPARE(actual->coordinate().altitude(), testCase->expectedItem.coordinate.altitude());
QCOMPARE((int)actual->command(), (int)testCase->expectedItem.command);
QCOMPARE(actual->param1(), testCase->expectedItem.param1);
QCOMPARE(actual->param2(), testCase->expectedItem.param2);
QCOMPARE(actual->param3(), testCase->expectedItem.param3);
QCOMPARE(actual->param4(), testCase->expectedItem.param4);
QCOMPARE(actual->autoContinue(), testCase->expectedItem.autocontinue);
QCOMPARE(actual->frame(), testCase->expectedItem.frame);
}
}
src/MissionManager/MissionManagerTest.h 0 → 100644
View file @ e1ed35c3
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2015 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 MissionManagerTest_H
#define MissionManagerTest_H
#include "UnitTest.h"
#include "MockLink.h"
#include "MissionManager.h"
#include "MultiSignalSpy.h"
class MissionManagerTest : public UnitTest
{
Q_OBJECT
public:
MissionManagerTest(void);
private slots:
void init(void);
void cleanup(void);
void _readEmptyVehicle(void);
void _roundTripItems(void);
private:
void _checkInProgressValues(bool inProgress);
MockLink* _mockLink;
MissionManager* _missionManager;
enum {
canEditChangedSignalIndex = 0,
newMissionItemsAvailableSignalIndex,
inProgressChangedSignalIndex,
maxSignalIndex
};
enum {
canEditChangedSignalMask = 1 << canEditChangedSignalIndex,
newMissionItemsAvailableSignalMask = 1 << newMissionItemsAvailableSignalIndex,
inProgressChangedSignalMask = 1 << inProgressChangedSignalIndex,
};
MultiSignalSpy* _multiSpy;
static const size_t _cSignals = maxSignalIndex;
const char* _rgSignals[_cSignals];
typedef struct {
int sequenceNumber;
QGeoCoordinate coordinate;
int command;
double param1;
double param2;
double param3;
double param4;
bool autocontinue;
bool isCurrentItem;
int frame;
} ItemInfo_t;
typedef struct {
const char* itemStream;
const ItemInfo_t expectedItem;
} TestCase_t;
static const TestCase_t _rgTestCases[];
};
#endif
src/comm/MockLink.cc
View file @ e1ed35c3
......@@ -88,7 +88,7 @@ MockLink::MockLink(MockConfiguration* config) :
_fileServer = new MockLinkFileServer(_vehicleSystemId, _vehicleComponentId, this);
Q_CHECK_PTR(_fileServer);
_missionItemHandler = new MockLinkMissionItemHandler(_vehicleSystemId, this);
_missionItemHandler = new MockLinkMissionItemHandler(this);
Q_CHECK_PTR(_missionItemHandler);
moveToThread(this);
......@@ -302,9 +302,11 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
if (!mavlink_parse_char(getMavlinkChannel(), bytes[i], &msg, &comm)) {
continue;
}
Q_ASSERT(_missionItemHandler);
_missionItemHandler->handleMessage(msg);
if (_missionItemHandler->handleMessage(msg)) {
continue;
}
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
......@@ -326,24 +328,6 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
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
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
_handleFTP(msg);
......@@ -627,67 +611,6 @@ void MockLink::_handleParamRequestRead(const mavlink_message_t& msg)
respondWithMavlinkMessage(responseMsg);
}
void MockLink::_handleMissionRequestList(const mavlink_message_t& msg)
{
mavlink_mission_request_list_t request;
mavlink_msg_mission_request_list_decode(&msg, &request);
Q_ASSERT(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
respondWithMavlinkMessage(responseMsg);
}
void MockLink::_handleMissionRequest(const mavlink_message_t& msg)
{
mavlink_mission_request_t request;
mavlink_msg_mission_request_decode(&msg, &request);
Q_ASSERT(request.target_system == _vehicleSystemId);
Q_ASSERT(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);
respondWithMavlinkMessage(responseMsg);
}
void MockLink::_handleMissionItem(const mavlink_message_t& msg)
{
mavlink_mission_item_t request;
mavlink_msg_mission_item_decode(&msg, &request);
Q_ASSERT(request.target_system == _vehicleSystemId);
// FIXME: What do you do with duplication sequence numbers?
Q_ASSERT(!_missionItems.contains(request.seq));
_missionItems[request.seq] = request;
}
void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw)
{
uint16_t chanRaw[18];
......
src/comm/MockLink.h
View file @ e1ed35c3
......@@ -61,14 +61,8 @@ public:
MockLink(MockConfiguration* config = NULL);
~MockLink(void);
// Virtuals from LinkInterface
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 qint64 bytesAvailable(void) { return 0; }
// MockLink methods
int vehicleId(void) { return _vehicleSystemId; }
MAV_AUTOPILOT getAutopilotType(void) { return _autopilotType; }
void setAutopilotType(MAV_AUTOPILOT autopilot) { _autopilotType = autopilot; }
void emitRemoteControlChannelRawChanged(int channel, uint16_t raw);
......@@ -78,6 +72,13 @@ public:
MockLinkFileServer* getFileServer(void) { return _fileServer; }
// Virtuals from LinkInterface
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 qint64 bytesAvailable(void) { return 0; }
// These are left unimplemented in order to cause linker errors which indicate incorrect usage of
// connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager.
bool connect(void);
......@@ -120,9 +121,6 @@ private:
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);
void _handleFTP(const mavlink_message_t& msg);
void _handleCommandLong(const mavlink_message_t& msg);
float _floatUnionForParam(int componentId, const QString& paramName);
......@@ -142,9 +140,6 @@ private:
QMap<int, QMap<QString, QVariant> > _mapParamName2Value;
QMap<QString, MAV_PARAM_TYPE> _mapParamName2MavParamType;
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
MissionList_t _missionItems;
uint8_t _mavBaseMode;
uint32_t _mavCustomMode;
uint8_t _mavState;
......
src/comm/MockLinkMissionItemHandler.cc
View file @ e1ed35c3
......@@ -21,1158 +21,187 @@
======================================================================*/
/**
* @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 "MockLink.h"
#include <cmath>
#include "QGC.h"
#include <QDebug>
QGC_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog, "MockLinkMissionItemHandlerLog")
MockLinkMissionItemHandler::MockLinkMissionItemHandler(uint16_t systemId, QObject* parent) :
QObject(parent),
_vehicleSystemId(systemId)
MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink)
: QObject(mockLink)
, _mockLink(mockLink)
{
Q_ASSERT(mockLink);
}
void MockLinkMissionItemHandler::handleMessage(const mavlink_message_t& msg)
bool 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.
_handleMissionRequestList(msg);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
// Request the specified mission item. Requests should be in order.
_handleMissionRequest(msg);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
_handleMissionItem(msg);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
// Return the current number of mission items
_handleMissionCount(msg);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
// FIXME: Figure out
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_CLEAR_ALL:
// Delete all mission items
_missionItems.clear();
break;
default:
return false;
}
return true;
}
#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
void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message_t& msg)
{
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;
}
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList read sequence";
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];
}
}
mavlink_mission_request_list_t request;
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;
}
mavlink_msg_mission_request_list_decode(&msg, &request);
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;
}
}
Q_ASSERT(request.target_system == _mockLink->vehicleId());
// === 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;
}
mavlink_message_t responseMsg;
friend class PxMatrix3x3;
protected:
float m_vec[3];
};
mavlink_msg_mission_count_pack(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
&responseMsg, // Outgoing message
msg.sysid, // Target is original sender
msg.compid, // Target is original sender
_missionItems.count()); // Number of mission items
_mockLink->respondWithMavlinkMessage(responseMsg);
}
/**
* @brief Pixhawk 3D vector class in double precision, can be cast to a local OpenCV CvMat.
*
*/
class PxVector3Double
void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t& msg)
{
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;
}
}
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequest read sequence";
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];
}
}
mavlink_mission_request_t request;
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];
}
mavlink_msg_mission_request_decode(&msg, &request);
// === 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;
}
Q_ASSERT(request.target_system == _mockLink->vehicleId());
Q_ASSERT(request.seq < _missionItems.count());
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];
}
}
mavlink_message_t responseMsg;
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;
}
mavlink_mission_item_t item = _missionItems[request.seq];
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);
mavlink_msg_mission_item_pack(_mockLink->vehicleId(),
MAV_COMP_ID_MISSIONPLANNER,
&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);
_mockLink->respondWithMavlinkMessage(responseMsg);
}
void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count)
void MockLinkMissionItemHandler::_handleMissionCount(const mavlink_message_t& msg)
{
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);
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionCount write sequence";
mavlink_mission_count_t missionCount;
mavlink_msg_mission_count_decode(&msg, &missionCount);
Q_ASSERT(missionCount.target_system == _mockLink->vehicleId());
_writeSequenceCount = missionCount.count;
Q_ASSERT(_writeSequenceCount >= 0);
// FIXME: Set up a timer for a failed write sequence
_missionItems.clear();
if (_writeSequenceCount == 0) {
// FIXME: NYI
} else {
if (verbose) qDebug("ERROR: index out of bounds\n");
_writeSequenceIndex = 0;
_requestNextMissionItem(_writeSequenceIndex);
}
}
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)
void MockLinkMissionItemHandler::_requestNextMissionItem(int sequenceNumber)
{
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();
qCDebug(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem write sequence sequenceNumber:" << sequenceNumber;
if (sequenceNumber >= _writeSequenceCount) {
qCWarning(MockLinkMissionItemHandlerLog) << "_requestNextMissionItem requested seqeuence number > write count sequenceNumber::_writeSequenceCount" << sequenceNumber << _writeSequenceCount;
return;
}
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);
mavlink_message_t message;
mavlink_mission_request_t missionRequest;
missionRequest.target_system = MAVLinkProtocol::instance()->getSystemId();
missionRequest.target_component = MAVLinkProtocol::instance()->getComponentId();
missionRequest.seq = sequenceNumber;
mavlink_msg_mission_request_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionRequest);
_mockLink->respondWithMavlinkMessage(message);
return (C-A).length();
}
return -1.f;
// FIXME: Timeouts
}
void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg)
{
mavlink_handler(&msg);
}
void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* msg)
void MockLinkMissionItemHandler::_handleMissionItem(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("MissionItem: 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("MissionItem: 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);
}
}
}
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem write sequence";
mavlink_mission_item_t missionItem;
mavlink_msg_mission_item_decode(&msg, &missionItem);
Q_ASSERT(missionItem.target_system == _mockLink->vehicleId());
Q_ASSERT(!_missionItems.contains(missionItem.seq));
Q_ASSERT(missionItem.seq == _writeSequenceIndex);
_missionItems[missionItem.seq] = missionItem;
// FIXME: Timeouts
_writeSequenceIndex++;
if (_writeSequenceIndex < _writeSequenceCount) {
_requestNextMissionItem(_writeSequenceIndex);
} else {
timestamp_lastoutside_orbit = now;
qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionItem sending final ack, write sequence complete";
mavlink_message_t message;
mavlink_mission_ack_t missionAck;
missionAck.target_system = MAVLinkProtocol::instance()->getSystemId();
missionAck.target_component = MAVLinkProtocol::instance()->getComponentId();
missionAck.type = MAV_MISSION_ACCEPTED;
mavlink_msg_mission_ack_encode(_mockLink->vehicleId(), MAV_COMP_ID_MISSIONPLANNER, &message, &missionAck);
_mockLink->respondWithMavlinkMessage(message);
}
}
#endif
\ No newline at end of file
src/comm/MockLinkMissionItemHandler.h
View file @ e1ed35c3
......@@ -21,90 +21,46 @@
======================================================================*/
#ifndef MOCKLINKMISSIONITEMHANDLER_H
#define MOCKLINKMISSIONITEMHANDLER_H
// FIXME: This file is a work in progress
#ifndef MockLinkMissionItemHandler_H
#define MockLinkMissionItemHandler_H
#include <QObject>
#include <vector>
#include <QMap>
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
/* Alreedy defined in MAVLinkSimulationLink.h above!
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 MockLink;
Q_DECLARE_LOGGING_CATEGORY(MockLinkMissionItemHandlerLog)
class MockLinkMissionItemHandler : public QObject
{
Q_OBJECT
public:
MockLinkMissionItemHandler(uint16_t systemId, QObject* parent = NULL);
MockLinkMissionItemHandler(MockLink* mockLink);
/// @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
/// @return true: message handled
bool handleMessage(const mavlink_message_t& msg);
private:
uint16_t _vehicleSystemId; ///< System id of this vehicle
QList<mavlink_mission_item_t> _missionItems; ///< Current set of mission itemss
void _handleMissionRequestList(const mavlink_message_t& msg);
void _handleMissionRequest(const mavlink_message_t& msg);
void _handleMissionItem(const mavlink_message_t& msg);
void _handleMissionCount(const mavlink_message_t& msg);
void _requestNextMissionItem(int sequenceNumber);
private:
MockLink* _mockLink;
int _writeSequenceCount; ///< Numbers of items about to be written
int _writeSequenceIndex; ///< Current index being reqested
typedef QMap<uint16_t, mavlink_mission_item_t> MissionList_t;
MissionList_t _missionItems;
};
#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H
#endif
src/qgcunittest/MultiSignalSpy.cc
View file @ e1ed35c3
......@@ -223,10 +223,7 @@ bool MultiSignalSpy::waitForSignalByIndex(
Q_ASSERT(spy);
while (spy->count() == 0 && !_timeout) {
QCoreApplication::sendPostedEvents();
QCoreApplication::processEvents();
QCoreApplication::flush();
QTest::qSleep(100);
QTest::qWait(100);
}
// Clean up and return status
......
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