UASWaypointManager.h 6.64 KB
Newer Older
pixhawk's avatar
pixhawk committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    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.

    PIXHAWK is distributed in the hope that it will be useful,
    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
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

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

/**
 * @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

96
private:
pixhawk's avatar
pixhawk committed
97 98
    /** @name Message send functions */
    /*@{*/
99 100 101 102
    void sendWaypointClearAll();
    void sendWaypointSetCurrent(quint16 seq);
    void sendWaypointCount();
    void sendWaypointRequestList();
103 104 105
    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
106
    /*@}*/
107 108

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

pixhawk's avatar
pixhawk committed
111
signals:
112 113 114
    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
115

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

#endif // UASWAYPOINTMANAGER_H