PlanManager.h 6.63 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

10
#pragma once
11 12 13 14 15 16 17 18 19 20 21

#include <QObject>
#include <QLoggingCategory>
#include <QTimer>

#include "MissionItem.h"
#include "QGCMAVLink.h"
#include "QGCLoggingCategory.h"
#include "LinkInterface.h"

class Vehicle;
22
class MissionCommandTree;
23 24 25 26 27 28 29 30

Q_DECLARE_LOGGING_CATEGORY(PlanManagerLog)

/// The PlanManager class is the base class for the Mission, GeoFence and Rally Point managers. All of which use the
/// new mavlink v2 mission protocol.
class PlanManager : public QObject
{
    Q_OBJECT
31

32 33 34
public:
    PlanManager(Vehicle* vehicle, MAV_MISSION_TYPE planType);
    ~PlanManager();
35

36 37 38 39 40 41 42 43
    bool inProgress(void) const;
    const QList<MissionItem*>& missionItems(void) { return _missionItems; }

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

    /// Last current mission item reported while in Mission flight mode
    int lastCurrentIndex(void) const { return _lastCurrentIndex; }
44

45 46 47
    /// Load the mission items from the vehicle
    ///     Signals newMissionItemsAvailable when done
    void loadFromVehicle(void);
48

49
    /// Writes the specified set of mission items to the vehicle
50
    /// IMPORTANT NOTE: PlanManager will take control of the MissionItem objects with the missionItems list. It will free them when done.
51 52 53 54 55 56 57 58 59 60 61 62
    ///     @param missionItems Items to send to vehicle
    ///     Signals sendComplete when done
    void writeMissionItems(const QList<MissionItem*>& missionItems);

    /// Removes all mission items from vehicle
    ///     Signals removeAllComplete when done
    void removeAll(void);

    /// Error codes returned in error signal
    typedef enum {
        InternalError,
        AckTimeoutError,        ///< Timed out waiting for response from vehicle
63
        ProtocolError,          ///< Incorrect protocol sequence from vehicle
64 65
        RequestRangeError,      ///< Vehicle requested item out of range
        ItemMismatchError,      ///< Vehicle returned item with seq # different than requested
66
        VehicleAckError,        ///< Vehicle returned error in ack
67 68 69 70 71 72
        MissingRequestsError,   ///< Vehicle did not request all items during write sequence
        MaxRetryExceeded,       ///< Retry failed
        MissionTypeMismatch,    ///< MAV_MISSION_TYPE does not match _planType
    } ErrorCode_t;

    // These values are public so the unit test can set appropriate signal wait times
73
    // When passively waiting for a mission process, use a longer timeout.
74
    static const int _ackTimeoutMilliseconds = 1500;
75 76
    // When actively retrying to request mission items, use a shorter timeout instead.
    static const int _retryTimeoutMilliseconds = 250;
77
    static const int _maxRetryCount = 5;
78

79 80 81 82 83 84 85 86 87 88 89 90 91 92 93
signals:
    void newMissionItemsAvailable   (bool removeAllRequested);
    void inProgressChanged          (bool inProgress);
    void error                      (int errorCode, const QString& errorMsg);
    void currentIndexChanged        (int currentIndex);
    void lastCurrentIndexChanged    (int lastCurrentIndex);
    void progressPct                (double progressPercentPct);
    void removeAllComplete          (bool error);
    void sendComplete               (bool error);
    void resumeMissionReady         (void);
    void resumeMissionUploadFail    (void);

private slots:
    void _mavlinkMessageReceived(const mavlink_message_t& message);
    void _ackTimeout(void);
94

95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123
protected:
    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
        AckMissionClearAll, ///< MISSION_CLEAR_ALL sent, MISSION_ACK is expected
        AckGuidedItem,      ///< MISSION_ACK expected in response to ArduPilot guided mode single item send
    } AckType_t;

    typedef enum {
        TransactionNone,
        TransactionRead,
        TransactionWrite,
        TransactionRemoveAll
    } TransactionType_t;

    void _startAckTimeout(AckType_t ack);
    bool _checkForExpectedAck(AckType_t receivedAck);
    void _readTransactionComplete(void);
    void _handleMissionCount(const mavlink_message_t& message);
    void _handleMissionItem(const mavlink_message_t& message, bool missionItemInt);
    void _handleMissionRequest(const mavlink_message_t& message, bool missionItemInt);
    void _handleMissionAck(const mavlink_message_t& message);
    void _requestNextMissionItem(void);
    void _clearMissionItems(void);
    void _sendError(ErrorCode_t errorCode, const QString& errorMsg);
    QString _ackTypeToString(AckType_t ackType);
    QString _missionResultToString(MAV_MISSION_RESULT result);
124
    void _finishTransaction(bool success, bool apmGuidedItemWrite = false);
125 126 127 128 129 130 131
    void _requestList(void);
    void _writeMissionCount(void);
    void _writeMissionItemsWorker(void);
    void _clearAndDeleteMissionItems(void);
    void _clearAndDeleteWriteMissionItems(void);
    QString _lastMissionReqestString(MAV_MISSION_RESULT result);
    void _removeAllWorker(void);
132 133
    void _connectToMavlink(void);
    void _disconnectFromMavlink(void);
134 135 136
    QString _planTypeString(void);

protected:
137 138
    Vehicle*            _vehicle =              nullptr;
    MissionCommandTree* _missionCommandTree =   nullptr;
139
    MAV_MISSION_TYPE    _planType;
140
    LinkInterface*      _dedicatedLink =        nullptr;
141

142
    QTimer*             _ackTimeoutTimer =      nullptr;
143 144
    AckType_t           _expectedAck;
    int                 _retryCount;
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
151
    int                 _missionItemCountToRead;///< Count of all mission items to read
152

153 154 155 156
    QList<MissionItem*> _missionItems;          ///< Set of mission items on vehicle
    QList<MissionItem*> _writeMissionItems;     ///< Set of mission items currently being written to vehicle
    int                 _currentMissionIndex;
    int                 _lastCurrentIndex;
157 158 159

private:
    void _setTransactionInProgress(TransactionType_t type);
160
};