diff --git a/QGCApplication.pro b/QGCApplication.pro index 393ca86c13a43b85d4c523b0fab7da2073ffdfaa..fb0c0999f7dffc98590331cd86c2718a1d1a7cfb 100644 --- a/QGCApplication.pro +++ b/QGCApplication.pro @@ -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 \ diff --git a/src/MissionManager/MissionManager.cc b/src/MissionManager/MissionManager.cc index 6e0255e012914a3428d231dc413375953e0f6d36..51d71675c342e019bc959f13a24d2ab4579f302f 100644 --- a/src/MissionManager/MissionManager.cc +++ b/src/MissionManager/MissionManager.cc @@ -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 diff --git a/src/MissionManager/MissionManager.h b/src/MissionManager/MissionManager.h index f9e57d3cee6ef42acf57f6d884e8bd5c27f8459f..d039f463086b8f2e5eec0c08cba3028394c06140 100644 --- a/src/MissionManager/MissionManager.h +++ b/src/MissionManager/MissionManager.h @@ -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); diff --git a/src/MissionManager/MissionManagerTest.cc b/src/MissionManager/MissionManagerTest.cc new file mode 100644 index 0000000000000000000000000000000000000000..991d674ae8eb8e939de7909e4b30c90bb2233035 --- /dev/null +++ b/src/MissionManager/MissionManagerTest.cc @@ -0,0 +1,214 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2014 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "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 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; iitemStream, 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(_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); + } +} diff --git a/src/MissionManager/MissionManagerTest.h b/src/MissionManager/MissionManagerTest.h new file mode 100644 index 0000000000000000000000000000000000000000..22ee3903f2097e3dd40a79b7b31449832b4d063f --- /dev/null +++ b/src/MissionManager/MissionManagerTest.h @@ -0,0 +1,90 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2015 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#ifndef 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 diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 4ad64ba3149c114eee6a5bcf48ea9399225b465f..a847ad7646151a7d495814b18558f368fd44c0d2 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -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]; diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 7ca8f179d7c768f83c192e22b9fa9bf923e23453..6c7950cbc88bcec60899e57e908834d46462f35e 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -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 > _mapParamName2Value; QMap _mapParamName2MavParamType; - typedef QMap MissionList_t; - MissionList_t _missionItems; - uint8_t _mavBaseMode; uint32_t _mavCustomMode; uint8_t _mavState; diff --git a/src/comm/MockLinkMissionItemHandler.cc b/src/comm/MockLinkMissionItemHandler.cc index 6e3a4abd3259698f031e04160b084e3f28f79951..59bf013318baa13e30e8ded63761e89984bc40ed 100644 --- a/src/comm/MockLinkMissionItemHandler.cc +++ b/src/comm/MockLinkMissionItemHandler.cc @@ -21,1158 +21,187 @@ ======================================================================*/ -/** -* @file -* @brief a program to manage waypoints and exchange them with the ground station -* -* @author Petri Tanskanen -* @author Benjamin Knecht -* @author Christian Schluchter -*/ - -// FIXME: This file is a work in progress - #include "MockLinkMissionItemHandler.h" +#include "MockLink.h" -#include - -#include "QGC.h" #include +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(pos.lat)/1E7; - float y = static_cast(pos.lon)/1E7; - //float z = static_cast(pos.alt)/1000; - - //qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z; - - posReached = false; - yawReached = true; - - // FIXME big hack for simulation! - //float oneDegreeOfLatMeters = 111131.745f; - float orbit = 0.00008f; - - // compare current position (given in message) with current waypoint - //float orbit = wp->param1; - - // Convert to degrees - - - float dist; - dist = distanceToPoint(current_active_wp_id, x, y); - - if (dist >= 0.f && dist <= orbit && yawReached) { - posReached = true; - qDebug() << "WP PLANNER: REACHED POSITION"; - } - } - } - break; - } - - case MAVLINK_MSG_ID_COMMAND_LONG: - { // special action from ground station - mavlink_command_long_t action; - mavlink_msg_command_long_decode(msg, &action); - if(action.target_system == systemid) { - if (verbose) qDebug("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(waypoints->size()); - send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); - } - } else { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because not my systemid or compid.\n"); - } - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) { - if (verbose && current_state == PX_WPP_SENDLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - current_state = PX_WPP_SENDLIST_SENDWPS; - protocol_current_wp_id = wpr.seq; - send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); - } else { - if (verbose) { - if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { - qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", current_state); - break; - } else if (current_state == PX_WPP_SENDLIST) { - if (wpr.seq != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } else if (current_state == PX_WPP_SENDLIST_SENDWPS) { - if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); - else if (wpr.seq >= waypoints->size()) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); - } - } - } else { - //we we're target but already communicating with someone else - if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - if(wpc.target_system == systemid && wpc.target_component == compid) { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) { - if (wpc.count > 0) { - if (verbose && current_state == PX_WPP_IDLE) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - current_state = PX_WPP_GETLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - protocol_current_count = wpc.count; - - qDebug("clearing receive buffer and readying for receiving waypoints\n"); - while(waypoints_receive_buffer->size() > 0) { - delete waypoints_receive_buffer->back(); - waypoints_receive_buffer->pop_back(); - } - - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } else { - if (verbose) qDebug("Ignoring MAVLINK_MSG_ID_MISSION_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } else { - if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm doing something else already (state=%i).\n", current_state); - else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_COUNT - FIXME: missed error description\n"); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) { - if (verbose && current_state == PX_WPP_GETLIST) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); - - current_state = PX_WPP_GETLIST_GETWPS; - protocol_current_wp_id = wp.seq + 1; - mavlink_mission_item_t* newwp = new mavlink_mission_item_t; - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); - waypoints_receive_buffer->push_back(newwp); - - if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) { - if (verbose) qDebug("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); - - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - - if (current_active_wp_id > waypoints_receive_buffer->size()-1) { - current_active_wp_id = static_cast(waypoints_receive_buffer->size()) - 1; - } - - // switch the waypoints list - std::vector* waypoints_temp = waypoints; - waypoints = waypoints_receive_buffer; - waypoints_receive_buffer = waypoints_temp; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < waypoints->size(); i++) { - if (waypoints->at(i)->current == 1) { - current_active_wp_id = i; - //if (verbose) qDebug("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == waypoints->size()) { - current_active_wp_id = -1; - yawReached = false; - posReached = false; - timestamp_firstinside_orbit = 0; - } - - current_state = PX_WPP_IDLE; - } else { - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - } else { - if (current_state == PX_WPP_IDLE) { - //we're done receiving waypoints, answer with ack. - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - qDebug("Received MAVLINK_MSG_ID_MISSION_ITEM while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) { - if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { - qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); - break; - } else if (current_state == PX_WPP_GETLIST) { - if(!(wp.seq == 0)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } else if (current_state == PX_WPP_GETLIST_GETWPS) { - if (!(wp.seq == protocol_current_wp_id)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); - else if (!(wp.seq < protocol_current_count)) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } else qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - } else { - // We're target but already communicating with someone else - if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); - } else if(wp.target_system == systemid && wp.target_component == compid) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) { - protocol_timestamp_lastaction = now; - - if (verbose) qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - while(waypoints->size() > 0) { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - } else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) { - if (verbose) qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); - } - break; - } - - default: { - if (debug) qDebug("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 diff --git a/src/comm/MockLinkMissionItemHandler.h b/src/comm/MockLinkMissionItemHandler.h index 801c80e53459f80ca3b8169fb09e5aa3f53cf404..2978b55ccf3fc7ffacdc637016a966a15ad76218 100644 --- a/src/comm/MockLinkMissionItemHandler.h +++ b/src/comm/MockLinkMissionItemHandler.h @@ -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 -#include +#include #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 waypoints1; ///< vector1 that holds the waypoints - std::vector waypoints2; ///< vector2 that holds the waypoints - - std::vector* waypoints; ///< pointer to the currently active waypoint vector - std::vector* waypoints_receive_buffer; ///< pointer to the receive buffer waypoint vector - PX_WAYPOINTPLANNER_STATES current_state; - uint16_t protocol_current_wp_id; - uint16_t protocol_current_count; - uint8_t protocol_current_partner_systemid; - uint8_t protocol_current_partner_compid; - uint64_t protocol_timestamp_lastaction; - unsigned int protocol_timeout; - uint64_t timestamp_last_send_setpoint; - uint8_t systemid; - uint8_t compid; - unsigned int setpointDelay; - float yawTolerance; - bool verbose; - bool debug; - bool silent; - - void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type); - void send_waypoint_current(uint16_t seq); - void send_setpoint(uint16_t seq); - void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count); - void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); - void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); - void send_waypoint_reached(uint16_t seq); - float distanceToSegment(uint16_t seq, float x, float y, float z); - float distanceToPoint(uint16_t seq, float x, float y, float z); - float distanceToPoint(uint16_t seq, float x, float y); - void mavlink_handler(const mavlink_message_t* msg); -#endif + /// @return true: message handled + bool handleMessage(const mavlink_message_t& msg); private: - uint16_t _vehicleSystemId; ///< System id of this vehicle - - QList _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 MissionList_t; + MissionList_t _missionItems; }; -#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H +#endif diff --git a/src/qgcunittest/MultiSignalSpy.cc b/src/qgcunittest/MultiSignalSpy.cc index d9f786f4f4641333ce49f831c26f4d24a7f2082b..9bcc3124c1d3d59894378a18d125b57302b23b1f 100644 --- a/src/qgcunittest/MultiSignalSpy.cc +++ b/src/qgcunittest/MultiSignalSpy.cc @@ -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