Add MissionManager unit test

- fix bugs found by test
- Implemented Mock mission item protocol in

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 \

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

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);

src/MissionManager/MissionManagerTest.h

+/*=====================================================================
+ 
+ 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

@@ -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

@@ -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.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 <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

@@ -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