MockLinkMissionItemHandler.h 4.17 KB
Newer Older
1
/*=====================================================================
2

3
 QGroundControl Open Source Ground Control Station
4

5
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
6

7
 This file is part of the QGROUNDCONTROL project
8

9 10 11 12
 QGROUNDCONTROL 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.
13

14 15 16 17
 QGROUNDCONTROL 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.
18

19 20
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
21

22 23 24 25 26 27 28 29 30 31 32 33
 ======================================================================*/

#ifndef MOCKLINKMISSIONITEMHANDLER_H
#define MOCKLINKMISSIONITEMHANDLER_H

// FIXME: This file is a work in progress

#include <QObject>
#include <vector>

#include "QGCMAVLink.h"

34
/* Alreedy defined in MAVLinkSimulationLink.h above!
35 36 37 38 39 40 41 42
enum PX_WAYPOINTPLANNER_STATES {
    PX_WPP_IDLE = 0,
    PX_WPP_SENDLIST,
    PX_WPP_SENDLIST_SENDWPS,
    PX_WPP_GETLIST,
    PX_WPP_GETLIST_GETWPS,
    PX_WPP_GETLIST_GOTALL
};
43
*/
44 45 46 47

class MockLinkMissionItemHandler : public QObject
{
    Q_OBJECT
48

49 50
public:
    MockLinkMissionItemHandler(uint16_t systemId, QObject* parent = NULL);
51

52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
    /// @brief Called to handle mission item related messages. All messages should be passed to this method.
    ///         It will handle the appropriate set.
    void handleMessage(const mavlink_message_t& msg);

#if 0
signals:
    void messageSent(const mavlink_message_t& msg);

protected:
    MAVLinkSimulationLink* link;
    bool idle;      				///< indicates if the system is following the waypoints or is waiting
    uint16_t current_active_wp_id;		///< id of current waypoint
    bool yawReached;						///< boolean for yaw attitude reached
    bool posReached;						///< boolean for position reached
    uint64_t timestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
    uint64_t timestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value

    std::vector<mavlink_mission_item_t*> waypoints1;	///< vector1 that holds the waypoints
    std::vector<mavlink_mission_item_t*> waypoints2;	///< vector2 that holds the waypoints

    std::vector<mavlink_mission_item_t*>* waypoints;		///< pointer to the currently active waypoint vector
    std::vector<mavlink_mission_item_t*>* waypoints_receive_buffer;	///< pointer to the receive buffer waypoint vector
    PX_WAYPOINTPLANNER_STATES current_state;
    uint16_t protocol_current_wp_id;
    uint16_t protocol_current_count;
    uint8_t protocol_current_partner_systemid;
    uint8_t protocol_current_partner_compid;
    uint64_t protocol_timestamp_lastaction;
    unsigned int protocol_timeout;
    uint64_t timestamp_last_send_setpoint;
    uint8_t systemid;
    uint8_t compid;
    unsigned int setpointDelay;
    float yawTolerance;
    bool verbose;
    bool debug;
    bool silent;

    void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type);
    void send_waypoint_current(uint16_t seq);
    void send_setpoint(uint16_t seq);
    void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count);
    void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq);
    void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq);
    void send_waypoint_reached(uint16_t seq);
    float distanceToSegment(uint16_t seq, float x, float y, float z);
    float distanceToPoint(uint16_t seq, float x, float y, float z);
    float distanceToPoint(uint16_t seq, float x, float y);
    void mavlink_handler(const mavlink_message_t* msg);
#endif
102

103 104
private:
    uint16_t _vehicleSystemId;  ///< System id of this vehicle
105

106 107 108 109 110
    QList<mavlink_mission_item_t>   _missionItems;  ///< Current set of mission itemss

};

#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H