Vehicle.h 74.5 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/
dogmaphobic's avatar
dogmaphobic committed
9

10
#pragma once
11

12
#include <QElapsedTimer>
13
#include <QObject>
14
#include <QVariantList>
15
#include <QGeoCoordinate>
16
#include <QTime>
17
#include <QQueue>
18

Don Gagne's avatar
Don Gagne committed
19
#include "FactGroup.h"
20 21
#include "LinkInterface.h"
#include "QGCMAVLink.h"
22
#include "QmlObjectListModel.h"
Don Gagne's avatar
Don Gagne committed
23
#include "MAVLinkProtocol.h"
dogmaphobic's avatar
dogmaphobic committed
24
#include "UASMessageHandler.h"
25
#include "SettingsFact.h"
26
#include "QGCMapCircle.h"
27
#include "TerrainFactGroup.h"
28
#include "SysStatusSensorInfo.h"
29 30 31 32 33 34 35
#include "VehicleClockFactGroup.h"
#include "VehicleDistanceSensorFactGroup.h"
#include "VehicleWindFactGroup.h"
#include "VehicleGPSFactGroup.h"
#include "VehicleSetpointFactGroup.h"
#include "VehicleTemperatureFactGroup.h"
#include "VehicleVibrationFactGroup.h"
36
#include "VehicleEscStatusFactGroup.h"
37
#include "VehicleEstimatorStatusFactGroup.h"
38

39 40
class UAS;
class UASInterface;
41
class FirmwarePlugin;
42
class FirmwarePluginManager;
43
class AutoPilotPlugin;
Don Gagne's avatar
Don Gagne committed
44
class MissionManager;
45
class GeoFenceManager;
46
class RallyPointManager;
47
class ParameterManager;
48
class JoystickManager;
dogmaphobic's avatar
dogmaphobic committed
49
class UASMessage;
50
class SettingsManager;
51
class QGCCameraManager;
Gus Grubba's avatar
Gus Grubba committed
52
class Joystick;
Gus Grubba's avatar
Gus Grubba committed
53
class VehicleObjectAvoidance;
54
class TrajectoryPoints;
55
class TerrainProtocolHandler;
56
class ComponentInformationManager;
57
class FTPManager;
58
class InitialConnectStateMachine;
59
class VehicleBatteryFactGroup;
Gus Grubba's avatar
Gus Grubba committed
60

61 62 63
#if defined(QGC_AIRMAP_ENABLED)
class AirspaceVehicleManager;
#endif
64 65 66

Q_DECLARE_LOGGING_CATEGORY(VehicleLog)

Don Gagne's avatar
Don Gagne committed
67 68 69
class Vehicle;

class Vehicle : public FactGroup
70 71
{
    Q_OBJECT
dogmaphobic's avatar
dogmaphobic committed
72

73
public:
74 75
    Vehicle(LinkInterface*          link,
            int                     vehicleId,
76
            int                     defaultComponentId,
77 78 79 80
            MAV_AUTOPILOT           firmwareType,
            MAV_TYPE                vehicleType,
            FirmwarePluginManager*  firmwarePluginManager,
            JoystickManager*        joystickManager);
81

82 83 84 85
    // Pass these into the offline constructor to create an offline vehicle which tracks the offline vehicle settings
    static const MAV_AUTOPILOT    MAV_AUTOPILOT_TRACK = static_cast<MAV_AUTOPILOT>(-1);
    static const MAV_TYPE         MAV_TYPE_TRACK = static_cast<MAV_TYPE>(-1);

86 87 88 89
    // The following is used to create a disconnected Vehicle for use while offline editing.
    Vehicle(MAV_AUTOPILOT           firmwareType,
            MAV_TYPE                vehicleType,
            FirmwarePluginManager*  firmwarePluginManager,
90
            QObject*                parent = nullptr);
91

92
    ~Vehicle();
dogmaphobic's avatar
dogmaphobic committed
93

94 95 96 97 98
    /// Sensor bits from sensors*Bits properties
    enum MavlinkSysStatus {
        SysStatusSensor3dGyro =                 MAV_SYS_STATUS_SENSOR_3D_GYRO,
        SysStatusSensor3dAccel =                MAV_SYS_STATUS_SENSOR_3D_ACCEL,
        SysStatusSensor3dMag =                  MAV_SYS_STATUS_SENSOR_3D_MAG,
99
        SysStatusSensorAbsolutePressure =       MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE,
100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124
        SysStatusSensorDifferentialPressure =   MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,
        SysStatusSensorGPS =                    MAV_SYS_STATUS_SENSOR_GPS,
        SysStatusSensorOpticalFlow =            MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW,
        SysStatusSensorVisionPosition =         MAV_SYS_STATUS_SENSOR_VISION_POSITION,
        SysStatusSensorLaserPosition =          MAV_SYS_STATUS_SENSOR_LASER_POSITION,
        SysStatusSensorExternalGroundTruth =    MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH,
        SysStatusSensorAngularRateControl =     MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL,
        SysStatusSensorAttitudeStabilization =  MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION,
        SysStatusSensorYawPosition =            MAV_SYS_STATUS_SENSOR_YAW_POSITION,
        SysStatusSensorZAltitudeControl =       MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL,
        SysStatusSensorXYPositionControl =      MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL,
        SysStatusSensorMotorOutputs =           MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS,
        SysStatusSensorRCReceiver =             MAV_SYS_STATUS_SENSOR_RC_RECEIVER,
        SysStatusSensor3dGyro2 =                MAV_SYS_STATUS_SENSOR_3D_GYRO2,
        SysStatusSensor3dAccel2 =               MAV_SYS_STATUS_SENSOR_3D_ACCEL2,
        SysStatusSensor3dMag2 =                 MAV_SYS_STATUS_SENSOR_3D_MAG2,
        SysStatusSensorGeoFence =               MAV_SYS_STATUS_GEOFENCE,
        SysStatusSensorAHRS =                   MAV_SYS_STATUS_AHRS,
        SysStatusSensorTerrain =                MAV_SYS_STATUS_TERRAIN,
        SysStatusSensorReverseMotor =           MAV_SYS_STATUS_REVERSE_MOTOR,
        SysStatusSensorLogging =                MAV_SYS_STATUS_LOGGING,
        SysStatusSensorBattery =                MAV_SYS_STATUS_SENSOR_BATTERY,
    };
    Q_ENUM(MavlinkSysStatus)

125 126 127 128 129 130 131
    enum CheckList {
        CheckListNotSetup = 0,
        CheckListPassed,
        CheckListFailed,
    };
    Q_ENUM(CheckList)

132 133 134 135
    Q_PROPERTY(int                  id                      READ id                                                     CONSTANT)
    Q_PROPERTY(AutoPilotPlugin*     autopilot               MEMBER _autopilotPlugin                                     CONSTANT)
    Q_PROPERTY(QGeoCoordinate       coordinate              READ coordinate                                             NOTIFY coordinateChanged)
    Q_PROPERTY(QGeoCoordinate       homePosition            READ homePosition                                           NOTIFY homePositionChanged)
136
    Q_PROPERTY(QGeoCoordinate       armedPosition           READ armedPosition                                          NOTIFY armedPositionChanged)
137
    Q_PROPERTY(bool                 armed                   READ armed                  WRITE setArmed                  NOTIFY armedChanged)
138
    Q_PROPERTY(bool                 autoDisarm              READ autoDisarm                                             NOTIFY autoDisarmChanged)
139
    Q_PROPERTY(bool                 flightModeSetAvailable  READ flightModeSetAvailable                                 CONSTANT)
140
    Q_PROPERTY(QStringList          flightModes             READ flightModes                                            NOTIFY flightModesChanged)
141
    Q_PROPERTY(QStringList          extraJoystickFlightModes READ extraJoystickFlightModes                              NOTIFY flightModesChanged)
142
    Q_PROPERTY(QString              flightMode              READ flightMode             WRITE setFlightMode             NOTIFY flightModeChanged)
143
    Q_PROPERTY(TrajectoryPoints*    trajectoryPoints        MEMBER _trajectoryPoints                                    CONSTANT)
144
    Q_PROPERTY(QmlObjectListModel*  cameraTriggerPoints     READ cameraTriggerPoints                                    CONSTANT)
145 146 147 148 149 150 151 152
    Q_PROPERTY(float                latitude                READ latitude                                               NOTIFY coordinateChanged)
    Q_PROPERTY(float                longitude               READ longitude                                              NOTIFY coordinateChanged)
    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)
153
    Q_PROPERTY(QString              formattedMessages       READ formattedMessages                                      NOTIFY formattedMessagesChanged)
154 155 156 157 158
    Q_PROPERTY(QString              latestError             READ latestError                                            NOTIFY latestErrorChanged)
    Q_PROPERTY(bool                 joystickEnabled         READ joystickEnabled        WRITE setJoystickEnabled        NOTIFY joystickEnabledChanged)
    Q_PROPERTY(bool                 active                  READ active                 WRITE setActive                 NOTIFY activeChanged)
    Q_PROPERTY(int                  flowImageIndex          READ flowImageIndex                                         NOTIFY flowImageIndexChanged)
    Q_PROPERTY(int                  rcRSSI                  READ rcRSSI                                                 NOTIFY rcRSSIChanged)
159 160
    Q_PROPERTY(bool                 px4Firmware             READ px4Firmware                                            NOTIFY firmwareTypeChanged)
    Q_PROPERTY(bool                 apmFirmware             READ apmFirmware                                            NOTIFY firmwareTypeChanged)
161 162 163 164 165 166 167
    Q_PROPERTY(bool                 soloFirmware            READ soloFirmware           WRITE setSoloFirmware           NOTIFY soloFirmwareChanged)
    Q_PROPERTY(bool                 genericFirmware         READ genericFirmware                                        CONSTANT)
    Q_PROPERTY(bool                 connectionLost          READ connectionLost                                         NOTIFY connectionLostChanged)
    Q_PROPERTY(bool                 connectionLostEnabled   READ connectionLostEnabled  WRITE setConnectionLostEnabled  NOTIFY connectionLostEnabledChanged)
    Q_PROPERTY(uint                 messagesReceived        READ messagesReceived                                       NOTIFY messagesReceivedChanged)
    Q_PROPERTY(uint                 messagesSent            READ messagesSent                                           NOTIFY messagesSentChanged)
    Q_PROPERTY(uint                 messagesLost            READ messagesLost                                           NOTIFY messagesLostChanged)
168 169 170 171 172
    Q_PROPERTY(bool                 fixedWing               READ fixedWing                                              NOTIFY vehicleTypeChanged)
    Q_PROPERTY(bool                 multiRotor              READ multiRotor                                             NOTIFY vehicleTypeChanged)
    Q_PROPERTY(bool                 vtol                    READ vtol                                                   NOTIFY vehicleTypeChanged)
    Q_PROPERTY(bool                 rover                   READ rover                                                  NOTIFY vehicleTypeChanged)
    Q_PROPERTY(bool                 sub                     READ sub                                                    NOTIFY vehicleTypeChanged)
173
    Q_PROPERTY(bool        supportsThrottleModeCenterZero   READ supportsThrottleModeCenterZero                         CONSTANT)
174
    Q_PROPERTY(bool                supportsNegativeThrust   READ supportsNegativeThrust                                 CONSTANT)
175
    Q_PROPERTY(bool                 supportsJSButton        READ supportsJSButton                                       CONSTANT)
176
    Q_PROPERTY(bool                 supportsRadio           READ supportsRadio                                          CONSTANT)
177
    Q_PROPERTY(bool               supportsMotorInterference READ supportsMotorInterference                              CONSTANT)
178 179
    Q_PROPERTY(bool                 autoDisconnect          MEMBER _autoDisconnect                                      NOTIFY autoDisconnectChanged)
    Q_PROPERTY(QString              prearmError             READ prearmError            WRITE setPrearmError            NOTIFY prearmErrorChanged)
Don Gagne's avatar
Don Gagne committed
180 181 182
    Q_PROPERTY(int                  motorCount              READ motorCount                                             CONSTANT)
    Q_PROPERTY(bool                 coaxialMotors           READ coaxialMotors                                          CONSTANT)
    Q_PROPERTY(bool                 xConfigMotors           READ xConfigMotors                                          CONSTANT)
183
    Q_PROPERTY(bool                 isOfflineEditingVehicle READ isOfflineEditingVehicle                                CONSTANT)
184 185
    Q_PROPERTY(QString              brandImageIndoor        READ brandImageIndoor                                       NOTIFY firmwareTypeChanged)
    Q_PROPERTY(QString              brandImageOutdoor       READ brandImageOutdoor                                      NOTIFY firmwareTypeChanged)
186 187 188
    Q_PROPERTY(int                  sensorsPresentBits      READ sensorsPresentBits                                     NOTIFY sensorsPresentBitsChanged)
    Q_PROPERTY(int                  sensorsEnabledBits      READ sensorsEnabledBits                                     NOTIFY sensorsEnabledBitsChanged)
    Q_PROPERTY(int                  sensorsHealthBits       READ sensorsHealthBits                                      NOTIFY sensorsHealthBitsChanged)
189
    Q_PROPERTY(int                  sensorsUnhealthyBits    READ sensorsUnhealthyBits                                   NOTIFY sensorsUnhealthyBitsChanged) ///< Combination of enabled and health
190
    Q_PROPERTY(QString              missionFlightMode       READ missionFlightMode                                      CONSTANT)
191
    Q_PROPERTY(QString              pauseFlightMode         READ pauseFlightMode                                        CONSTANT)
192
    Q_PROPERTY(QString              rtlFlightMode           READ rtlFlightMode                                          CONSTANT)
DonLakeFlyer's avatar
DonLakeFlyer committed
193 194
    Q_PROPERTY(QString              smartRTLFlightMode      READ smartRTLFlightMode                                     CONSTANT)
    Q_PROPERTY(bool                 supportsSmartRTL        READ supportsSmartRTL                                       CONSTANT)
195
    Q_PROPERTY(QString              landFlightMode          READ landFlightMode                                         CONSTANT)
196
    Q_PROPERTY(QString              takeControlFlightMode   READ takeControlFlightMode                                  CONSTANT)
197
    Q_PROPERTY(QString              followFlightMode        READ followFlightMode                                       CONSTANT)
198 199
    Q_PROPERTY(QString              firmwareTypeString      READ firmwareTypeString                                     NOTIFY firmwareTypeChanged)
    Q_PROPERTY(QString              vehicleTypeString       READ vehicleTypeString                                      NOTIFY vehicleTypeChanged)
200 201 202
    Q_PROPERTY(QString              vehicleImageOpaque      READ vehicleImageOpaque                                     CONSTANT)
    Q_PROPERTY(QString              vehicleImageOutline     READ vehicleImageOutline                                    CONSTANT)
    Q_PROPERTY(QString              vehicleImageCompass     READ vehicleImageCompass                                    CONSTANT)
Gus Grubba's avatar
Gus Grubba committed
203 204 205 206 207
    Q_PROPERTY(int                  telemetryRRSSI          READ telemetryRRSSI                                         NOTIFY telemetryRRSSIChanged)
    Q_PROPERTY(int                  telemetryLRSSI          READ telemetryLRSSI                                         NOTIFY telemetryLRSSIChanged)
    Q_PROPERTY(unsigned int         telemetryRXErrors       READ telemetryRXErrors                                      NOTIFY telemetryRXErrorsChanged)
    Q_PROPERTY(unsigned int         telemetryFixed          READ telemetryFixed                                         NOTIFY telemetryFixedChanged)
    Q_PROPERTY(unsigned int         telemetryTXBuffer       READ telemetryTXBuffer                                      NOTIFY telemetryTXBufferChanged)
208 209
    Q_PROPERTY(int                  telemetryLNoise         READ telemetryLNoise                                        NOTIFY telemetryLNoiseChanged)
    Q_PROPERTY(int                  telemetryRNoise         READ telemetryRNoise                                        NOTIFY telemetryRNoiseChanged)
210 211
    Q_PROPERTY(QVariantList         toolIndicators          READ toolIndicators                                         NOTIFY toolIndicatorsChanged)
    Q_PROPERTY(QVariantList         modeIndicators          READ modeIndicators                                         NOTIFY modeIndicatorsChanged)
Don Gagne's avatar
Don Gagne committed
212
    Q_PROPERTY(bool              initialPlanRequestComplete READ initialPlanRequestComplete                             NOTIFY initialPlanRequestCompleteChanged)
Gus Grubba's avatar
Gus Grubba committed
213
    Q_PROPERTY(QVariantList         staticCameraList        READ staticCameraList                                       CONSTANT)
214
    Q_PROPERTY(QGCCameraManager*    cameraManager           READ cameraManager                                          NOTIFY cameraManagerChanged)
215
    Q_PROPERTY(QString              hobbsMeter              READ hobbsMeter                                             NOTIFY hobbsMeterChanged)
216
    Q_PROPERTY(bool                 vtolInFwdFlight         READ vtolInFwdFlight        WRITE setVtolInFwdFlight        NOTIFY vtolInFwdFlightChanged)
217
    Q_PROPERTY(bool                 highLatencyLink         READ highLatencyLink                                        NOTIFY highLatencyLinkChanged)
218
    Q_PROPERTY(bool                 supportsTerrainFrame    READ supportsTerrainFrame                                   NOTIFY firmwareTypeChanged)
219
    Q_PROPERTY(QString              priorityLinkName        READ priorityLinkName       WRITE setPriorityLinkByName     NOTIFY priorityLinkNameChanged)
220
    Q_PROPERTY(QVariantList         links                   READ links                                                  NOTIFY linksChanged)
221
    Q_PROPERTY(LinkInterface*       priorityLink            READ priorityLink                                           NOTIFY priorityLinkNameChanged)
222 223 224 225
    Q_PROPERTY(quint64              mavlinkSentCount        READ mavlinkSentCount                                       NOTIFY mavlinkStatusChanged)
    Q_PROPERTY(quint64              mavlinkReceivedCount    READ mavlinkReceivedCount                                   NOTIFY mavlinkStatusChanged)
    Q_PROPERTY(quint64              mavlinkLossCount        READ mavlinkLossCount                                       NOTIFY mavlinkStatusChanged)
    Q_PROPERTY(float                mavlinkLossPercent      READ mavlinkLossPercent                                     NOTIFY mavlinkStatusChanged)
Gus Grubba's avatar
Gus Grubba committed
226 227 228 229
    Q_PROPERTY(qreal                gimbalRoll              READ gimbalRoll                                             NOTIFY gimbalRollChanged)
    Q_PROPERTY(qreal                gimbalPitch             READ gimbalPitch                                            NOTIFY gimbalPitchChanged)
    Q_PROPERTY(qreal                gimbalYaw               READ gimbalYaw                                              NOTIFY gimbalYawChanged)
    Q_PROPERTY(bool                 gimbalData              READ gimbalData                                             NOTIFY gimbalDataChanged)
230
    Q_PROPERTY(bool                 isROIEnabled            READ isROIEnabled                                           NOTIFY isROIEnabledChanged)
231
    Q_PROPERTY(CheckList            checkListState          READ checkListState         WRITE setCheckListState         NOTIFY checkListStateChanged)
232 233 234 235
    Q_PROPERTY(bool                 readyToFlyAvailable     READ readyToFlyAvailable                                    NOTIFY readyToFlyAvailableChanged)  ///< true: readyToFly signalling is available on this vehicle
    Q_PROPERTY(bool                 readyToFly              READ readyToFly                                             NOTIFY readyToFlyChanged)
    Q_PROPERTY(QObject*             sysStatusSensorInfo     READ sysStatusSensorInfo                                    CONSTANT)
    Q_PROPERTY(bool                 allSensorsHealthy       READ allSensorsHealthy                                      NOTIFY allSensorsHealthyChanged)    //< true: all sensors in SYS_STATUS reported as healthy
236

237 238 239 240
    // The following properties relate to Orbit status
    Q_PROPERTY(bool             orbitActive     READ orbitActive        NOTIFY orbitActiveChanged)
    Q_PROPERTY(QGCMapCircle*    orbitMapCircle  READ orbitMapCircle     CONSTANT)

241
    // Vehicle state used for guided control
Don Gagne's avatar
Don Gagne committed
242 243 244 245 246 247
    Q_PROPERTY(bool     flying                  READ flying                                         NOTIFY flyingChanged)       ///< Vehicle is flying
    Q_PROPERTY(bool     landing                 READ landing                                        NOTIFY landingChanged)      ///< Vehicle is in landing pattern (DO_LAND_START)
    Q_PROPERTY(bool     guidedMode              READ guidedMode                 WRITE setGuidedMode NOTIFY guidedModeChanged)   ///< Vehicle is in Guided mode and can respond to guided commands
    Q_PROPERTY(bool     guidedModeSupported     READ guidedModeSupported                            CONSTANT)                   ///< Guided mode commands are supported by this vehicle
    Q_PROPERTY(bool     pauseVehicleSupported   READ pauseVehicleSupported                          CONSTANT)                   ///< Pause vehicle command is supported
    Q_PROPERTY(bool     orbitModeSupported      READ orbitModeSupported                             CONSTANT)                   ///< Orbit mode is supported by this vehicle
Gus Grubba's avatar
Gus Grubba committed
248
    Q_PROPERTY(bool     roiModeSupported        READ roiModeSupported                               CONSTANT)                   ///< Orbit mode is supported by this vehicle
Don Gagne's avatar
Don Gagne committed
249 250
    Q_PROPERTY(bool     takeoffVehicleSupported READ takeoffVehicleSupported                        CONSTANT)                   ///< Guided takeoff supported
    Q_PROPERTY(QString  gotoFlightMode          READ gotoFlightMode                                 CONSTANT)                   ///< Flight mode vehicle is in while performing goto
251

Gus Grubba's avatar
Gus Grubba committed
252 253
    Q_PROPERTY(ParameterManager*        parameterManager    READ parameterManager   CONSTANT)
    Q_PROPERTY(VehicleObjectAvoidance*  objectAvoidance     READ objectAvoidance    CONSTANT)
254

Don Gagne's avatar
Don Gagne committed
255 256 257 258 259
    // FactGroup object model properties

    Q_PROPERTY(Fact* roll               READ roll               CONSTANT)
    Q_PROPERTY(Fact* pitch              READ pitch              CONSTANT)
    Q_PROPERTY(Fact* heading            READ heading            CONSTANT)
DonLakeFlyer's avatar
DonLakeFlyer committed
260 261 262
    Q_PROPERTY(Fact* rollRate           READ rollRate           CONSTANT)
    Q_PROPERTY(Fact* pitchRate          READ pitchRate          CONSTANT)
    Q_PROPERTY(Fact* yawRate            READ yawRate            CONSTANT)
Don Gagne's avatar
Don Gagne committed
263 264 265 266 267
    Q_PROPERTY(Fact* groundSpeed        READ groundSpeed        CONSTANT)
    Q_PROPERTY(Fact* airSpeed           READ airSpeed           CONSTANT)
    Q_PROPERTY(Fact* climbRate          READ climbRate          CONSTANT)
    Q_PROPERTY(Fact* altitudeRelative   READ altitudeRelative   CONSTANT)
    Q_PROPERTY(Fact* altitudeAMSL       READ altitudeAMSL       CONSTANT)
268
    Q_PROPERTY(Fact* flightDistance     READ flightDistance     CONSTANT)
269
    Q_PROPERTY(Fact* distanceToHome     READ distanceToHome     CONSTANT)
270
    Q_PROPERTY(Fact* missionItemIndex   READ missionItemIndex   CONSTANT)
271
    Q_PROPERTY(Fact* headingToNextWP    READ headingToNextWP    CONSTANT)
272
    Q_PROPERTY(Fact* headingToHome      READ headingToHome      CONSTANT)
273
    Q_PROPERTY(Fact* distanceToGCS      READ distanceToGCS      CONSTANT)
274
    Q_PROPERTY(Fact* hobbs              READ hobbs              CONSTANT)
275
    Q_PROPERTY(Fact* throttlePct        READ throttlePct        CONSTANT)
Don Gagne's avatar
Don Gagne committed
276

277 278 279 280 281 282
    Q_PROPERTY(FactGroup*           gps             READ gpsFactGroup               CONSTANT)
    Q_PROPERTY(FactGroup*           wind            READ windFactGroup              CONSTANT)
    Q_PROPERTY(FactGroup*           vibration       READ vibrationFactGroup         CONSTANT)
    Q_PROPERTY(FactGroup*           temperature     READ temperatureFactGroup       CONSTANT)
    Q_PROPERTY(FactGroup*           clock           READ clockFactGroup             CONSTANT)
    Q_PROPERTY(FactGroup*           setpoint        READ setpointFactGroup          CONSTANT)
283
    Q_PROPERTY(FactGroup*           escStatus       READ escStatusFactGroup         CONSTANT)
284 285 286 287
    Q_PROPERTY(FactGroup*           estimatorStatus READ estimatorStatusFactGroup   CONSTANT)
    Q_PROPERTY(FactGroup*           terrain         READ terrainFactGroup           CONSTANT)
    Q_PROPERTY(FactGroup*           distanceSensors READ distanceSensorFactGroup    CONSTANT)
    Q_PROPERTY(QmlObjectListModel*  batteries       READ batteries                  CONSTANT)
Don Gagne's avatar
Don Gagne committed
288

289 290 291 292 293 294 295 296 297
    Q_PROPERTY(int      firmwareMajorVersion        READ firmwareMajorVersion       NOTIFY firmwareVersionChanged)
    Q_PROPERTY(int      firmwareMinorVersion        READ firmwareMinorVersion       NOTIFY firmwareVersionChanged)
    Q_PROPERTY(int      firmwarePatchVersion        READ firmwarePatchVersion       NOTIFY firmwareVersionChanged)
    Q_PROPERTY(int      firmwareVersionType         READ firmwareVersionType        NOTIFY firmwareVersionChanged)
    Q_PROPERTY(QString  firmwareVersionTypeString   READ firmwareVersionTypeString  NOTIFY firmwareVersionChanged)
    Q_PROPERTY(int      firmwareCustomMajorVersion  READ firmwareCustomMajorVersion NOTIFY firmwareCustomVersionChanged)
    Q_PROPERTY(int      firmwareCustomMinorVersion  READ firmwareCustomMinorVersion NOTIFY firmwareCustomVersionChanged)
    Q_PROPERTY(int      firmwareCustomPatchVersion  READ firmwareCustomPatchVersion NOTIFY firmwareCustomVersionChanged)
    Q_PROPERTY(QString  gitHash                     READ gitHash                    NOTIFY gitHashChanged)
Gus Grubba's avatar
Gus Grubba committed
298 299
    Q_PROPERTY(quint64  vehicleUID                  READ vehicleUID                 NOTIFY vehicleUIDChanged)
    Q_PROPERTY(QString  vehicleUIDStr               READ vehicleUIDStr              NOTIFY vehicleUIDChanged)
300

301 302
    /// Resets link status counters
    Q_INVOKABLE void resetCounters  ();
303

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

Gus Grubba's avatar
Gus Grubba committed
307
    Q_INVOKABLE void virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust);
308
    Q_INVOKABLE void disconnectInactiveVehicle();
Don Gagne's avatar
Don Gagne committed
309

Don Gagne's avatar
Don Gagne committed
310
    /// Command vehicle to return to launch
DonLakeFlyer's avatar
DonLakeFlyer committed
311
    Q_INVOKABLE void guidedModeRTL(bool smartRTL);
Don Gagne's avatar
Don Gagne committed
312 313

    /// Command vehicle to land at current location
314
    Q_INVOKABLE void guidedModeLand();
Don Gagne's avatar
Don Gagne committed
315 316

    /// Command vehicle to takeoff from current location
317
    Q_INVOKABLE void guidedModeTakeoff(double altitudeRelative);
Don Gagne's avatar
Don Gagne committed
318

319
    /// @return The minimum takeoff altitude (relative) for guided takeoff.
320
    Q_INVOKABLE double minimumTakeoffAltitude();
321

Don Gagne's avatar
Don Gagne committed
322 323 324
    /// Command vehicle to move to specified location (altitude is included and relative)
    Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);

325 326 327
    /// Command vehicle to change altitude
    ///     @param altitudeChange If > 0, go up by amount specified, if < 0, go down by amount specified
    Q_INVOKABLE void guidedModeChangeAltitude(double altitudeChange);
Don Gagne's avatar
Don Gagne committed
328

329
    /// Command vehicle to orbit given center point
DonLakeFlyer's avatar
DonLakeFlyer committed
330
    ///     @param centerCoord Orit around this point
331
    ///     @param radius Distance from vehicle to centerCoord
DonLakeFlyer's avatar
DonLakeFlyer committed
332 333
    ///     @param amslAltitude Desired vehicle altitude
    Q_INVOKABLE void guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double amslAltitude);
334

Gus Grubba's avatar
Gus Grubba committed
335 336 337
    /// Command vehicle to keep given point as ROI
    ///     @param centerCoord ROI coordinates
    Q_INVOKABLE void guidedModeROI(const QGeoCoordinate& centerCoord);
338
    Q_INVOKABLE void stopGuidedModeROI();
Gus Grubba's avatar
Gus Grubba committed
339

Don Gagne's avatar
Don Gagne committed
340 341
    /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left
    /// in guided mode after pause.
342
    Q_INVOKABLE void pauseVehicle();
Don Gagne's avatar
Don Gagne committed
343 344

    /// Command vehicle to kill all motors no matter what state
345
    Q_INVOKABLE void emergencyStop();
Don Gagne's avatar
Don Gagne committed
346

347
    /// Command vehicle to abort landing
348
    Q_INVOKABLE void abortLanding(double climbOutAltitude);
349

350
    Q_INVOKABLE void startMission();
351

352 353 354
    /// Alter the current mission item on the vehicle
    Q_INVOKABLE void setCurrentMissionSequence(int seq);

355 356 357
    /// Reboot vehicle
    Q_INVOKABLE void rebootVehicle();

dogmaphobic's avatar
dogmaphobic committed
358 359 360
    /// Clear Messages
    Q_INVOKABLE void clearMessages();

Don Gagne's avatar
Don Gagne committed
361
    Q_INVOKABLE void sendPlan(QString planFile);
Don Gagne's avatar
Don Gagne committed
362

363 364 365 366 367
    /// Used to check if running current version is equal or higher than the one being compared.
    //  returns 1 if current > compare, 0 if current == compare, -1 if current < compare
    Q_INVOKABLE int versionCompare(QString& compare);
    Q_INVOKABLE int versionCompare(int major, int minor, int patch);

Don Gagne's avatar
Don Gagne committed
368 369 370
    /// Test motor
    ///     @param motor Motor number, 1-based
    ///     @param percent 0-no power, 100-full power
371 372
    ///     @param timeoutSec Disabled motor after this amount of time
    Q_INVOKABLE void motorTest(int motor, int percent, int timeoutSecs);
Don Gagne's avatar
Don Gagne committed
373

Don Gagne's avatar
Don Gagne committed
374 375
    Q_INVOKABLE void setPIDTuningTelemetryMode(bool pidTuning);

Gus Grubba's avatar
Gus Grubba committed
376 377 378 379
    Q_INVOKABLE void gimbalControlValue (double pitch, double yaw);
    Q_INVOKABLE void gimbalPitchStep    (int direction);
    Q_INVOKABLE void gimbalYawStep      (int direction);
    Q_INVOKABLE void centerGimbal       ();
380
    Q_INVOKABLE void forceArm           ();
381

DoinLakeFlyer's avatar
DoinLakeFlyer committed
382 383 384 385 386 387 388
    /// Sends PARAM_MAP_RC message to vehicle
    Q_INVOKABLE void sendParamMapRC(const QString& paramName, double scale, double centerValue, int tuningID, double minValue, double maxValue);

    /// Clears all PARAM_MAP_RC settings from vehicle
    Q_INVOKABLE void clearAllParamMapRC(void);


389
#if !defined(NO_ARDUPILOT_DIALECT)
390
    Q_INVOKABLE void flashBootloader();
391 392
#endif

393 394 395 396 397 398
    bool    guidedModeSupported     () const;
    bool    pauseVehicleSupported   () const;
    bool    orbitModeSupported      () const;
    bool    roiModeSupported        () const;
    bool    takeoffVehicleSupported () const;
    QString gotoFlightMode          () const;
Don Gagne's avatar
Don Gagne committed
399

400
    // Property accessors
Don Gagne's avatar
Don Gagne committed
401

402 403
    QGeoCoordinate coordinate() { return _coordinate; }
    QGeoCoordinate armedPosition    () { return _armedPosition; }
dogmaphobic's avatar
dogmaphobic committed
404

Gus Grubba's avatar
Gus Grubba committed
405
    void updateFlightDistance(double distance);
406

407 408 409
    bool joystickEnabled            ();
    void setJoystickEnabled         (bool enabled);
    void sendJoystickDataThreadSafe (float roll, float pitch, float yaw, float thrust, quint16 buttons);
dogmaphobic's avatar
dogmaphobic committed
410

411
    // Is vehicle active with respect to current active vehicle in QGC
412
    bool active();
413
    void setActive(bool active);
dogmaphobic's avatar
dogmaphobic committed
414

415
    // Property accesors
416 417 418 419
    int id() { return _id; }
    MAV_AUTOPILOT firmwareType() const { return _firmwareType; }
    MAV_TYPE vehicleType() const { return _vehicleType; }
    Q_INVOKABLE QString vehicleTypeName() const;
dogmaphobic's avatar
dogmaphobic committed
420

Patrick José Pereira's avatar
Patrick José Pereira committed
421
    /// Returns the highest quality link available to the Vehicle. If you need to hold a reference to this link use
422
    /// LinkManager::sharedLinkInterfaceForGet to get QSharedPointer for link.
423
    LinkInterface* priorityLink() { return _priorityLink.data(); }
424 425 426

    /// Sends a message to the specified link
    /// @return true: message sent, false: Link no longer connected
427
    bool sendMessageOnLinkThreadSafe(LinkInterface* link, mavlink_message_t message);
428

429 430 431
    /// 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
432

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

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

Don Gagne's avatar
Don Gagne committed
439
    /// Provides access to the Firmware Plugin for this Vehicle
440
    FirmwarePlugin* firmwarePlugin() { return _firmwarePlugin; }
dogmaphobic's avatar
dogmaphobic committed
441

442 443 444
    MissionManager*     missionManager()    { return _missionManager; }
    GeoFenceManager*    geoFenceManager()   { return _geoFenceManager; }
    RallyPointManager*  rallyPointManager() { return _rallyPointManager; }
dogmaphobic's avatar
dogmaphobic committed
445

446
    QGeoCoordinate homePosition();
dogmaphobic's avatar
dogmaphobic committed
447

Gus Grubba's avatar
Gus Grubba committed
448 449
    bool armed      () { return _armed; }
    void setArmed   (bool armed);
Don Gagne's avatar
Don Gagne committed
450

451 452 453 454
    bool flightModeSetAvailable             ();
    QStringList flightModes                 ();
    QStringList extraJoystickFlightModes    ();
    QString flightMode                      () const;
455
    void setFlightMode                      (const QString& flightMode);
Don Gagne's avatar
Don Gagne committed
456

457 458
    QString priorityLinkName() const;
    QVariantList links() const;
459 460
    void setPriorityLinkByName(const QString& priorityLinkName);

461 462 463 464 465
    bool fixedWing() const;
    bool multiRotor() const;
    bool vtol() const;
    bool rover() const;
    bool sub() const;
466

467 468 469 470 471 472
    bool supportsThrottleModeCenterZero () const;
    bool supportsNegativeThrust         ();
    bool supportsRadio                  () const;
    bool supportsJSButton               () const;
    bool supportsMotorInterference      () const;
    bool supportsTerrainFrame           () const;
473

Don Gagne's avatar
Don Gagne committed
474 475
    void setGuidedMode(bool guidedMode);

476
    QString prearmError() const { return _prearmError; }
Don Gagne's avatar
Don Gagne committed
477 478
    void setPrearmError(const QString& prearmError);

479
    QmlObjectListModel* cameraTriggerPoints () { return &_cameraTriggerPoints; }
dogmaphobic's avatar
dogmaphobic committed
480 481 482

    int  flowImageIndex() { return _flowImageIndex; }

483 484 485 486
    //-- Mavlink Logging
    void startMavlinkLog();
    void stopMavlinkLog();

487 488 489
    /// 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
490 491
    ///     @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
492

493 494 495 496
    // The follow method are used to turn on/off the tracking of settings updates for firmware/vehicle type on offline vehicles.
    void trackFirmwareVehicleTypeChanges(void);
    void stopTrackingFirmwareVehicleTypeChanges(void);

497 498 499 500 501 502
    typedef enum {
        MessageNone,
        MessageNormal,
        MessageWarning,
        MessageError
    } MessageType_t;
dogmaphobic's avatar
dogmaphobic committed
503

504 505 506 507 508 509
    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; }
510
    QString         formattedMessages       ();
511
    QString         latestError             () { return _latestError; }
512 513 514
    float           latitude                () { return static_cast<float>(_coordinate.latitude()); }
    float           longitude               () { return static_cast<float>(_coordinate.longitude()); }
    bool            mavPresent              () { return _mav != nullptr; }
515 516 517 518 519 520 521 522 523 524
    int             rcRSSI                  () { return _rcRSSI; }
    bool            px4Firmware             () const { return _firmwareType == MAV_AUTOPILOT_PX4; }
    bool            apmFirmware             () const { return _firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA; }
    bool            genericFirmware         () const { return !px4Firmware() && !apmFirmware(); }
    bool            connectionLost          () const { return _connectionLost; }
    bool            connectionLostEnabled   () const { return _connectionLostEnabled; }
    uint            messagesReceived        () { return _messagesReceived; }
    uint            messagesSent            () { return _messagesSent; }
    uint            messagesLost            () { return _messagesLost; }
    bool            flying                  () const { return _flying; }
525
    bool            landing                 () const { return _landing; }
526
    bool            guidedMode              () const;
527
    bool            vtolInFwdFlight         () const { return _vtolInFwdFlight; }
528 529 530
    uint8_t         baseMode                () const { return _base_mode; }
    uint32_t        customMode              () const { return _custom_mode; }
    bool            isOfflineEditingVehicle () const { return _offlineEditingVehicle; }
531 532
    QString         brandImageIndoor        () const;
    QString         brandImageOutdoor       () const;
533 534 535 536
    int             sensorsPresentBits      () const { return static_cast<int>(_onboardControlSensorsPresent); }
    int             sensorsEnabledBits      () const { return static_cast<int>(_onboardControlSensorsEnabled); }
    int             sensorsHealthBits       () const { return static_cast<int>(_onboardControlSensorsHealth); }
    int             sensorsUnhealthyBits    () const { return static_cast<int>(_onboardControlSensorsUnhealthy); }
537
    QString         missionFlightMode       () const;
538
    QString         pauseFlightMode         () const;
539
    QString         rtlFlightMode           () const;
DonLakeFlyer's avatar
DonLakeFlyer committed
540 541
    QString         smartRTLFlightMode      () const;
    bool            supportsSmartRTL        () const;
542
    QString         landFlightMode          () const;
543
    QString         takeControlFlightMode   () const;
544
    QString         followFlightMode        () const;
DonLakeFlyer's avatar
DonLakeFlyer committed
545 546
    double          defaultCruiseSpeed      () const { return _defaultCruiseSpeed; }
    double          defaultHoverSpeed       () const { return _defaultHoverSpeed; }
547
    QString         firmwareTypeString      () const;
Gus Grubba's avatar
Gus Grubba committed
548 549 550 551 552 553
    QString         vehicleTypeString       () const;
    int             telemetryRRSSI          () { return _telemetryRRSSI; }
    int             telemetryLRSSI          () { return _telemetryLRSSI; }
    unsigned int    telemetryRXErrors       () { return _telemetryRXErrors; }
    unsigned int    telemetryFixed          () { return _telemetryFixed; }
    unsigned int    telemetryTXBuffer       () { return _telemetryTXBuffer; }
554 555
    int             telemetryLNoise         () { return _telemetryLNoise; }
    int             telemetryRNoise         () { return _telemetryRNoise; }
556
    bool            autoDisarm              ();
557
    bool            highLatencyLink         () const { return _highLatencyLink; }
558 559
    bool            orbitActive             () const { return _orbitActive; }
    QGCMapCircle*   orbitMapCircle          () { return &_orbitMapCircle; }
560 561 562 563
    bool            readyToFlyAvailable     () { return _readyToFlyAvailable; }
    bool            readyToFly              () { return _readyToFly; }
    bool            allSensorsHealthy       () { return _allSensorsHealthy; }
    QObject*        sysStatusSensorInfo     () { return &_sysStatusSensorInfo; }
564

565 566 567
    /// Get the maximum MAVLink protocol version supported
    /// @return the maximum version
    unsigned        maxProtoVersion         () const { return _maxProtoVersion; }
568

569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586
    enum CalibrationType {
        CalibrationRadio,
        CalibrationGyro,
        CalibrationMag,
        CalibrationAccel,
        CalibrationLevel,
        CalibrationEsc,
        CalibrationCopyTrims,
        CalibrationAPMCompassMot,
        CalibrationAPMPressureAirspeed,
        CalibrationAPMPreFlight,
        CalibrationPX4Airspeed,
        CalibrationPX4Pressure,
    };

    void startCalibration   (CalibrationType calType);
    void stopCalibration    (void);

587 588 589
    void startUAVCANBusConfig(void);
    void stopUAVCANBusConfig(void);

590 591 592 593 594 595 596 597 598 599 600 601 602
    Fact* roll                              () { return &_rollFact; }
    Fact* pitch                             () { return &_pitchFact; }
    Fact* heading                           () { return &_headingFact; }
    Fact* rollRate                          () { return &_rollRateFact; }
    Fact* pitchRate                         () { return &_pitchRateFact; }
    Fact* yawRate                           () { return &_yawRateFact; }
    Fact* airSpeed                          () { return &_airSpeedFact; }
    Fact* groundSpeed                       () { return &_groundSpeedFact; }
    Fact* climbRate                         () { return &_climbRateFact; }
    Fact* altitudeRelative                  () { return &_altitudeRelativeFact; }
    Fact* altitudeAMSL                      () { return &_altitudeAMSLFact; }
    Fact* flightDistance                    () { return &_flightDistanceFact; }
    Fact* distanceToHome                    () { return &_distanceToHomeFact; }
603
    Fact* missionItemIndex                  () { return &_missionItemIndexFact; }
604 605 606 607 608 609 610 611 612 613 614 615 616
    Fact* headingToNextWP                   () { return &_headingToNextWPFact; }
    Fact* headingToHome                     () { return &_headingToHomeFact; }
    Fact* distanceToGCS                     () { return &_distanceToGCSFact; }
    Fact* hobbs                             () { return &_hobbsFact; }
    Fact* throttlePct                       () { return &_throttlePctFact; }

    FactGroup* gpsFactGroup                 () { return &_gpsFactGroup; }
    FactGroup* windFactGroup                () { return &_windFactGroup; }
    FactGroup* vibrationFactGroup           () { return &_vibrationFactGroup; }
    FactGroup* temperatureFactGroup         () { return &_temperatureFactGroup; }
    FactGroup* clockFactGroup               () { return &_clockFactGroup; }
    FactGroup* setpointFactGroup            () { return &_setpointFactGroup; }
    FactGroup* distanceSensorFactGroup      () { return &_distanceSensorFactGroup; }
617
    FactGroup* escStatusFactGroup           () { return &_escStatusFactGroup; }
618
    FactGroup* estimatorStatusFactGroup     () { return &_estimatorStatusFactGroup; }
619
    FactGroup* terrainFactGroup             () { return &_terrainFactGroup; }
620
    QmlObjectListModel* batteries           () { return &_batteryFactGroupListModel; }
Don Gagne's avatar
Don Gagne committed
621

622
    void setConnectionLostEnabled(bool connectionLostEnabled);
623

624 625 626 627
    ParameterManager*               parameterManager    () { return _parameterManager; }
    ParameterManager*               parameterManager    () const { return _parameterManager; }
    FTPManager*                     ftpManager          () { return _ftpManager; }
    ComponentInformationManager*    compInfoManager     () { return _componentInformationManager; }
628
    VehicleObjectAvoidance* objectAvoidance     () { return _objectAvoidance; }
dogmaphobic's avatar
dogmaphobic committed
629

Don Gagne's avatar
Don Gagne committed
630 631
    static const int cMaxRcChannels = 18;

Don Gagne's avatar
Don Gagne committed
632
    bool containsLink(LinkInterface* link) { return _links.contains(link); }
633 634 635

    /// Sends the specified MAV_CMD to the vehicle. If no Ack is received command will be retried. If a sendMavCommand is already in progress
    /// the command will be queued and sent when the previous command completes.
636
    ///     @param compId Component to send to.
637 638
    ///     @param command MAV_CMD to send
    ///     @param showError true: Display error to user if command failed, false:  no error shown
DonLakeFlyer's avatar
DonLakeFlyer committed
639
    /// Signals: mavCommandResult on success or failure
640 641
    void sendMavCommand(int compId, MAV_CMD command, bool showError, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);
    void sendMavCommandInt(int compId, MAV_CMD command, MAV_FRAME frame, bool showError, float param1, float param2, float param3, float param4, double param5, double param6, float param7);
Don Gagne's avatar
Don Gagne committed
642

643
    /// Same as sendMavCommand but available from Qml.
644
    Q_INVOKABLE void sendCommand(int compId, int command, bool showError, double param1 = 0.0, double param2 = 0.0, double param3 = 0.0, double param4 = 0.0, double param5 = 0.0, double param6 = 0.0, double param7 = 0.0)
645 646
    {
        sendMavCommand(
647
            compId, static_cast<MAV_CMD>(command),
648 649 650 651 652 653 654 655 656
            showError,
            static_cast<float>(param1),
            static_cast<float>(param2),
            static_cast<float>(param3),
            static_cast<float>(param4),
            static_cast<float>(param5),
            static_cast<float>(param6),
            static_cast<float>(param7));
    }
657

658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687
    /// Callback for sendMavCommandWithHandle
    ///     @param resultHandleData     Opaque data passed in to sendMavCommand call
    ///     @param commandResult        Ack result for command send
    ///     @param noReponseFromVehicle true: The vehicle did not responsed to the COMMAND_LONG message
    typedef void (*MavCmdResultHandler)(void* resultHandlerData, int compId, MAV_RESULT commandResult, bool noResponsefromVehicle);

    /// Sends the command and calls the callback with the result
    ///     @param resultHandler    Callback for result, nullptr for no callback
    ///     @param resultHandleData Opaque data passed through callback
    void sendMavCommandWithHandler(MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f);

    typedef enum {
        RequestMessageNoFailure,
        RequestMessageFailureCommandError,
        RequestMessageFailureCommandNotAcked,
        RequestMessageFailureMessageNotReceived,
    } RequestMessageResultHandlerFailureCode_t;

    /// Callback for requestMessage
    ///     @param resultHandlerData    Opaque data which was passed in to requestMessage call
    ///     @param commandResult        Result from ack to REQUEST_MESSAGE command
    ///     @param noResponseToCommand  true: Vehicle never acked the REQUEST_MESSAGE command
    ///     @param messageNotReceived   true: Vehicle never sent requested message
    typedef void (*RequestMessageResultHandler)(void* resultHandlerData, MAV_RESULT commandResult, RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t& message);

    /// Requests the vehicle to send the specified message. Will retry a number of times.
    ///     @param resultHandler Callback for result
    ///     @param resultHandlerData Opaque data passed back to resultHandler
    void requestMessage(RequestMessageResultHandler resultHandler, void* resultHandlerData, int compId, int messageId, float param1 = 0.0f, float param2 = 0.0f, float param3 = 0.0f, float param4 = 0.0f, float param5 = 0.0f);

688 689 690 691 692 693 694 695
    int firmwareMajorVersion() const { return _firmwareMajorVersion; }
    int firmwareMinorVersion() const { return _firmwareMinorVersion; }
    int firmwarePatchVersion() const { return _firmwarePatchVersion; }
    int firmwareVersionType() const { return _firmwareVersionType; }
    int firmwareCustomMajorVersion() const { return _firmwareCustomMajorVersion; }
    int firmwareCustomMinorVersion() const { return _firmwareCustomMinorVersion; }
    int firmwareCustomPatchVersion() const { return _firmwareCustomPatchVersion; }
    QString firmwareVersionTypeString() const;
696
    void setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType = FIRMWARE_VERSION_TYPE_OFFICIAL);
697
    void setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion);
698
    static const int versionNotSetValue = -1;
Don Gagne's avatar
Don Gagne committed
699

700 701
    QString gitHash() const { return _gitHash; }
    quint64 vehicleUID() const { return _uid; }
Gus Grubba's avatar
Gus Grubba committed
702
    QString vehicleUIDStr();
703

704
    bool soloFirmware() const { return _soloFirmware; }
705 706
    void setSoloFirmware(bool soloFirmware);

707
    int defaultComponentId() { return _defaultComponentId; }
708 709 710

    /// Sets the default component id for an offline editing vehicle
    void setOfflineEditingDefaultComponentId(int defaultComponentId);
Don Gagne's avatar
Don Gagne committed
711

Don Gagne's avatar
Don Gagne committed
712
    /// @return -1 = Unknown, Number of motors on vehicle
713
    int motorCount();
Don Gagne's avatar
Don Gagne committed
714 715

    /// @return true: Motors are coaxial like an X8 config, false: Quadcopter for example
716
    bool coaxialMotors();
Don Gagne's avatar
Don Gagne committed
717 718

    /// @return true: X confiuration, false: Plus configuration
719
    bool xConfigMotors();
Don Gagne's avatar
Don Gagne committed
720

721
    /// @return Firmware plugin instance data associated with this Vehicle
722
    QObject* firmwarePluginInstanceData() { return _firmwarePluginInstanceData; }
723 724 725 726 727

    /// Sets the firmware plugin instance data associated with this Vehicle. This object will be parented to the Vehicle
    /// and destroyed when the vehicle goes away.
    void setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData);

728 729 730 731
    QString vehicleImageOpaque  () const;
    QString vehicleImageOutline () const;
    QString vehicleImageCompass () const;

732 733
    const QVariantList&         toolIndicators      ();
    const QVariantList&         modeIndicators      ();
734
    const QVariantList&         staticCameraList    () const;
735

736
    bool capabilitiesKnown      () const { return _capabilityBitsKnown; }
737
    uint64_t capabilityBits     () const { return _capabilityBits; }    // Change signalled by capabilityBitsChanged
738

739
    QGCCameraManager*           cameraManager       () { return _cameraManager; }
740
    QString                     hobbsMeter          ();
741

DonLakeFlyer's avatar
DonLakeFlyer committed
742 743
    /// The vehicle is responsible for making the initial request for the Plan.
    /// @return: true: initial request is complete, false: initial request is still in progress;
744
    bool initialPlanRequestComplete() const { return _initialPlanRequestComplete; }
DonLakeFlyer's avatar
DonLakeFlyer committed
745

746
    void forceInitialPlanRequestComplete();
747

748 749
    void _setFlying(bool flying);
    void _setLanding(bool landing);
DonLakeFlyer's avatar
DonLakeFlyer committed
750
    void _setHomePosition(QGeoCoordinate& homeCoord);
751 752
    void _setMaxProtoVersion(unsigned version);
    void _setMaxProtoVersionFromBothSources();
DonLakeFlyer's avatar
DonLakeFlyer committed
753

754 755 756
    /// Vehicle is about to be deleted
    void prepareDelete();

757 758 759 760 761
    quint64     mavlinkSentCount        () { return _mavlinkSentCount; }        /// Calculated total number of messages sent to us
    quint64     mavlinkReceivedCount    () { return _mavlinkReceivedCount; }    /// Total number of sucessful messages received
    quint64     mavlinkLossCount        () { return _mavlinkLossCount; }        /// Total number of lost messages
    float       mavlinkLossPercent      () { return _mavlinkLossPercent; }      /// Running loss rate

Gus Grubba's avatar
Gus Grubba committed
762 763 764 765
    qreal       gimbalRoll              () { return static_cast<qreal>(_curGimbalRoll);}
    qreal       gimbalPitch             () { return static_cast<qreal>(_curGimbalPitch); }
    qreal       gimbalYaw               () { return static_cast<qreal>(_curGinmbalYaw); }
    bool        gimbalData              () { return _haveGimbalData; }
766
    bool        isROIEnabled            () { return _isROIEnabled; }
Gus Grubba's avatar
Gus Grubba committed
767

768 769 770
    CheckList   checkListState          () { return _checkListState; }
    void        setCheckListState       (CheckList cl)  { _checkListState = cl; emit checkListStateChanged(); }

Gus Grubba's avatar
Gus Grubba committed
771
public slots:
772
    void setVtolInFwdFlight                 (bool vtolInFwdFlight);
773 774
    void _offlineFirmwareTypeSettingChanged (QVariant varFirmwareType); // Should only be used by MissionControler to set firmware from Plan file
    void _offlineVehicleTypeSettingChanged  (QVariant varVehicleType);  // Should only be used by MissionController to set vehicle type from Plan file
Gus Grubba's avatar
Gus Grubba committed
775

776
signals:
777 778 779 780 781 782
    void allLinksInactive               (Vehicle* vehicle);
    void coordinateChanged              (QGeoCoordinate coordinate);
    void joystickEnabledChanged         (bool enabled);
    void activeChanged                  (bool active);
    void mavlinkMessageReceived         (const mavlink_message_t& message);
    void homePositionChanged            (const QGeoCoordinate& homePosition);
783
    void armedPositionChanged();
784 785 786 787 788 789 790 791 792 793 794 795 796 797 798
    void armedChanged                   (bool armed);
    void flightModeChanged              (const QString& flightMode);
    void connectionLostChanged          (bool connectionLost);
    void connectionLostEnabledChanged   (bool connectionLostEnabled);
    void autoDisconnectChanged          (bool autoDisconnectChanged);
    void flyingChanged                  (bool flying);
    void landingChanged                 (bool landing);
    void guidedModeChanged              (bool guidedMode);
    void vtolInFwdFlightChanged         (bool vtolInFwdFlight);
    void prearmErrorChanged             (const QString& prearmError);
    void soloFirmwareChanged            (bool soloFirmware);
    void defaultCruiseSpeedChanged      (double cruiseSpeed);
    void defaultHoverSpeedChanged       (double hoverSpeed);
    void firmwareTypeChanged            ();
    void vehicleTypeChanged             ();
799
    void cameraManagerChanged           ();
800 801
    void hobbsMeterChanged              ();
    void capabilitiesKnownChanged       (bool capabilitiesKnown);
Don Gagne's avatar
Don Gagne committed
802
    void initialPlanRequestCompleteChanged(bool initialPlanRequestComplete);
803
    void capabilityBitsChanged          (uint64_t capabilityBits);
804 805
    void toolIndicatorsChanged          ();
    void modeIndicatorsChanged          ();
806 807 808 809 810 811 812 813 814 815 816 817
    void highLatencyLinkChanged         (bool highLatencyLink);
    void priorityLinkNameChanged        (const QString& priorityLinkName);
    void linksChanged                   ();
    void linksPropertiesChanged         ();
    void textMessageReceived            (int uasid, int componentid, int severity, QString text);
    void checkListStateChanged          ();
    void messagesReceivedChanged        ();
    void messagesSentChanged            ();
    void messagesLostChanged            ();
    void messageTypeChanged             ();
    void newMessageCountChanged         ();
    void messageCountChanged            ();
818 819
    void formattedMessagesChanged       ();
    void newFormattedMessage            (QString formattedMessage);
820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838
    void latestErrorChanged             ();
    void longitudeChanged               ();
    void currentConfigChanged           ();
    void flowImageIndexChanged          ();
    void rcRSSIChanged                  (int rcRSSI);
    void telemetryRRSSIChanged          (int value);
    void telemetryLRSSIChanged          (int value);
    void telemetryRXErrorsChanged       (unsigned int value);
    void telemetryFixedChanged          (unsigned int value);
    void telemetryTXBufferChanged       (unsigned int value);
    void telemetryLNoiseChanged         (int value);
    void telemetryRNoiseChanged         (int value);
    void autoDisarmChanged              ();
    void flightModesChanged             ();
    void sensorsPresentBitsChanged      (int sensorsPresentBits);
    void sensorsEnabledBitsChanged      (int sensorsEnabledBits);
    void sensorsHealthBitsChanged       (int sensorsHealthBits);
    void sensorsUnhealthyBitsChanged    (int sensorsUnhealthyBits);
    void orbitActiveChanged             (bool orbitActive);
839 840 841
    void readyToFlyAvailableChanged     (bool readyToFlyAvailable);
    void readyToFlyChanged              (bool readyToFy);
    void allSensorsHealthyChanged       (bool allSensorsHealthy);
842 843 844 845 846

    void firmwareVersionChanged         ();
    void firmwareCustomVersionChanged   ();
    void gitHashChanged                 (QString hash);
    void vehicleUIDChanged              ();
847

848
    /// New RC channel values coming from RC_CHANNELS message
Don Gagne's avatar
Don Gagne committed
849 850
    ///     @param channelCount Number of available channels, cMaxRcChannels max
    ///     @param pwmValues -1 signals channel not available
851
    void rcChannelsChanged              (int channelCount, int pwmValues[cMaxRcChannels]);
Don Gagne's avatar
Don Gagne committed
852 853

    /// Remote control RSSI changed  (0% - 100%)
854
    void remoteControlRSSIChanged       (uint8_t rssi);
Don Gagne's avatar
Don Gagne committed
855

856 857 858 859
    void mavlinkRawImu                  (mavlink_message_t message);
    void mavlinkScaledImu1              (mavlink_message_t message);
    void mavlinkScaledImu2              (mavlink_message_t message);
    void mavlinkScaledImu3              (mavlink_message_t message);
860

861
    // Mavlink Log Download
862
    void mavlinkLogData                 (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked);
863

864 865 866 867 868 869
    /// Signalled in response to usage of sendMavCommand
    ///     @param vehicleId Vehicle which command was sent to
    ///     @param component Component which command was sent to
    ///     @param command MAV_CMD Command which was sent
    ///     @param result MAV_RESULT returned in ack
    ///     @param noResponseFromVehicle true: vehicle did not respond to command, false: vehicle responsed, MAV_RESULT in result
870
    void mavCommandResult               (int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
871

872
    // MAVlink Serial Data
873
    void mavlinkSerialControl           (uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, QByteArray data);
874

875
    // MAVLink protocol version
876 877
    void requestProtocolVersion         (unsigned version);
    void mavlinkStatusChanged           ();
Gus Grubba's avatar
Gus Grubba committed
878

879 880 881 882 883
    void gimbalRollChanged              ();
    void gimbalPitchChanged             ();
    void gimbalYawChanged               ();
    void gimbalDataChanged              ();
    void isROIEnabledChanged            ();
884
    void initialConnectComplete         ();
885

886
private slots:
887 888 889 890 891 892 893
    void _mavlinkMessageReceived        (LinkInterface* link, mavlink_message_t message);
    void _linkInactiveOrDeleted         (LinkInterface* link);
    void _sendMessageMultipleNext       ();
    void _parametersReady               (bool parametersReady);
    void _remoteControlRSSIChanged      (uint8_t rssi);
    void _handleFlightModeChanged       (const QString& flightMode);
    void _announceArmedChanged          (bool armed);
894 895
    void _offlineCruiseSpeedSettingChanged(QVariant value);
    void _offlineHoverSpeedSettingChanged(QVariant value);
896
    void _updateHighLatencyLink         (bool sendCommand = true);
897

898 899
    void _handleTextMessage             (int newCount);
    void _handletextMessageReceived     (UASMessage* message);
dogmaphobic's avatar
dogmaphobic committed
900
    /** @brief A new camera image has arrived */
901 902
    void _imageReady                    (UASInterface* uas);
    void _prearmErrorTimeout            ();
903 904 905
    void _firstMissionLoadComplete           ();
    void _firstGeoFenceLoadComplete          ();
    void _firstRallyPointLoadComplete        ();
906 907 908
    void _sendMavCommandAgain           ();
    void _clearCameraTriggerPoints      ();
    void _updateDistanceHeadingToHome   ();
909
    void _updateMissionItemIndex        ();
910 911 912 913 914 915 916 917 918 919
    void _updateHeadingToNextWP         ();
    void _updateDistanceToGCS           ();
    void _updateHobbsMeter              ();
    void _vehicleParamLoaded            (bool ready);
    void _sendQGCTimeToVehicle          ();
    void _mavlinkMessageStatus          (int uasId, uint64_t totalSent, uint64_t totalReceived, uint64_t totalLoss, float lossPercent);

    void _trafficUpdate                 (bool alert, QString traffic_id, QString vehicle_id, QGeoCoordinate location, float heading);
    void _orbitTelemetryTimeout         ();
    void _updateFlightTime              ();
920

921
private:
922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941
    bool _containsLink                  (LinkInterface* link);
    void _addLink                       (LinkInterface* link);
    void _joystickChanged               (Joystick* joystick);
    void _loadSettings                  ();
    void _saveSettings                  ();
    void _startJoystick                 (bool start);
    void _handlePing                    (LinkInterface* link, mavlink_message_t& message);
    void _handleHomePosition            (mavlink_message_t& message);
    void _handleHeartbeat               (mavlink_message_t& message);
    void _handleRadioStatus             (mavlink_message_t& message);
    void _handleRCChannels              (mavlink_message_t& message);
    void _handleBatteryStatus           (mavlink_message_t& message);
    void _handleSysStatus               (mavlink_message_t& message);
    void _handleExtendedSysState        (mavlink_message_t& message);
    void _handleCommandAck              (mavlink_message_t& message);
    void _handleCommandLong             (mavlink_message_t& message);
    void _handleGpsRawInt               (mavlink_message_t& message);
    void _handleGlobalPositionInt       (mavlink_message_t& message);
    void _handleAltitude                (mavlink_message_t& message);
    void _handleVfrHud                  (mavlink_message_t& message);
942
    void _handleHighLatency             (mavlink_message_t& message);
943 944 945 946
    void _handleHighLatency2            (mavlink_message_t& message);
    void _handleAttitudeWorker          (double rollRadians, double pitchRadians, double yawRadians);
    void _handleAttitude                (mavlink_message_t& message);
    void _handleAttitudeQuaternion      (mavlink_message_t& message);
947
    void _handleStatusText              (mavlink_message_t& message);
948 949 950 951
    void _handleOrbitExecutionStatus    (const mavlink_message_t& message);
    void _handleMessageInterval         (const mavlink_message_t& message);
    void _handleGimbalOrientation       (const mavlink_message_t& message);
    void _handleObstacleDistance        (const mavlink_message_t& message);
952 953
    // ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT)
954
    void _handleCameraFeedback          (const mavlink_message_t& message);
955
#endif
956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978
    void _handleCameraImageCaptured     (const mavlink_message_t& message);
    void _handleADSBVehicle             (const mavlink_message_t& message);
    void _missionManagerError           (int errorCode, const QString& errorMsg);
    void _geoFenceManagerError          (int errorCode, const QString& errorMsg);
    void _rallyPointManagerError        (int errorCode, const QString& errorMsg);
    void _linkActiveChanged             (LinkInterface* link, bool active, int vehicleID);
    void _say                           (const QString& text);
    QString _vehicleIdSpeech            ();
    void _handleMavlinkLoggingData      (mavlink_message_t& message);
    void _handleMavlinkLoggingDataAcked (mavlink_message_t& message);
    void _ackMavlinkLogData             (uint16_t sequence);
    void _sendNextQueuedMavCommand      ();
    void _updatePriorityLink            (bool updateActive, bool sendCommand);
    void _commonInit                    ();
    void _setupAutoDisarmSignalling     ();
    void _setCapabilities               (uint64_t capabilityBits);
    void _updateArmed                   (bool armed);
    bool _apmArmingNotRequired          ();
    void _pidTuningAdjustRates          (bool setRatesForTuning);
    void _initializeCsv                 ();
    void _writeCsvLine                  ();
    void _flightTimerStart              ();
    void _flightTimerStop               ();
979 980
    void _chunkedStatusTextTimeout      (void);
    void _chunkedStatusTextCompleted    (uint8_t compId);
Don Gagne's avatar
Don Gagne committed
981

982
    int     _id;                    ///< Mavlink system id
983
    int     _defaultComponentId;
984
    bool    _active;
985
    bool    _offlineEditingVehicle; ///< This Vehicle is a "disconnected" vehicle for ui use while offline editing
dogmaphobic's avatar
dogmaphobic committed
986

987
    MAV_AUTOPILOT       _firmwareType;
988
    MAV_TYPE            _vehicleType;
989
    FirmwarePlugin*     _firmwarePlugin;
990
    QObject*            _firmwarePluginInstanceData;
991
    AutoPilotPlugin*    _autopilotPlugin;
Don Gagne's avatar
Don Gagne committed
992
    MAVLinkProtocol*    _mavlink;
993
    bool                _soloFirmware;
994
    QGCToolbox*         _toolbox;
995
    SettingsManager*    _settingsManager;
dogmaphobic's avatar
dogmaphobic committed
996

997 998
    QTimer              _csvLogTimer;
    QFile               _csvLogFile;
999

Don Gagne's avatar
Don Gagne committed
1000
    QList<LinkInterface*> _links;
dogmaphobic's avatar
dogmaphobic committed
1001

1002
    bool            _joystickEnabled;
dogmaphobic's avatar
dogmaphobic committed
1003

1004
    UAS* _uas;
dogmaphobic's avatar
dogmaphobic committed
1005

1006
    QGeoCoordinate  _coordinate;
1007
    QGeoCoordinate  _homePosition;
1008
    QGeoCoordinate  _armedPosition;
dogmaphobic's avatar
dogmaphobic committed
1009

1010 1011 1012 1013 1014 1015 1016 1017 1018
    UASInterface*   _mav;
    int             _currentMessageCount;
    int             _messageCount;
    int             _currentErrorCount;
    int             _currentWarningCount;
    int             _currentNormalCount;
    MessageType_t   _currentMessageType;
    QString         _latestError;
    int             _updateCount;
Don Gagne's avatar
Don Gagne committed
1019 1020
    int             _rcRSSI;
    double          _rcRSSIstore;
Don Gagne's avatar
Don Gagne committed
1021
    bool            _autoDisconnect;    ///< true: Automatically disconnect vehicle when last connection goes away or lost heartbeat
Don Gagne's avatar
Don Gagne committed
1022
    bool            _flying;
1023
    bool            _landing;
1024
    bool            _vtolInFwdFlight;
1025 1026 1027 1028
    uint32_t        _onboardControlSensorsPresent;
    uint32_t        _onboardControlSensorsEnabled;
    uint32_t        _onboardControlSensorsHealth;
    uint32_t        _onboardControlSensorsUnhealthy;
1029 1030 1031
    bool            _gpsRawIntMessageAvailable              = false;
    bool            _globalPositionIntMessageAvailable      = false;
    bool            _altitudeMessageAvailable               = false;
DonLakeFlyer's avatar
DonLakeFlyer committed
1032 1033
    double          _defaultCruiseSpeed;
    double          _defaultHoverSpeed;
Gus Grubba's avatar
Gus Grubba committed
1034 1035 1036 1037 1038
    int             _telemetryRRSSI;
    int             _telemetryLRSSI;
    uint32_t        _telemetryRXErrors;
    uint32_t        _telemetryFixed;
    uint32_t        _telemetryTXBuffer;
1039 1040
    int             _telemetryLNoise;
    int             _telemetryRNoise;
1041 1042 1043 1044
    bool            _mavlinkProtocolRequestComplete         = false;
    unsigned        _mavlinkProtocolRequestMaxProtoVersion  = 0;
    unsigned        _maxProtoVersion                        = 0;
    bool            _capabilityBitsKnown                    = false;
1045
    uint64_t        _capabilityBits;
1046
    bool            _highLatencyLink;
1047
    bool            _receivingAttitudeQuaternion;
1048 1049 1050 1051 1052 1053
    CheckList       _checkListState                         = CheckListNotSetup;
    bool            _readyToFlyAvailable                    = false;
    bool            _readyToFly                             = false;
    bool            _allSensorsHealthy                      = true;

    SysStatusSensorInfo _sysStatusSensorInfo;
dogmaphobic's avatar
dogmaphobic committed
1054

1055
    QGCCameraManager* _cameraManager = nullptr;
1056

Don Gagne's avatar
Don Gagne committed
1057 1058 1059 1060
    QString             _prearmError;
    QTimer              _prearmErrorTimer;
    static const int    _prearmErrorTimeoutMSecs = 35 * 1000;   ///< Take away prearm error after 35 seconds

1061 1062 1063 1064
    // Lost connection handling
    bool                _connectionLost;
    bool                _connectionLostEnabled;

DonLakeFlyer's avatar
DonLakeFlyer committed
1065 1066
    bool                _initialPlanRequestComplete;

Don Gagne's avatar
Don Gagne committed
1067
    MissionManager*     _missionManager;
1068
    GeoFenceManager*    _geoFenceManager;
1069
    RallyPointManager*  _rallyPointManager;
1070

1071
    ParameterManager*               _parameterManager               = nullptr;
1072
    FTPManager*                     _ftpManager                     = nullptr;
1073 1074 1075
    ComponentInformationManager*    _componentInformationManager    = nullptr;
    InitialConnectStateMachine*     _initialConnectStateMachine     = nullptr;
    VehicleObjectAvoidance*         _objectAvoidance                = nullptr;
1076

1077
#if defined(QGC_AIRMAP_ENABLED)
1078
    AirspaceVehicleManager* _airspaceVehicleManager;
1079
#endif
dogmaphobic's avatar
dogmaphobic committed
1080

Don Gagne's avatar
Don Gagne committed
1081 1082 1083
    bool    _armed;         ///< true: vehicle is armed
    uint8_t _base_mode;     ///< base_mode from HEARTBEAT
    uint32_t _custom_mode;  ///< custom_mode from HEARTBEAT
1084 1085 1086 1087 1088 1089

    /// 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
1090

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

1093 1094
    static const int _sendMessageMultipleRetries = 5;
    static const int _sendMessageMultipleIntraMessageDelay = 500;
dogmaphobic's avatar
dogmaphobic committed
1095

1096 1097
    QTimer  _sendMultipleTimer;
    int     _nextSendMessageMultipleIndex;
dogmaphobic's avatar
dogmaphobic committed
1098

1099
    QElapsedTimer                   _flightTimer;
DonLakeFlyer's avatar
DonLakeFlyer committed
1100
    QTimer                          _flightTimeUpdater;
1101 1102
    TrajectoryPoints*               _trajectoryPoints;
    QmlObjectListModel              _cameraTriggerPoints;
1103
    //QMap<QString, ADSBVehicle*>     _trafficVehicleMap;
1104

1105
    // Toolbox references
dogmaphobic's avatar
dogmaphobic committed
1106 1107 1108 1109 1110
    FirmwarePluginManager*      _firmwarePluginManager;
    JoystickManager*            _joystickManager;

    int                         _flowImageIndex;

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

1113 1114 1115 1116 1117 1118 1119
    uint                _messagesReceived;
    uint                _messagesSent;
    uint                _messagesLost;
    uint8_t             _messageSeq;
    uint8_t             _compID;
    bool                _heardFrom;

Gus Grubba's avatar
Gus Grubba committed
1120 1121 1122 1123
    float               _curGimbalRoll  = 0.0f;
    float               _curGimbalPitch = 0.0f;
    float               _curGinmbalYaw  = 0.0f;
    bool                _haveGimbalData = false;
1124
    bool                _isROIEnabled   = false;
Gus Grubba's avatar
Gus Grubba committed
1125 1126
    Joystick*           _activeJoystick = nullptr;

1127
    bool _checkLatestStableFWDone = false;
Don Gagne's avatar
Don Gagne committed
1128 1129 1130
    int _firmwareMajorVersion;
    int _firmwareMinorVersion;
    int _firmwarePatchVersion;
1131 1132 1133
    int _firmwareCustomMajorVersion;
    int _firmwareCustomMinorVersion;
    int _firmwareCustomPatchVersion;
1134
    FIRMWARE_VERSION_TYPE _firmwareVersionType;
Don Gagne's avatar
Don Gagne committed
1135

1136
    QString _gitHash;
Gus Grubba's avatar
Gus Grubba committed
1137
    quint64 _uid;
1138

1139
    SharedLinkInterfacePointer _priorityLink;  // We always keep a reference to the priority link to manage shutdown ordering
1140
    bool _priorityLinkCommanded;
1141

1142 1143 1144 1145 1146
    uint64_t    _mavlinkSentCount       = 0;
    uint64_t    _mavlinkReceivedCount   = 0;
    uint64_t    _mavlinkLossCount       = 0;
    float       _mavlinkLossPercent     = 0.0f;

1147 1148
    QMap<QString, QTime> _noisySpokenPrearmMap; ///< Used to prevent PreArm messages from being spoken too often

1149 1150 1151 1152 1153 1154
    // Orbit status values
    bool            _orbitActive;
    QGCMapCircle    _orbitMapCircle;
    QTimer          _orbitTelemetryTimer;
    static const int _orbitTelemetryTimeoutMsecs = 3000; // No telemetry for this amount and orbit will go inactive

1155 1156 1157 1158 1159 1160
    // PID Tuning telemetry mode
    bool            _pidTuningTelemetryMode;
    bool            _pidTuningWaitingForRates;
    QList<int>      _pidTuningMessages;
    QMap<int, int>  _pidTuningMessageRatesUsecs;

1161 1162 1163 1164 1165 1166 1167 1168 1169
    // Chunked status text support
    typedef struct {
        uint16_t    chunkId;
        uint8_t     severity;
        QStringList rgMessageChunks;
    } ChunkedStatusTextInfo_t;
    QMap<uint8_t /* compId */, ChunkedStatusTextInfo_t> _chunkedStatusTextInfoMap;
    QTimer _chunkedStatusTextTimer;

1170 1171 1172 1173 1174
    /// Callback for waitForMavlinkMessage
    ///     @param resultHandleData     Opaque data passed in to waitForMavlinkMessage call
    ///     @param commandResult        Ack result for command send
    ///     @param noReponseFromVehicle true: The vehicle did not responsed to the COMMAND_LONG message
    typedef void (*WaitForMavlinkMessageResultHandler)(void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);
1175 1176

    /// Waits for the specified msecs for the message to be received. Calls timeoutHandler if not received.
1177
    void _waitForMavlinkMessage     (WaitForMavlinkMessageResultHandler resultHandler, void* resultHandlerData, int messageId, int timeoutMsecs);
1178 1179
    void _waitForMavlinkMessageClear(void);

1180 1181 1182
    int                                 _waitForMavlinkMessageId                = 0;
    bool                                _waitForMavlinkMessageTimeoutActive     = false;
    int                                 _waitForMavlinkMessageTimeoutMsecs      = 0;
1183
    QElapsedTimer                       _waitForMavlinkMessageElapsed;
1184 1185 1186 1187 1188 1189 1190
    WaitForMavlinkMessageResultHandler  _waitForMavlinkMessageResultHandler     = nullptr;
    void*                               _waitForMavlinkMessageResultHandlerData = nullptr;

    void _waitForMavlinkMessageMessageReceived(const mavlink_message_t& message);

    // requestMessage handling
    typedef struct {
1191 1192 1193 1194 1195 1196
        bool                        commandAckReceived; // We keep track of the ack/message being received since the order in which this will come in is random
        bool                        messageReceived;    // We only delete the allocated RequestMessageInfo_t when both happen (or the message wait times out)
        int                         msgId;
        int                         compId;
        RequestMessageResultHandler resultHandler;
        void*                       resultHandlerData;
1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221
    } RequestMessageInfo_t;

    static void _requestMessageCmdResultHandler             (void* resultHandlerData, int compId, MAV_RESULT result, bool noResponsefromVehicle);
    static void _requestMessageWaitForMessageResultHandler  (void* resultHandlerData, bool noResponsefromVehicle, const mavlink_message_t& message);

    typedef struct {
        int                 compId;
        bool                commandInt;     // true: use COMMAND_INT, false: use COMMAND_LONG
        MAV_CMD             command;
        MAV_FRAME           frame;
        float               rgParam[7];
        bool                showError;
        bool                requestMessage; // true: this is from a requestMessage call
        MavCmdResultHandler resultHandler;
        void*               resultHandlerData;
    } MavCommandQueueEntry_t;

    QQueue<MavCommandQueueEntry_t>  _mavCommandQueue;
    QTimer                          _mavCommandAckTimer;
    int                             _mavCommandRetryCount;
    static const int                _mavCommandMaxRetryCount =              3;
    static const int                _mavCommandAckTimeoutMSecs =            3000;
    static const int                _mavCommandAckTimeoutMSecsHighLatency = 120000;

    void _sendMavCommandWorker(bool commandInt, bool requestMessage, bool showError, MavCmdResultHandler resultHandler, void* resultHandlerData, int compId, MAV_CMD command, MAV_FRAME frame, float param1, float param2, float param3, float param4, float param5, float param6, float param7);
1222

1223 1224
    QMap<uint8_t /* batteryId */, uint8_t /* MAV_BATTERY_CHARGE_STATE_OK */> _lowestBatteryChargeStateAnnouncedMap;

Don Gagne's avatar
Don Gagne committed
1225 1226 1227 1228 1229
    // FactGroup facts

    Fact _rollFact;
    Fact _pitchFact;
    Fact _headingFact;
DonLakeFlyer's avatar
DonLakeFlyer committed
1230 1231 1232
    Fact _rollRateFact;
    Fact _pitchRateFact;
    Fact _yawRateFact;
Don Gagne's avatar
Don Gagne committed
1233 1234 1235 1236 1237
    Fact _groundSpeedFact;
    Fact _airSpeedFact;
    Fact _climbRateFact;
    Fact _altitudeRelativeFact;
    Fact _altitudeAMSLFact;
1238
    Fact _flightDistanceFact;
1239
    Fact _flightTimeFact;
1240
    Fact _distanceToHomeFact;
1241
    Fact _missionItemIndexFact;
1242
    Fact _headingToNextWPFact;
1243
    Fact _headingToHomeFact;
1244
    Fact _distanceToGCSFact;
1245
    Fact _hobbsFact;
1246
    Fact _throttlePctFact;
Don Gagne's avatar
Don Gagne committed
1247

1248 1249 1250 1251 1252 1253 1254
    VehicleGPSFactGroup             _gpsFactGroup;
    VehicleWindFactGroup            _windFactGroup;
    VehicleVibrationFactGroup       _vibrationFactGroup;
    VehicleTemperatureFactGroup     _temperatureFactGroup;
    VehicleClockFactGroup           _clockFactGroup;
    VehicleSetpointFactGroup        _setpointFactGroup;
    VehicleDistanceSensorFactGroup  _distanceSensorFactGroup;
1255
    VehicleEscStatusFactGroup       _escStatusFactGroup;
1256
    VehicleEstimatorStatusFactGroup _estimatorStatusFactGroup;
1257
    TerrainFactGroup                _terrainFactGroup;
1258
    QmlObjectListModel              _batteryFactGroupListModel;
1259 1260

    TerrainProtocolHandler* _terrainProtocolHandler = nullptr;
Don Gagne's avatar
Don Gagne committed
1261 1262 1263 1264

    static const char* _rollFactName;
    static const char* _pitchFactName;
    static const char* _headingFactName;
DonLakeFlyer's avatar
DonLakeFlyer committed
1265 1266 1267
    static const char* _rollRateFactName;
    static const char* _pitchRateFactName;
    static const char* _yawRateFactName;
Don Gagne's avatar
Don Gagne committed
1268 1269 1270 1271 1272
    static const char* _groundSpeedFactName;
    static const char* _airSpeedFactName;
    static const char* _climbRateFactName;
    static const char* _altitudeRelativeFactName;
    static const char* _altitudeAMSLFactName;
1273
    static const char* _flightDistanceFactName;
1274
    static const char* _flightTimeFactName;
1275
    static const char* _distanceToHomeFactName;
1276
    static const char* _missionItemIndexFactName;
1277
    static const char* _headingToNextWPFactName;
1278
    static const char* _headingToHomeFactName;
1279
    static const char* _distanceToGCSFactName;
1280
    static const char* _hobbsFactName;
1281
    static const char* _throttlePctFactName;
Don Gagne's avatar
Don Gagne committed
1282

Don Gagne's avatar
Don Gagne committed
1283
    static const char* _gpsFactGroupName;
Don Gagne's avatar
Don Gagne committed
1284
    static const char* _windFactGroupName;
1285
    static const char* _vibrationFactGroupName;
1286
    static const char* _temperatureFactGroupName;
dheideman's avatar
dheideman committed
1287
    static const char* _clockFactGroupName;
1288
    static const char* _distanceSensorFactGroupName;
1289
    static const char* _escStatusFactGroupName;
1290
    static const char* _estimatorStatusFactGroupName;
1291
    static const char* _terrainFactGroupName;
Don Gagne's avatar
Don Gagne committed
1292 1293 1294

    static const int _vehicleUIUpdateRateMSecs = 100;

1295
    // Settings keys
1296
    static const char* _settingsGroup;
1297
    static const char* _joystickEnabledSettingsKey;
1298 1299

    friend class InitialConnectStateMachine;
1300
    friend class VehicleBatteryFactGroup;       // Allow VehicleBatteryFactGroup to call _addFactGroup
1301
};