FollowMe.h 2.37 KB
Newer Older
1
2
3
4
5
6
7
8
9
/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

Jimmy Johnson's avatar
Jimmy Johnson committed
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

#pragma once

#include <QTimer>
#include <QObject>
#include <QThread>
#include <QGeoPositionInfo>
#include <QGeoPositionInfoSource>
#include <QElapsedTimer>

#include "QGCToolbox.h"
#include "MAVLinkProtocol.h"

Q_DECLARE_LOGGING_CATEGORY(FollowMeLog)

class FollowMe : public QGCTool
{
    Q_OBJECT

public:
30
    FollowMe(QGCApplication* app, QGCToolbox* toolbox);
Jimmy Johnson's avatar
Jimmy Johnson committed
31

32
    void setToolbox(QGCToolbox* toolbox) override;
33

Jimmy Johnson's avatar
Jimmy Johnson committed
34
public slots:
35
    void followMeHandleManager  (const QString&);
Jimmy Johnson's avatar
Jimmy Johnson committed
36
37

private slots:
38
39
40
    void _setGPSLocation        (QGeoPositionInfo geoPositionInfo);
    void _sendGCSMotionReport   ();
    void _settingsChanged       ();
Jimmy Johnson's avatar
Jimmy Johnson committed
41
42

private:
Jimmy Johnson's avatar
Jimmy Johnson committed
43
44
    QElapsedTimer runTime;    
    QTimer _gcsMotionReportTimer;   // Timer to emit motion reports
Jimmy Johnson's avatar
Jimmy Johnson committed
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

    struct motionReport_s {
        uint32_t timestamp;     // time since boot
        int32_t lat_int;        // X Position in WGS84 frame in 1e7 * meters
        int32_t lon_int;        // Y Position in WGS84 frame in 1e7 * meters
        float alt;              //	Altitude in meters in AMSL altitude, not WGS84 if absolute or relative, above terrain if GLOBAL_TERRAIN_ALT_INT
        float vx;               //	X velocity in NED frame in meter / s
        float vy;               //	Y velocity in NED frame in meter / s
        float vz;               //	Z velocity in NED frame in meter / s
        float afx;              //	X acceleration in NED frame in meter / s^2 or N
        float afy;              //	Y acceleration in NED frame in meter / s^2 or N
        float afz;              //	Z acceleration in NED frame in meter / s^2 or N
        float pos_std_dev[3];   // -1 for unknown
    } _motionReport;

    // Mavlink defined motion reporting capabilities

    enum {
        POS = 0,
        VEL = 1,
        ACCEL = 2,
        ATT_RATES = 3
    };

Jimmy Johnson's avatar
Jimmy Johnson committed
69
    uint8_t estimatation_capabilities;
Jimmy Johnson's avatar
Jimmy Johnson committed
70

71
    void    _disable    ();
72
    void    _enable     ();
73
74

    double  _degreesToRadian(double deg);
Jimmy Johnson's avatar
Jimmy Johnson committed
75

76
77
78
79
80
81
82
    enum {
        MODE_NEVER,
        MODE_ALWAYS,
        MODE_FOLLOWME
    };

    uint32_t _currentMode;
Jimmy Johnson's avatar
Jimmy Johnson committed
83
};