UASWaypointManager.h 11.7 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2
/*=====================================================================

3
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed
4

5
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
6

7
This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk committed
8

9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
10 11 12 13
    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.

14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15 16 17 18 19
    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
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
pixhawk's avatar
pixhawk committed
21 22 23 24 25 26 27 28 29 30 31

======================================================================*/

/**
 * @file
 *   @brief Definition of the waypoint protocol handler
 *
 *   @author Petri Tanskanen <mavteam@student.ethz.ch>
 *
 */

32 33 34 35
#ifndef UASWAYPOINTMANAGER_H
#define UASWAYPOINTMANAGER_H

#include <QObject>
36
#include <QList>
pixhawk's avatar
pixhawk committed
37
#include <QTimer>
38
#include "Waypoint.h"
pixhawk's avatar
pixhawk committed
39
#include "QGCMAVLink.h"
40
class UAS;
41
class UASInterface;
42

43 44 45 46
/**
 * @brief Implementation of the MAVLINK waypoint protocol
 *
 * This class handles the communication with a waypoint manager on the MAV.
47
 * All waypoints are stored in the QList waypoints, modifications can be done with the WaypointList widget.
48 49 50 51
 * Notice that currently the access to the internal waypoint storage is not guarded nor thread-safe. This works as long as no other widget alters the data.
 *
 * See http://qgroundcontrol.org/waypoint_protocol for more information about the protocol and the states.
 */
52 53
class UASWaypointManager : public QObject
{
54
    Q_OBJECT
55 56
private:
    enum WaypointState {
57 58 59
        WP_IDLE = 0,        ///< Waiting for commands
        WP_SENDLIST,        ///< Initial state for sending waypoints to the MAV
        WP_SENDLIST_SENDWPS,///< Sending waypoints
60
        WP_GETLIST,         ///< Initial state for retrieving waypoints from the MAV
61
        WP_GETLIST_GETWPS,  ///< Receiving waypoints
62 63
        WP_CLEARLIST,       ///< Clearing waypoint list on the MAV
        WP_SETCURRENT       ///< Setting new current waypoint on the MAV
64 65 66
    }; ///< The possible states for the waypoint protocol

public:
lm's avatar
lm committed
67
    UASWaypointManager(UAS* uas=NULL);   ///< Standard constructor
LM's avatar
LM committed
68
    ~UASWaypointManager();
69
    bool guidedModeSupported();
70

71
    void goToWaypoint(Waypoint *wp);
pixhawk's avatar
pixhawk committed
72
    /** @name Received message handlers */
73 74
    /*@{*/
    void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);                            ///< Handles received waypoint count messages
lm's avatar
lm committed
75 76 77 78 79
    void handleWaypoint(quint8 systemId, quint8 compId, mavlink_mission_item_t *wp);                        ///< Handles received waypoint messages
    void handleWaypointAck(quint8 systemId, quint8 compId, mavlink_mission_ack_t *wpa);                ///< Handles received waypoint ack messages
    void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_mission_request_t *wpr);        ///< Handles received waypoint request messages
    void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_mission_item_reached_t *wpr);        ///< Handles received waypoint reached messages
    void handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_mission_current_t *wpc);        ///< Handles received set current waypoint messages
80
    /*@}*/
81

82 83 84
    /** @name Remote operations */
    /*@{*/
    void clearWaypointList();                       ///< Sends the waypoint clear all message to the MAV
pixhawk's avatar
pixhawk committed
85 86

    void readWaypoints(bool read_to_edit=false);    ///< Requests the MAV's current waypoint list.
87
    void writeWaypoints();                          ///< Sends the waypoint list to the MAV
88 89
    int setCurrentWaypoint(quint16 seq);            ///< Sends the sequence number of the waypoint that should get the new target waypoint to the UAS
    int setCurrentEditable(quint16 seq);          ///< Changes the current waypoint in edit tab
90 91
    /*@}*/

92
    /** @name Waypoint list operations */
93
    /*@{*/
94
    const QList<Waypoint *> &getWaypointEditableList(void) {
pixhawk's avatar
pixhawk committed
95 96
        return waypointsEditable;    ///< Returns a const reference to the waypoint list.
    }
97
    const QList<Waypoint *> &getWaypointViewOnlyList(void) {
pixhawk's avatar
pixhawk committed
98
        return waypointsViewOnly;    ///< Returns a const reference to the waypoint list.
99
    }
100 101 102
    const QList<Waypoint *> getGlobalFrameWaypointList();  ///< Returns a global waypoint list
    const QList<Waypoint *> getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
    const QList<Waypoint *> getNavTypeWaypointList(); ///< Returns a waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out.
103 104
    int getIndexOf(Waypoint* wp);                   ///< Get the index of a waypoint in the list
    int getGlobalFrameIndexOf(Waypoint* wp);    ///< Get the index of a waypoint in the list, counting only global waypoints
lm's avatar
lm committed
105
    int getGlobalFrameAndNavTypeIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only global AND navigation mode waypoints
LM's avatar
LM committed
106
    int getNavTypeIndexOf(Waypoint* wp); ///< Get the index of a waypoint in the list, counting only navigation mode waypoints
107 108 109
    int getLocalFrameIndexOf(Waypoint* wp);     ///< Get the index of a waypoint in the list, counting only local waypoints
    int getMissionFrameIndexOf(Waypoint* wp);   ///< Get the index of a waypoint in the list, counting only mission waypoints
    int getGlobalFrameCount(); ///< Get the count of global waypoints in the list
lm's avatar
lm committed
110
    int getGlobalFrameAndNavTypeCount(); ///< Get the count of global waypoints in navigation mode in the list
LM's avatar
LM committed
111
    int getNavTypeCount(); ///< Get the count of global waypoints in navigation mode in the list
112
    int getLocalFrameCount();   ///< Get the count of local waypoints in the list
113
    /*@}*/
114

115 116 117 118 119
    UAS* getUAS();
    float getAltitudeRecommendation();
    int getFrameRecommendation();
    float getAcceptanceRadiusRecommendation();
    float getHomeAltitudeOffsetDefault();
120

121
private:
pixhawk's avatar
pixhawk committed
122 123
    /** @name Message send functions */
    /*@{*/
124 125 126 127
    void sendWaypointClearAll();
    void sendWaypointSetCurrent(quint16 seq);
    void sendWaypointCount();
    void sendWaypointRequestList();
128 129 130
    void sendWaypointRequest(quint16 seq);          ///< Requests a waypoint with sequence number seq
    void sendWaypoint(quint16 seq);                 ///< Sends a waypoint with sequence number seq
    void sendWaypointAck(quint8 type);              ///< Sends a waypoint ack
pixhawk's avatar
pixhawk committed
131
    /*@}*/
132 133

public slots:
134
    void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
135 136
    /** @name Waypoint list operations */
    /*@{*/
137
    void addWaypointEditable(Waypoint *wp, bool enforceFirstActive=true);                 ///< adds a new waypoint to the end of the editable list and changes its sequence number accordingly
pixhawk's avatar
pixhawk committed
138
    void addWaypointViewOnly(Waypoint *wp);                                               ///< adds a new waypoint to the end of the view-only list and changes its sequence number accordingly
139
    Waypoint* createWaypoint(bool enforceFirstActive=true);     ///< Creates a waypoint
140 141 142 143
    int removeWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
    void moveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
    void saveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
    void loadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
144
    void notifyOfChangeEditable(Waypoint* wp);             ///< Notifies manager to changes to an editable waypoint
145
    void notifyOfChangeViewOnly(Waypoint* wp);             ///< Notifies manager to changes to a viewonly waypoint, e.g. some widget wants to change "current"
146
    /*@}*/
147
    void handleLocalPositionChanged(UASInterface* mav, double x, double y, double z, quint64 time);
148
    void handleGlobalPositionChanged(UASInterface* mav, double lat, double lon, double altAMSL, double altWGS84, quint64 time);
149

pixhawk's avatar
pixhawk committed
150
signals:
151 152 153
    void waypointEditableListChanged(void);                 ///< emits signal that the list of editable waypoints has been changed
    void waypointEditableListChanged(int uasid);            ///< emits signal that the list of editable waypoints has been changed
    void waypointEditableChanged(int uasid, Waypoint* wp);  ///< emits signal that a single editable waypoint has been changed
pixhawk's avatar
pixhawk committed
154 155 156
    void waypointViewOnlyListChanged(void);                 ///< emits signal that the list of editable waypoints has been changed
    void waypointViewOnlyListChanged(int uasid);            ///< emits signal that the list of editable waypoints has been changed
    void waypointViewOnlyChanged(int uasid, Waypoint* wp);  ///< emits signal that a single editable waypoint has been changed
157 158
    void currentWaypointChanged(quint16);           ///< emits the new current waypoint sequence number
    void updateStatusString(const QString &);       ///< emits the current status string
159
    void waypointDistanceChanged(double distance);   ///< Distance to next waypoint changed (in meters)
pixhawk's avatar
pixhawk committed
160

161 162
    void loadWPFile();                              ///< emits signal that a file wp has been load
    void readGlobalWPFromUAS(bool value);           ///< emits signal when finish to read Global WP from UAS
163 164 165 166 167 168 169
    
    void _startProtocolTimer(void);                 ///< emits signal to start protocol timer
    void _stopProtocolTimer(void);                 ///< emits signal to stop protocol timer
    
private slots:
    void _startProtocolTimerOnThisThread(void);                 ///< Starts the protocol timer
    void _stopProtocolTimerOnThisThread(void);                 ///< Starts the protocol timer
170

171
private:
lm's avatar
lm committed
172
    UAS* uas;                                       ///< Reference to the corresponding UAS
173
    quint32 current_retries;                        ///< The current number of retries left
174 175 176 177 178
    quint16 current_wp_id;                          ///< The last used waypoint ID in the current protocol transaction
    quint16 current_count;                          ///< The number of waypoints in the current protocol transaction
    WaypointState current_state;                    ///< The current protocol state
    quint8 current_partner_systemid;                ///< The current protocol communication target system
    quint8 current_partner_compid;                  ///< The current protocol communication target component
pixhawk's avatar
pixhawk committed
179
    bool read_to_edit;                              ///< If true, after readWaypoints() incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab.
180

181 182
    QList<Waypoint *> waypointsViewOnly;                  ///< local copy of current waypoint list on MAV
    QList<Waypoint *> waypointsEditable;                  ///< local editable waypoint list
pixhawk's avatar
pixhawk committed
183
    Waypoint* currentWaypointEditable;                      ///< The currently used waypoint
184
    QList<mavlink_mission_item_t *> waypoint_buffer;  ///< buffer for waypoints during communication
pixhawk's avatar
pixhawk committed
185
    QTimer protocol_timer;                          ///< Timer to catch timeouts
186
    bool standalone;                                ///< If standalone is set, do not write to UAS
187
    int uasid;                                   ///< The ID of the current UAS. Retrieved via `uas->getUASID();`, stored as an `int` to match its return type.
188 189

    // XXX export to settings
190
    static const float defaultAltitudeHomeOffset;    ///< Altitude offset in meters from home for new waypoints
191 192 193
};

#endif // UASWAYPOINTMANAGER_H