MissionManager.h 6.23 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

Don Gagne's avatar
Don Gagne committed
10 11 12 13 14 15 16 17 18 19

#ifndef MissionManager_H
#define MissionManager_H

#include <QObject>
#include <QLoggingCategory>
#include <QThread>
#include <QMutex>
#include <QTimer>

20
#include "MissionItem.h"
Don Gagne's avatar
Don Gagne committed
21 22
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
23
#include "LinkInterface.h"
Don Gagne's avatar
Don Gagne committed
24 25 26 27 28

class Vehicle;

Q_DECLARE_LOGGING_CATEGORY(MissionManagerLog)

29
class MissionManager : public QObject
Don Gagne's avatar
Don Gagne committed
30 31 32 33 34 35 36
{
    Q_OBJECT
    
public:
    MissionManager(Vehicle* vehicle);
    ~MissionManager();
    
37
    bool inProgress(void);
38
    const QList<MissionItem*>& missionItems(void) { return _missionItems; }
39 40

    /// Current mission item as reported by MISSION_CURRENT
41
    int currentIndex(void) const { return _currentMissionIndex; }
42 43

    /// Last current mission item reported while in Mission flight mode
44
    int lastCurrentIndex(void) const { return _lastCurrentIndex; }
Don Gagne's avatar
Don Gagne committed
45
    
DonLakeFlyer's avatar
DonLakeFlyer committed
46 47 48
    /// Load the mission items from the vehicle
    ///     Signals newMissionItemsAvailable when done
    void loadFromVehicle(void);
Don Gagne's avatar
Don Gagne committed
49 50
    
    /// Writes the specified set of mission items to the vehicle
51
    ///     @param missionItems Items to send to vehicle
DonLakeFlyer's avatar
DonLakeFlyer committed
52
    ///     Signals sendComplete when done
53
    void writeMissionItems(const QList<MissionItem*>& missionItems);
54
    
Don Gagne's avatar
Don Gagne committed
55 56 57 58 59
    /// Writes the specified set mission items to the vehicle as an ArduPilot guided mode mission item.
    ///     @param gotoCoord Coordinate to move to
    ///     @param altChangeOnly true: only altitude change, false: lat/lon/alt change
    void writeArduPilotGuidedMissionItem(const QGeoCoordinate& gotoCoord, bool altChangeOnly);

60
    /// Removes all mission items from vehicle
DonLakeFlyer's avatar
DonLakeFlyer committed
61
    ///     Signals removeAllComplete when done
62 63
    void removeAll(void);

64 65 66 67
    /// Generates a new mission which starts from the specified index. It will include all the CMD_DO items
    /// from mission start to resumeIndex in the generate mission.
    void generateResumeMission(int resumeIndex);

68 69 70 71 72 73 74 75 76
    /// Error codes returned in error signal
    typedef enum {
        InternalError,
        AckTimeoutError,        ///< Timed out waiting for response from vehicle
        ProtocolOrderError,     ///< Incorrect protocol sequence from vehicle
        RequestRangeError,      ///< Vehicle requested item out of range
        ItemMismatchError,      ///< Vehicle returned item with seq # different than requested
        VehicleError,           ///< Vehicle returned error
        MissingRequestsError,   ///< Vehicle did not request all items during write sequence
77
        MaxRetryExceeded,       ///< Retry failed
78
    } ErrorCode_t;
Don Gagne's avatar
Don Gagne committed
79

Don Gagne's avatar
Don Gagne committed
80
    // These values are public so the unit test can set appropriate signal wait times
81
    static const int _ackTimeoutMilliseconds = 1000;
Don Gagne's avatar
Don Gagne committed
82 83
    static const int _maxRetryCount = 5;
    
Don Gagne's avatar
Don Gagne committed
84
signals:
85
    void newMissionItemsAvailable(bool removeAllRequested);
Don Gagne's avatar
Don Gagne committed
86
    void inProgressChanged(bool inProgress);
87
    void error(int errorCode, const QString& errorMsg);
88 89
    void currentIndexChanged(int currentIndex);
    void lastCurrentIndexChanged(int lastCurrentIndex);
90
    void resumeMissionReady(void);
91
    void progressPct(double progressPercentPct);
92 93
    void removeAllComplete              (bool error);
    void sendComplete                   (bool error);
94

Don Gagne's avatar
Don Gagne committed
95 96 97 98 99 100 101 102 103 104
private slots:
    void _mavlinkMessageReceived(const mavlink_message_t& message);
    void _ackTimeout(void);
    
private:
    typedef enum {
        AckNone,            ///< State machine is idle
        AckMissionCount,    ///< MISSION_COUNT message expected
        AckMissionItem,     ///< MISSION_ITEM expected
        AckMissionRequest,  ///< MISSION_REQUEST is expected, or MISSION_ACK to end sequence
105
        AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
Ricardo de Almeida Gonzaga's avatar
Ricardo de Almeida Gonzaga committed
106
        AckGuidedItem,      ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
Don Gagne's avatar
Don Gagne committed
107
    } AckType_t;
108 109 110 111 112

    typedef enum {
        TransactionNone,
        TransactionRead,
        TransactionWrite,
DonLakeFlyer's avatar
DonLakeFlyer committed
113
        TransactionRemoveAll
114 115
    } TransactionType_t;

116
    void _startAckTimeout(AckType_t ack);
117
    bool _checkForExpectedAck(AckType_t receivedAck);
118
    void _readTransactionComplete(void);
Don Gagne's avatar
Don Gagne committed
119
    void _handleMissionCount(const mavlink_message_t& message);
120 121
    void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
    void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
Don Gagne's avatar
Don Gagne committed
122
    void _handleMissionAck(const mavlink_message_t& message);
123
    void _handleMissionCurrent(const mavlink_message_t& message);
124
    void _requestNextMissionItem(void);
Don Gagne's avatar
Don Gagne committed
125
    void _clearMissionItems(void);
126
    void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
Don Gagne's avatar
Don Gagne committed
127
    QString _ackTypeToString(AckType_t ackType);
Don Gagne's avatar
Don Gagne committed
128
    QString _missionResultToString(MAV_MISSION_RESULT result);
129
    void _finishTransaction(bool success);
130 131
    void _requestList(void);
    void _writeMissionCount(void);
132 133
    void _writeMissionItemsWorker(void);
    void _clearAndDeleteMissionItems(void);
134
    void _clearAndDeleteWriteMissionItems(void);
Don Gagne's avatar
Don Gagne committed
135
    QString _lastMissionReqestString(MAV_MISSION_RESULT result);
136
    void _removeAllWorker(void);
Don Gagne's avatar
Don Gagne committed
137 138 139

private:
    Vehicle*            _vehicle;
140
    LinkInterface*      _dedicatedLink;
Don Gagne's avatar
Don Gagne committed
141 142
    
    QTimer*             _ackTimeoutTimer;
143 144
    AckType_t           _expectedAck;
    int                 _retryCount;
Don Gagne's avatar
Don Gagne committed
145
    
146 147 148 149 150
    TransactionType_t   _transactionInProgress;
    bool                _resumeMission;
    QList<int>          _itemIndicesToWrite;    ///< List of mission items which still need to be written to vehicle
    QList<int>          _itemIndicesToRead;     ///< List of mission items which still need to be requested from vehicle
    int                 _lastMissionRequest;    ///< Index of item last requested by MISSION_REQUEST
Don Gagne's avatar
Don Gagne committed
151 152 153
    
    QMutex _dataMutex;
    
154 155
    QList<MissionItem*> _missionItems;          ///< Set of mission items on vehicle
    QList<MissionItem*> _writeMissionItems;     ///< Set of mission items currently being written to vehicle
156 157
    int                 _currentMissionIndex;
    int                 _lastCurrentIndex;
Don Gagne's avatar
Don Gagne committed
158 159
};

160
#endif