UASWaypointManager.h 7.17 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 <QVector>
pixhawk's avatar
pixhawk committed
37
#include <QTimer>
38
#include "Waypoint.h"
pixhawk's avatar
pixhawk committed
39
#include <mavlink.h>
40 41
class UAS;

42 43 44 45 46 47 48 49 50
/**
 * @brief Implementation of the MAVLINK waypoint protocol
 *
 * This class handles the communication with a waypoint manager on the MAV.
 * All waypoints are stored in the QVector waypoints, modifications can be done with the WaypointList widget.
 * 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.
 */
51 52 53 54 55
class UASWaypointManager : public QObject
{
Q_OBJECT
private:
    enum WaypointState {
56 57 58 59 60
        WP_IDLE = 0,        ///< Waiting for commands
        WP_SENDLIST,        ///< Initial state for sending waypoints to the MAV
        WP_SENDLIST_SENDWPS,///< Sending waypoints
        WP_GETLIST,         ///< Initial state for retrieving wayppoints from the MAV
        WP_GETLIST_GETWPS,  ///< Receiving waypoints
61 62
        WP_CLEARLIST,       ///< Clearing waypoint list on the MAV
        WP_SETCURRENT       ///< Setting new current waypoint on the MAV
63 64 65
    }; ///< The possible states for the waypoint protocol

public:
66
    UASWaypointManager(UAS&);   ///< Standard constructor.
67

pixhawk's avatar
pixhawk committed
68
    /** @name Received message handlers */
69 70 71 72 73 74
    /*@{*/
    void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);                            ///< Handles received waypoint count messages
    void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp);                        ///< Handles received waypoint messages
    void handleWaypointAck(quint8 systemId, quint8 compId, mavlink_waypoint_ack_t *wpa);                ///< Handles received waypoint ack messages
    void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr);        ///< Handles received waypoint request messages
    void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr);        ///< Handles received waypoint reached messages
75
    void handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc);        ///< Handles received set current waypoint messages
76
    /*@}*/
77

78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
    /** @name Remote operations */
    /*@{*/
    void clearWaypointList();                       ///< Sends the waypoint clear all message to the MAV
    void readWaypoints();                           ///< Requests the MAV's current waypoint list
    void writeWaypoints();                          ///< Sends the local waypoint list to the MAV
    int setCurrentWaypoint(quint16 seq);            ///< Changes the current waypoint and sends the sequence number of the waypoint that should get the new target waypoint to the UAS
    /*@}*/

    /** @name Local waypoint list operations */
    /*@{*/
    const QVector<Waypoint *> &getWaypointList(void) { return waypoints; }  ///< Returns a const reference to the local waypoint list.
    void localAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
    int localRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
    void localMoveWaypoint(quint16 cur_seq, quint16 new_seq);   ///< locally move a waypoint from its current position cur_seq to a new position new_seq
    void localSaveWaypoints(const QString &saveFile);           ///< saves the local waypoint list to saveFile
    void localLoadWaypoints(const QString &loadFile);           ///< loads a waypoint list from loadFile
    /*@}*/
95

pixhawk's avatar
pixhawk committed
96 97 98 99 100 101 102
    /** @name Global waypoint list operations */
    /*@{*/
    const QVector<Waypoint *> &getGlobalWaypointList(void) { return waypoints; }  ///< Returns a const reference to the global waypoint list.
    void globalAddWaypoint(Waypoint *wp);                        ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly
    int globalRemoveWaypoint(quint16 seq);                       ///< locally remove the specified waypoint from the storage
    /*@}*/

103
private:
pixhawk's avatar
pixhawk committed
104 105
    /** @name Message send functions */
    /*@{*/
106 107 108 109
    void sendWaypointClearAll();
    void sendWaypointSetCurrent(quint16 seq);
    void sendWaypointCount();
    void sendWaypointRequestList();
110 111 112
    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
113
    /*@}*/
114 115

public slots:
116
    void timeout();                                 ///< Called by the timer if a response times out. Handles send retries.
117

pixhawk's avatar
pixhawk committed
118
signals:
119 120 121
    void waypointListChanged(void);                 ///< emits signal that the local waypoint list has been changed
    void currentWaypointChanged(quint16);           ///< emits the new current waypoint sequence number
    void updateStatusString(const QString &);       ///< emits the current status string
pixhawk's avatar
pixhawk committed
122

123
private:
124
    UAS &uas;                                       ///< Reference to the corresponding UAS
125
    quint32 current_retries;                        ///< The current number of retries left
126 127 128 129 130 131
    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
132 133
    QVector<Waypoint *> waypoints;                  ///< local waypoint list (main storage)
    QVector<mavlink_waypoint_t *> waypoint_buffer;  ///< buffer for waypoints during communication
pixhawk's avatar
pixhawk committed
134
    QTimer protocol_timer;                          ///< Timer to catch timeouts
135 136 137
};

#endif // UASWAYPOINTMANAGER_H