MissionManager.h 4.49 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 37
{
    Q_OBJECT
    
public:
    /// @param uas Uas which this set of facts is associated with
    MissionManager(Vehicle* vehicle);
    ~MissionManager();
    
38
    bool inProgress(void);
39
    const QList<MissionItem*>& missionItems(void) { return _missionItems; }
40
    int currentItem(void) { return _currentMissionItem; }
Don Gagne's avatar
Don Gagne committed
41
    
Don Gagne's avatar
Don Gagne committed
42 43 44
    void requestMissionItems(void);
    
    /// Writes the specified set of mission items to the vehicle
45 46
    ///     @param missionItems Items to send to vehicle
    void writeMissionItems(const QList<MissionItem*>& missionItems);
47
    
Don Gagne's avatar
Don Gagne committed
48 49 50 51 52
    /// 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);

53 54 55 56 57 58 59 60 61
    /// 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
62
        MaxRetryExceeded,       ///< Retry failed
63
    } ErrorCode_t;
Don Gagne's avatar
Don Gagne committed
64

Don Gagne's avatar
Don Gagne committed
65 66 67 68
    // These values are public so the unit test can set appropriate signal wait times
    static const int _ackTimeoutMilliseconds= 2000;
    static const int _maxRetryCount = 5;
    
Don Gagne's avatar
Don Gagne committed
69 70
signals:
    void newMissionItemsAvailable(void);
Don Gagne's avatar
Don Gagne committed
71
    void inProgressChanged(bool inProgress);
72
    void error(int errorCode, const QString& errorMsg);
73
    void currentItemChanged(int currentItem);
Don Gagne's avatar
Don Gagne committed
74
    
Don Gagne's avatar
Don Gagne committed
75 76 77 78 79 80 81 82 83 84
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
Don Gagne's avatar
Don Gagne committed
85
        AckGuidedItem,      ///< MISSION_ACK expected in reponse to ArduPilot guided mode single item send
Don Gagne's avatar
Don Gagne committed
86 87
    } AckType_t;
    
88
    void _startAckTimeout(AckType_t ack);
Don Gagne's avatar
Don Gagne committed
89
    bool _stopAckTimeout(AckType_t expectedAck);
90
    void _readTransactionComplete(void);
Don Gagne's avatar
Don Gagne committed
91 92 93 94
    void _handleMissionCount(const mavlink_message_t& message);
    void _handleMissionItem(const mavlink_message_t& message);
    void _handleMissionRequest(const mavlink_message_t& message);
    void _handleMissionAck(const mavlink_message_t& message);
95
    void _handleMissionCurrent(const mavlink_message_t& message);
96
    void _requestNextMissionItem(void);
Don Gagne's avatar
Don Gagne committed
97
    void _clearMissionItems(void);
98
    void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
Don Gagne's avatar
Don Gagne committed
99
    QString _ackTypeToString(AckType_t ackType);
Don Gagne's avatar
Don Gagne committed
100
    QString _missionResultToString(MAV_MISSION_RESULT result);
101
    void _finishTransaction(bool success);
Don Gagne's avatar
Don Gagne committed
102 103 104

private:
    Vehicle*            _vehicle;
105
    LinkInterface*      _dedicatedLink;
Don Gagne's avatar
Don Gagne committed
106 107 108
    
    QTimer*             _ackTimeoutTimer;
    AckType_t           _retryAck;
109
    int                 _requestItemRetryCount;
Don Gagne's avatar
Don Gagne committed
110
    
111 112 113 114
    bool        _readTransactionInProgress;
    bool        _writeTransactionInProgress;
    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
Don Gagne's avatar
Don Gagne committed
115 116 117
    
    QMutex _dataMutex;
    
118
    QList<MissionItem*> _missionItems;
119
    int                 _currentMissionItem;
Don Gagne's avatar
Don Gagne committed
120 121
};

122
#endif