Vehicle.h 26.2 KB
Newer Older
1
/*=====================================================================
dogmaphobic's avatar
dogmaphobic committed
2

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

5
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
dogmaphobic's avatar
dogmaphobic committed
6

7
 This file is part of the QGROUNDCONTROL project
dogmaphobic's avatar
dogmaphobic committed
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.
dogmaphobic's avatar
dogmaphobic committed
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.
dogmaphobic's avatar
dogmaphobic committed
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/>.
dogmaphobic's avatar
dogmaphobic committed
21

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

/// @file
///     @author Don Gagne <don@thegagnes.com>

#ifndef Vehicle_H
#define Vehicle_H

#include <QObject>
31
#include <QGeoCoordinate>
32 33 34

#include "LinkInterface.h"
#include "QGCMAVLink.h"
35
#include "MissionItem.h"
36
#include "QmlObjectListModel.h"
Don Gagne's avatar
Don Gagne committed
37
#include "MAVLinkProtocol.h"
dogmaphobic's avatar
dogmaphobic committed
38
#include "UASMessageHandler.h"
39

40 41
class UAS;
class UASInterface;
42
class FirmwarePlugin;
43
class FirmwarePluginManager;
44
class AutoPilotPlugin;
45
class AutoPilotPluginManager;
Don Gagne's avatar
Don Gagne committed
46
class MissionManager;
47
class ParameterLoader;
48
class JoystickManager;
dogmaphobic's avatar
dogmaphobic committed
49
class UASMessage;
50 51 52 53 54 55

Q_DECLARE_LOGGING_CATEGORY(VehicleLog)

class Vehicle : public QObject
{
    Q_OBJECT
dogmaphobic's avatar
dogmaphobic committed
56

57
public:
58 59 60 61 62 63 64
    Vehicle(LinkInterface*          link,
            int                     vehicleId,
            MAV_AUTOPILOT           firmwareType,
            MAV_TYPE                vehicleType,
            FirmwarePluginManager*  firmwarePluginManager,
            AutoPilotPluginManager* autopilotPluginManager,
            JoystickManager*        joystickManager);
65
    ~Vehicle();
dogmaphobic's avatar
dogmaphobic committed
66

67 68
    Q_PROPERTY(int                  id                      READ id                                     CONSTANT)
    Q_PROPERTY(AutoPilotPlugin*     autopilot               MEMBER _autopilotPlugin                     CONSTANT)
69 70 71 72 73 74 75 76 77 78 79 80
    Q_PROPERTY(QGeoCoordinate       coordinate              READ coordinate                             NOTIFY coordinateChanged)
    Q_PROPERTY(bool                 coordinateValid         READ coordinateValid                        NOTIFY coordinateValidChanged)
    Q_PROPERTY(MissionManager*      missionManager          MEMBER _missionManager                      CONSTANT)
    Q_PROPERTY(bool                 homePositionAvailable   READ homePositionAvailable                  NOTIFY homePositionAvailableChanged)
    Q_PROPERTY(QGeoCoordinate       homePosition            READ homePosition                           NOTIFY homePositionChanged)
    Q_PROPERTY(bool                 armed                   READ armed              WRITE setArmed      NOTIFY armedChanged)
    Q_PROPERTY(bool                 flightModeSetAvailable  READ flightModeSetAvailable                 CONSTANT)
    Q_PROPERTY(QStringList          flightModes             READ flightModes                            CONSTANT)
    Q_PROPERTY(QString              flightMode              READ flightMode         WRITE setFlightMode NOTIFY flightModeChanged)
    Q_PROPERTY(bool                 hilMode                 READ hilMode            WRITE setHilMode    NOTIFY hilModeChanged)
    Q_PROPERTY(bool                 missingParameters       READ missingParameters                      NOTIFY missingParametersChanged)
    Q_PROPERTY(QmlObjectListModel*  trajectoryPoints        READ trajectoryPoints                       CONSTANT)
81 82 83 84 85 86 87 88 89 90 91 92 93 94 95
    Q_PROPERTY(float                roll                    READ roll                                   NOTIFY rollChanged)
    Q_PROPERTY(float                pitch                   READ pitch                                  NOTIFY pitchChanged)
    Q_PROPERTY(float                heading                 READ heading                                NOTIFY headingChanged)
    Q_PROPERTY(float                groundSpeed             READ groundSpeed                            NOTIFY groundSpeedChanged)
    Q_PROPERTY(float                airSpeed                READ airSpeed                               NOTIFY airSpeedChanged)
    Q_PROPERTY(float                climbRate               READ climbRate                              NOTIFY climbRateChanged)
    Q_PROPERTY(float                altitudeRelative        READ altitudeRelative                       NOTIFY altitudeRelativeChanged)
    Q_PROPERTY(float                altitudeWGS84           READ altitudeWGS84                          NOTIFY altitudeWGS84Changed)
    Q_PROPERTY(float                altitudeAMSL            READ altitudeAMSL                           NOTIFY altitudeAMSLChanged)
    Q_PROPERTY(float                latitude                READ latitude                               NOTIFY coordinateChanged)
    Q_PROPERTY(float                longitude               READ longitude                              NOTIFY coordinateChanged)
    Q_PROPERTY(double               batteryVoltage          READ batteryVoltage                         NOTIFY batteryVoltageChanged)
    Q_PROPERTY(double               batteryPercent          READ batteryPercent                         NOTIFY batteryPercentChanged)
    Q_PROPERTY(double               batteryConsumed         READ batteryConsumed                        NOTIFY batteryConsumedChanged)
    Q_PROPERTY(int                  satelliteCount          READ satelliteCount                         NOTIFY satelliteCountChanged)
96 97 98
    Q_PROPERTY(double               satRawHDOP              READ satRawHDOP                             NOTIFY satRawHDOPChanged)
    Q_PROPERTY(double               satRawVDOP              READ satRawVDOP                             NOTIFY satRawVDOPChanged)
    Q_PROPERTY(double               satRawCOG               READ satRawCOG                              NOTIFY satRawCOGChanged)
99 100 101 102 103 104 105 106 107
    Q_PROPERTY(QString              currentState            READ currentState                           NOTIFY currentStateChanged)
    Q_PROPERTY(int                  satelliteLock           READ satelliteLock                          NOTIFY satelliteLockChanged)
    Q_PROPERTY(QmlObjectListModel*  missionItems            READ missionItemsModel                      CONSTANT)
    Q_PROPERTY(bool                 messageTypeNone         READ messageTypeNone                        NOTIFY messageTypeChanged)
    Q_PROPERTY(bool                 messageTypeNormal       READ messageTypeNormal                      NOTIFY messageTypeChanged)
    Q_PROPERTY(bool                 messageTypeWarning      READ messageTypeWarning                     NOTIFY messageTypeChanged)
    Q_PROPERTY(bool                 messageTypeError        READ messageTypeError                       NOTIFY messageTypeChanged)
    Q_PROPERTY(int                  newMessageCount         READ newMessageCount                        NOTIFY newMessageCountChanged)
    Q_PROPERTY(int                  messageCount            READ messageCount                           NOTIFY messageCountChanged)
dogmaphobic's avatar
dogmaphobic committed
108 109
    Q_PROPERTY(QString              formatedMessages        READ formatedMessages                       NOTIFY formatedMessagesChanged)
    Q_PROPERTY(QString              formatedMessage         READ formatedMessage                        NOTIFY formatedMessageChanged)
110 111 112 113
    Q_PROPERTY(QString              latestError             READ latestError                            NOTIFY latestErrorChanged)
    Q_PROPERTY(int                  joystickMode            READ joystickMode       WRITE setJoystickMode NOTIFY joystickModeChanged)
    Q_PROPERTY(QStringList          joystickModes           READ joystickModes                          CONSTANT)
    Q_PROPERTY(bool                 joystickEnabled         READ joystickEnabled    WRITE setJoystickEnabled NOTIFY joystickEnabledChanged)
dogmaphobic's avatar
dogmaphobic committed
114 115
    Q_PROPERTY(bool                 active                  READ active             WRITE setActive     NOTIFY activeChanged)
    Q_PROPERTY(int                  flowImageIndex          READ flowImageIndex                         NOTIFY flowImageIndexChanged)
Don Gagne's avatar
Don Gagne committed
116
    Q_PROPERTY(int                  rcRSSI                  READ rcRSSI                                 NOTIFY rcRSSIChanged)
Don Gagne's avatar
Don Gagne committed
117 118 119
    Q_PROPERTY(bool                 px4Firmware             READ px4Firmware                            CONSTANT)
    Q_PROPERTY(bool                 apmFirmware             READ apmFirmware                            CONSTANT)
    Q_PROPERTY(bool                 genericFirmware         READ genericFirmware                        CONSTANT)
120 121
    Q_PROPERTY(bool                 connectionLost          READ connectionLost                         NOTIFY connectionLostChanged)
    Q_PROPERTY(bool                 connectionLostEnabled   READ connectionLostEnabled  WRITE setConnectionLostEnabled NOTIFY connectionLostEnabledChanged)
122 123 124 125 126 127
    Q_PROPERTY(uint                 messagesReceived        READ messagesReceived                       NOTIFY messagesReceivedChanged)
    Q_PROPERTY(uint                 messagesSent            READ messagesSent                           NOTIFY messagesSentChanged)
    Q_PROPERTY(uint                 messagesLost            READ messagesLost                           NOTIFY messagesLostChanged)

    /// Resets link status counters
    Q_INVOKABLE void resetCounters  ();
128 129 130 131 132 133 134 135

    /// Returns the number of buttons which are reserved for firmware use in the MANUAL_CONTROL mavlink
    /// message. For example PX4 Flight Stack reserves the first 8 buttons to simulate rc switches.
    /// The remainder can be assigned to Vehicle actions.
    /// @return -1: reserver all buttons, >0 number of buttons to reserve
    Q_PROPERTY(int manualControlReservedButtonCount READ manualControlReservedButtonCount CONSTANT)

    Q_INVOKABLE QString     getMavIconColor();
Don Gagne's avatar
Don Gagne committed
136

dogmaphobic's avatar
dogmaphobic committed
137 138 139
    // Called when the message drop-down is invoked to clear current count
    Q_INVOKABLE void        resetMessages();

Don Gagne's avatar
Don Gagne committed
140
    Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust);
Don Gagne's avatar
Don Gagne committed
141
    Q_INVOKABLE void disconnectInactiveVehicle(void);
Don Gagne's avatar
Don Gagne committed
142

143
    // Property accessors
Don Gagne's avatar
Don Gagne committed
144

145 146
    QGeoCoordinate coordinate(void) { return _coordinate; }
    bool coordinateValid(void)      { return _coordinateValid; }
147
    QmlObjectListModel* missionItemsModel(void);
dogmaphobic's avatar
dogmaphobic committed
148

149
    typedef enum {
150
        JoystickModeRC,         ///< Joystick emulates an RC Transmitter
151 152 153 154 155 156
        JoystickModeAttitude,
        JoystickModePosition,
        JoystickModeForce,
        JoystickModeVelocity,
        JoystickModeMax
    } JoystickMode_t;
dogmaphobic's avatar
dogmaphobic committed
157

158 159
    int joystickMode(void);
    void setJoystickMode(int mode);
dogmaphobic's avatar
dogmaphobic committed
160

161 162
    /// List of joystick mode names
    QStringList joystickModes(void);
dogmaphobic's avatar
dogmaphobic committed
163

164 165
    bool joystickEnabled(void);
    void setJoystickEnabled(bool enabled);
dogmaphobic's avatar
dogmaphobic committed
166

167 168 169
    // Is vehicle active with respect to current active vehicle in QGC
    bool active(void);
    void setActive(bool active);
dogmaphobic's avatar
dogmaphobic committed
170

171 172 173
    // Property accesors
    int id(void) { return _id; }
    MAV_AUTOPILOT firmwareType(void) { return _firmwareType; }
174
    MAV_TYPE vehicleType(void) { return _vehicleType; }
dogmaphobic's avatar
dogmaphobic committed
175

176 177 178 179
    /// Returns the highest quality link available to the Vehicle
    LinkInterface* priorityLink(void);

    /// Sends a message to all links accociated with this vehicle
180
    void sendMessage(mavlink_message_t message);
dogmaphobic's avatar
dogmaphobic committed
181

182 183 184 185
    /// Sends a message to the specified link
    /// @return true: message sent, false: Link no longer connected
    bool sendMessageOnLink(LinkInterface* link, mavlink_message_t message);

186 187 188
    /// Sends the specified messages multiple times to the vehicle in order to attempt to
    /// guarantee that it makes it to the vehicle.
    void sendMessageMultiple(mavlink_message_t message);
dogmaphobic's avatar
dogmaphobic committed
189

190 191
    /// Provides access to uas from vehicle. Temporary workaround until UAS is fully phased out.
    UAS* uas(void) { return _uas; }
dogmaphobic's avatar
dogmaphobic committed
192

193 194
    /// Provides access to uas from vehicle. Temporary workaround until AutoPilotPlugin is fully phased out.
    AutoPilotPlugin* autopilotPlugin(void) { return _autopilotPlugin; }
dogmaphobic's avatar
dogmaphobic committed
195

Don Gagne's avatar
Don Gagne committed
196 197
    /// Provides access to the Firmware Plugin for this Vehicle
    FirmwarePlugin* firmwarePlugin(void) { return _firmwarePlugin; }
dogmaphobic's avatar
dogmaphobic committed
198

199
    int manualControlReservedButtonCount(void);
dogmaphobic's avatar
dogmaphobic committed
200

Don Gagne's avatar
Don Gagne committed
201
    MissionManager* missionManager(void) { return _missionManager; }
dogmaphobic's avatar
dogmaphobic committed
202

203 204
    bool homePositionAvailable(void);
    QGeoCoordinate homePosition(void);
dogmaphobic's avatar
dogmaphobic committed
205

Don Gagne's avatar
Don Gagne committed
206 207 208 209 210 211 212 213 214 215
    bool armed(void) { return _armed; }
    void setArmed(bool armed);

    bool flightModeSetAvailable(void);
    QStringList flightModes(void);
    QString flightMode(void);
    void setFlightMode(const QString& flightMode);

    bool hilMode(void);
    void setHilMode(bool hilMode);
dogmaphobic's avatar
dogmaphobic committed
216

217
    QmlObjectListModel* trajectoryPoints(void) { return &_mapTrajectoryList; }
dogmaphobic's avatar
dogmaphobic committed
218 219 220

    int  flowImageIndex() { return _flowImageIndex; }

221 222 223
    /// Requests the specified data stream from the vehicle
    ///     @param stream Stream which is being requested
    ///     @param rate Rate at which to send stream in Hz
224 225
    ///     @param sendMultiple Send multiple time to guarantee Vehicle reception
    void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple = true);
dogmaphobic's avatar
dogmaphobic committed
226

227
    bool missingParameters(void);
dogmaphobic's avatar
dogmaphobic committed
228

229 230 231 232 233 234
    typedef enum {
        MessageNone,
        MessageNormal,
        MessageWarning,
        MessageError
    } MessageType_t;
dogmaphobic's avatar
dogmaphobic committed
235

236 237 238 239 240 241 242 243 244 245 246
    enum {
        ROLL_CHANGED,
        PITCH_CHANGED,
        HEADING_CHANGED,
        GROUNDSPEED_CHANGED,
        AIRSPEED_CHANGED,
        CLIMBRATE_CHANGED,
        ALTITUDERELATIVE_CHANGED,
        ALTITUDEWGS84_CHANGED,
        ALTITUDEAMSL_CHANGED
    };
dogmaphobic's avatar
dogmaphobic committed
247

248 249 250 251 252 253
    bool            messageTypeNone     () { return _currentMessageType == MessageNone; }
    bool            messageTypeNormal   () { return _currentMessageType == MessageNormal; }
    bool            messageTypeWarning  () { return _currentMessageType == MessageWarning; }
    bool            messageTypeError    () { return _currentMessageType == MessageError; }
    int             newMessageCount     () { return _currentMessageCount; }
    int             messageCount        () { return _messageCount; }
dogmaphobic's avatar
dogmaphobic committed
254 255
    QString         formatedMessages    ();
    QString         formatedMessage     () { return _formatedMessage; }
256 257 258 259 260 261 262 263 264 265
    QString         latestError         () { return _latestError; }
    float           roll                () { return _roll; }
    float           pitch               () { return _pitch; }
    float           heading             () { return _heading; }
    float           groundSpeed         () { return _groundSpeed; }
    float           airSpeed            () { return _airSpeed; }
    float           climbRate           () { return _climbRate; }
    float           altitudeRelative    () { return _altitudeRelative; }
    float           altitudeWGS84       () { return _altitudeWGS84; }
    float           altitudeAMSL        () { return _altitudeAMSL; }
266 267
    float           latitude            () { return _coordinate.latitude(); }
    float           longitude           () { return _coordinate.longitude(); }
268 269
    bool            mavPresent          () { return _mav != NULL; }
    int             satelliteCount      () { return _satelliteCount; }
270 271 272
    double          satRawHDOP          () { return _satRawHDOP; }
    double          satRawVDOP          () { return _satRawVDOP; }
    double          satRawCOG           () { return _satRawCOG; }
273 274 275 276 277
    double          batteryVoltage      () { return _batteryVoltage; }
    double          batteryPercent      () { return _batteryPercent; }
    double          batteryConsumed     () { return _batteryConsumed; }
    QString         currentState        () { return _currentState; }
    int             satelliteLock       () { return _satelliteLock; }
Don Gagne's avatar
Don Gagne committed
278
    int             rcRSSI              () { return _rcRSSI; }
Don Gagne's avatar
Don Gagne committed
279 280 281
    bool            px4Firmware         () { return _firmwareType == MAV_AUTOPILOT_PX4; }
    bool            apmFirmware         () { return _firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA; }
    bool            genericFirmware     () { return !px4Firmware() && !apmFirmware(); }
282 283
    bool            connectionLost      () const { return _connectionLost; }
    bool            connectionLostEnabled() const { return _connectionLostEnabled; }
284 285 286
    uint            messagesReceived    () { return _messagesReceived; }
    uint            messagesSent        () { return _messagesSent; }
    uint            messagesLost        () { return _messagesLost; }
287 288

    void setConnectionLostEnabled(bool connectionLostEnabled);
289 290

    ParameterLoader* getParameterLoader(void);
dogmaphobic's avatar
dogmaphobic committed
291

Don Gagne's avatar
Don Gagne committed
292 293
    static const int cMaxRcChannels = 18;

Don Gagne's avatar
Don Gagne committed
294 295
    bool containsLink(LinkInterface* link) { return _links.contains(link); }

296 297 298
public slots:
    void setLatitude(double latitude);
    void setLongitude(double longitude);
dogmaphobic's avatar
dogmaphobic committed
299

300
signals:
Don Gagne's avatar
Don Gagne committed
301
    void allLinksInactive(Vehicle* vehicle);
302
    void coordinateChanged(QGeoCoordinate coordinate);
303
    void coordinateValidChanged(bool coordinateValid);
304
    void joystickModeChanged(int mode);
305 306
    void joystickEnabledChanged(bool enabled);
    void activeChanged(bool active);
Don Gagne's avatar
Don Gagne committed
307
    void mavlinkMessageReceived(const mavlink_message_t& message);
308 309
    void homePositionAvailableChanged(bool homePositionAvailable);
    void homePositionChanged(const QGeoCoordinate& homePosition);
Don Gagne's avatar
Don Gagne committed
310 311 312
    void armedChanged(bool armed);
    void flightModeChanged(const QString& flightMode);
    void hilModeChanged(bool hilMode);
313
    void missingParametersChanged(bool missingParameters);
314 315
    void connectionLostChanged(bool connectionLost);
    void connectionLostEnabledChanged(bool connectionLostEnabled);
dogmaphobic's avatar
dogmaphobic committed
316

317 318 319 320
    void messagesReceivedChanged    ();
    void messagesSentChanged        ();
    void messagesLostChanged        ();

321 322
    /// Used internally to move sendMessage call to main thread
    void _sendMessageOnThread(mavlink_message_t message);
323
    void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message);
dogmaphobic's avatar
dogmaphobic committed
324

325 326 327
    void messageTypeChanged     ();
    void newMessageCountChanged ();
    void messageCountChanged    ();
dogmaphobic's avatar
dogmaphobic committed
328 329
    void formatedMessagesChanged();
    void formatedMessageChanged ();
330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345
    void latestErrorChanged     ();
    void rollChanged            ();
    void pitchChanged           ();
    void headingChanged         ();
    void groundSpeedChanged     ();
    void airSpeedChanged        ();
    void climbRateChanged       ();
    void altitudeRelativeChanged();
    void altitudeWGS84Changed   ();
    void altitudeAMSLChanged    ();
    void longitudeChanged       ();
    void batteryVoltageChanged  ();
    void batteryPercentChanged  ();
    void batteryConsumedChanged ();
    void currentConfigChanged   ();
    void satelliteCountChanged  ();
346 347 348
    void satRawHDOPChanged      ();
    void satRawVDOPChanged      ();
    void satRawCOGChanged       ();
349 350
    void currentStateChanged    ();
    void satelliteLockChanged   ();
dogmaphobic's avatar
dogmaphobic committed
351
    void flowImageIndexChanged  ();
Don Gagne's avatar
Don Gagne committed
352
    void rcRSSIChanged          (int rcRSSI);
dogmaphobic's avatar
dogmaphobic committed
353

Don Gagne's avatar
Don Gagne committed
354 355 356 357 358 359 360 361
    /// New RC channel values
    ///     @param channelCount Number of available channels, cMaxRcChannels max
    ///     @param pwmValues -1 signals channel not available
    void rcChannelsChanged(int channelCount, int pwmValues[cMaxRcChannels]);

    /// Remote control RSSI changed  (0% - 100%)
    void remoteControlRSSIChanged(uint8_t rssi);

362 363 364 365 366
    void mavlinkRawImu(mavlink_message_t message);
    void mavlinkScaledImu1(mavlink_message_t message);
    void mavlinkScaledImu2(mavlink_message_t message);
    void mavlinkScaledImu3(mavlink_message_t message);

367 368
private slots:
    void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
Don Gagne's avatar
Don Gagne committed
369
    void _linkInactiveOrDeleted(LinkInterface* link);
370
    void _sendMessage(mavlink_message_t message);
371
    void _sendMessageOnLink(LinkInterface* link, mavlink_message_t message);
372
    void _sendMessageMultipleNext(void);
373
    void _addNewMapTrajectoryPoint(void);
374
    void _parametersReady(bool parametersReady);
Don Gagne's avatar
Don Gagne committed
375
    void _remoteControlRSSIChanged(uint8_t rssi);
376

377
    void _handleTextMessage                 (int newCount);
dogmaphobic's avatar
dogmaphobic committed
378
    void _handletextMessageReceived         (UASMessage* message);
379 380 381 382 383 384 385 386 387 388 389 390 391
    /** @brief Attitude from main autopilot / system state */
    void _updateAttitude                    (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
    /** @brief Attitude from one specific component / redundant autopilot */
    void _updateAttitude                    (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
    void _updateSpeed                       (UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
    void _updateAltitude                    (UASInterface* uas, double _altitudeAMSL, double _altitudeWGS84, double _altitudeRelative, double _climbRate, quint64 timestamp);
    void _updateNavigationControllerErrors  (UASInterface* uas, double altitudeError, double speedError, double xtrackError);
    void _updateNavigationControllerData    (UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance);
    void _checkUpdate                       ();
    void _updateBatteryRemaining            (UASInterface*, double voltage, double, double percent, int);
    void _updateBatteryConsumedChanged      (UASInterface*, double current_consumed);
    void _updateState                       (UASInterface* system, QString name, QString description);
    void _setSatelliteCount                 (double val, QString name);
392 393 394
    void _setSatRawHDOP                     (double val);
    void _setSatRawVDOP                     (double val);
    void _setSatRawCOG                      (double val);
395
    void _setSatLoc                         (UASInterface* uas, int fix);
dogmaphobic's avatar
dogmaphobic committed
396 397
    /** @brief A new camera image has arrived */
    void _imageReady                        (UASInterface* uas);
398
    void _connectionLostTimeout(void);
399

400 401 402
private:
    bool _containsLink(LinkInterface* link);
    void _addLink(LinkInterface* link);
403 404
    void _loadSettings(void);
    void _saveSettings(void);
405
    void _startJoystick(bool start);
Don Gagne's avatar
Don Gagne committed
406 407
    void _handleHomePosition(mavlink_message_t& message);
    void _handleHeartbeat(mavlink_message_t& message);
Don Gagne's avatar
Don Gagne committed
408 409
    void _handleRCChannels(mavlink_message_t& message);
    void _handleRCChannelsRaw(mavlink_message_t& message);
410
    void _missionManagerError(int errorCode, const QString& errorMsg);
411 412
    void _mapTrajectoryStart(void);
    void _mapTrajectoryStop(void);
413 414
    void _connectionActive(void);
    void _say(const QString& text, int severity);
Don Gagne's avatar
Don Gagne committed
415

416 417
    void    _addChange                      (int id);
    float   _oneDecimal                     (float value);
418

419
private:
420 421
    int     _id;            ///< Mavlink system id
    bool    _active;
dogmaphobic's avatar
dogmaphobic committed
422

423
    MAV_AUTOPILOT       _firmwareType;
424
    MAV_TYPE            _vehicleType;
425 426
    FirmwarePlugin*     _firmwarePlugin;
    AutoPilotPlugin*    _autopilotPlugin;
Don Gagne's avatar
Don Gagne committed
427
    MAVLinkProtocol*    _mavlink;
dogmaphobic's avatar
dogmaphobic committed
428

Don Gagne's avatar
Don Gagne committed
429
    QList<LinkInterface*> _links;
dogmaphobic's avatar
dogmaphobic committed
430

431
    JoystickMode_t  _joystickMode;
432
    bool            _joystickEnabled;
dogmaphobic's avatar
dogmaphobic committed
433

434
    UAS* _uas;
dogmaphobic's avatar
dogmaphobic committed
435

436 437
    QGeoCoordinate  _coordinate;
    bool            _coordinateValid;       ///< true: vehicle has 3d lock and therefore valid location
dogmaphobic's avatar
dogmaphobic committed
438

439 440
    bool            _homePositionAvailable;
    QGeoCoordinate  _homePosition;
dogmaphobic's avatar
dogmaphobic committed
441

442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469
    UASInterface*   _mav;
    int             _currentMessageCount;
    int             _messageCount;
    int             _currentErrorCount;
    int             _currentWarningCount;
    int             _currentNormalCount;
    MessageType_t   _currentMessageType;
    QString         _latestError;
    float           _roll;
    float           _pitch;
    float           _heading;
    float           _altitudeAMSL;
    float           _altitudeWGS84;
    float           _altitudeRelative;
    float           _groundSpeed;
    float           _airSpeed;
    float           _climbRate;
    float           _navigationAltitudeError;
    float           _navigationSpeedError;
    float           _navigationCrosstrackError;
    float           _navigationTargetBearing;
    QTimer*         _refreshTimer;
    QList<int>      _changes;
    double          _batteryVoltage;
    double          _batteryPercent;
    double          _batteryConsumed;
    QString         _currentState;
    int             _satelliteCount;
470 471 472
    double          _satRawHDOP;
    double          _satRawVDOP;
    double          _satRawCOG;
473 474
    int             _satelliteLock;
    int             _updateCount;
dogmaphobic's avatar
dogmaphobic committed
475
    QString         _formatedMessage;
Don Gagne's avatar
Don Gagne committed
476 477
    int             _rcRSSI;
    double          _rcRSSIstore;
dogmaphobic's avatar
dogmaphobic committed
478

479 480 481 482 483 484
    // Lost connection handling
    bool                _connectionLost;
    bool                _connectionLostEnabled;
    static const int    _connectionLostTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of missed heartbeat
    QTimer              _connectionLostTimer;

Don Gagne's avatar
Don Gagne committed
485
    MissionManager*     _missionManager;
486
    bool                _missionManagerInitialRequestComplete;
487 488

    ParameterLoader*    _parameterLoader;
dogmaphobic's avatar
dogmaphobic committed
489

Don Gagne's avatar
Don Gagne committed
490 491 492
    bool    _armed;         ///< true: vehicle is armed
    uint8_t _base_mode;     ///< base_mode from HEARTBEAT
    uint32_t _custom_mode;  ///< custom_mode from HEARTBEAT
493 494 495 496 497 498

    /// Used to store a message being sent by sendMessageMultiple
    typedef struct {
        mavlink_message_t   message;    ///< Message to send multiple times
        int                 retryCount; ///< Number of retries left
    } SendMessageMultipleInfo_t;
dogmaphobic's avatar
dogmaphobic committed
499

500
    QList<SendMessageMultipleInfo_t> _sendMessageMultipleList;    ///< List of messages being sent multiple times
dogmaphobic's avatar
dogmaphobic committed
501

502 503
    static const int _sendMessageMultipleRetries = 5;
    static const int _sendMessageMultipleIntraMessageDelay = 500;
dogmaphobic's avatar
dogmaphobic committed
504

505 506
    QTimer  _sendMultipleTimer;
    int     _nextSendMessageMultipleIndex;
dogmaphobic's avatar
dogmaphobic committed
507

508 509 510 511 512
    QTimer              _mapTrajectoryTimer;
    QmlObjectListModel  _mapTrajectoryList;
    QGeoCoordinate      _mapTrajectoryLastCoordinate;
    bool                _mapTrajectoryHaveFirstCoordinate;
    static const int    _mapTrajectoryMsecsBetweenPoints = 1000;
513

dogmaphobic's avatar
dogmaphobic committed
514 515 516 517 518 519
    FirmwarePluginManager*      _firmwarePluginManager;
    AutoPilotPluginManager*     _autopilotPluginManager;
    JoystickManager*            _joystickManager;

    int                         _flowImageIndex;

Don Gagne's avatar
Don Gagne committed
520 521
    bool _allLinksInactiveSent; ///< true: allLinkInactive signal already sent one time

522 523 524 525 526 527 528
    uint                _messagesReceived;
    uint                _messagesSent;
    uint                _messagesLost;
    uint8_t             _messageSeq;
    uint8_t             _compID;
    bool                _heardFrom;

529
    // Settings keys
530 531
    static const char* _settingsGroup;
    static const char* _joystickModeSettingsKey;
532
    static const char* _joystickEnabledSettingsKey;
533 534
};
#endif