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

dheideman's avatar
dheideman committed
10 11 12
#include <QTime>
#include <QDateTime>
#include <QLocale>
DonLakeFlyer's avatar
DonLakeFlyer committed
13
#include <QQuaternion>
14 15 16 17 18 19

#include "Vehicle.h"
#include "MAVLinkProtocol.h"
#include "FirmwarePluginManager.h"
#include "LinkManager.h"
#include "FirmwarePlugin.h"
20
#include "UAS.h"
21
#include "JoystickManager.h"
Don Gagne's avatar
Don Gagne committed
22
#include "MissionManager.h"
Don Gagne's avatar
Don Gagne committed
23
#include "MissionController.h"
24
#include "PlanMasterController.h"
25 26
#include "GeoFenceManager.h"
#include "RallyPointManager.h"
27
#include "CoordinateVector.h"
28
#include "ParameterManager.h"
29
#include "QGCApplication.h"
dogmaphobic's avatar
dogmaphobic committed
30
#include "QGCImageProvider.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
31
#include "FollowMe.h"
32
#include "MissionCommandTree.h"
33
#include "QGroundControlQmlGlobal.h"
34
#include "SettingsManager.h"
35
#include "QGCQGeoCoordinate.h"
36
#include "QGCCorePlugin.h"
37
#include "QGCOptions.h"
38
#include "ADSBVehicle.h"
39
#include "QGCCameraManager.h"
40 41
#include "VideoReceiver.h"
#include "VideoManager.h"
42
#include "VideoSettings.h"
43
#include "PositionManager.h"
44 45 46 47
#if defined(QGC_AIRMAP_ENABLED)
#include "AirspaceVehicleManager.h"
#endif

48 49
QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")

50 51 52 53
#define UPDATE_TIMER 50
#define DEFAULT_LAT  38.965767f
#define DEFAULT_LON -120.083923f

54
const QString guided_mode_not_supported_by_vehicle = QObject::tr("Guided mode not supported by Vehicle.");
55

56 57 58
const char* Vehicle::_settingsGroup =               "Vehicle%1";        // %1 replaced with mavlink system id
const char* Vehicle::_joystickModeSettingsKey =     "JoystickMode";
const char* Vehicle::_joystickEnabledSettingsKey =  "JoystickEnabled";
59

Don Gagne's avatar
Don Gagne committed
60 61 62
const char* Vehicle::_rollFactName =                "roll";
const char* Vehicle::_pitchFactName =               "pitch";
const char* Vehicle::_headingFactName =             "heading";
DonLakeFlyer's avatar
DonLakeFlyer committed
63 64 65
const char* Vehicle::_rollRateFactName =             "rollRate";
const char* Vehicle::_pitchRateFactName =           "pitchRate";
const char* Vehicle::_yawRateFactName =             "yawRate";
Don Gagne's avatar
Don Gagne committed
66 67 68 69 70
const char* Vehicle::_airSpeedFactName =            "airSpeed";
const char* Vehicle::_groundSpeedFactName =         "groundSpeed";
const char* Vehicle::_climbRateFactName =           "climbRate";
const char* Vehicle::_altitudeRelativeFactName =    "altitudeRelative";
const char* Vehicle::_altitudeAMSLFactName =        "altitudeAMSL";
71
const char* Vehicle::_flightDistanceFactName =      "flightDistance";
72
const char* Vehicle::_flightTimeFactName =          "flightTime";
73
const char* Vehicle::_distanceToHomeFactName =      "distanceToHome";
74
const char* Vehicle::_headingToHomeFactName =       "headingToHome";
75
const char* Vehicle::_distanceToGCSFactName =       "distanceToGCS";
76
const char* Vehicle::_hobbsFactName =               "hobbs";
77
const char* Vehicle::_throttlePctFactName =         "throttlePct";
Don Gagne's avatar
Don Gagne committed
78

79 80 81 82 83 84 85 86 87
const char* Vehicle::_gpsFactGroupName =                "gps";
const char* Vehicle::_battery1FactGroupName =           "battery";
const char* Vehicle::_battery2FactGroupName =           "battery2";
const char* Vehicle::_windFactGroupName =               "wind";
const char* Vehicle::_vibrationFactGroupName =          "vibration";
const char* Vehicle::_temperatureFactGroupName =        "temperature";
const char* Vehicle::_clockFactGroupName =              "clock";
const char* Vehicle::_distanceSensorFactGroupName =     "distanceSensor";
const char* Vehicle::_estimatorStatusFactGroupName =    "estimatorStatus";
Don Gagne's avatar
Don Gagne committed
88

89
// Standard connected vehicle
90 91
Vehicle::Vehicle(LinkInterface*             link,
                 int                        vehicleId,
92
                 int                        defaultComponentId,
93 94 95 96
                 MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 JoystickManager*           joystickManager)
Don Gagne's avatar
Don Gagne committed
97 98
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json")
    , _id(vehicleId)
99
    , _defaultComponentId(defaultComponentId)
100
    , _active(false)
101
    , _offlineEditingVehicle(false)
102
    , _firmwareType(firmwareType)
103
    , _vehicleType(vehicleType)
104 105 106 107
    , _firmwarePlugin(nullptr)
    , _firmwarePluginInstanceData(nullptr)
    , _autopilotPlugin(nullptr)
    , _mavlink(nullptr)
108
    , _soloFirmware(false)
109 110
    , _toolbox(qgcApp()->toolbox())
    , _settingsManager(_toolbox->settingsManager())
111
    , _joystickMode(JoystickModeRC)
112
    , _joystickEnabled(false)
113 114
    , _uas(nullptr)
    , _mav(nullptr)
115 116 117 118 119 120 121
    , _currentMessageCount(0)
    , _messageCount(0)
    , _currentErrorCount(0)
    , _currentWarningCount(0)
    , _currentNormalCount(0)
    , _currentMessageType(MessageNone)
    , _updateCount(0)
122
    , _rcRSSI(255)
123
    , _rcRSSIstore(255.)
Don Gagne's avatar
Don Gagne committed
124
    , _autoDisconnect(false)
Don Gagne's avatar
Don Gagne committed
125
    , _flying(false)
126
    , _landing(false)
127
    , _vtolInFwdFlight(false)
128 129 130 131
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
132 133
    , _gpsRawIntMessageAvailable(false)
    , _globalPositionIntMessageAvailable(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
134 135
    , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
    , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
Gus Grubba's avatar
Gus Grubba committed
136 137 138 139 140 141 142
    , _telemetryRRSSI(0)
    , _telemetryLRSSI(0)
    , _telemetryRXErrors(0)
    , _telemetryFixed(0)
    , _telemetryTXBuffer(0)
    , _telemetryLNoise(0)
    , _telemetryRNoise(0)
143
    , _mavlinkProtocolRequestComplete(false)
144
    , _maxProtoVersion(0)
145
    , _vehicleCapabilitiesKnown(false)
146
    , _capabilityBits(0)
147
    , _highLatencyLink(false)
148
    , _receivingAttitudeQuaternion(false)
149
    , _cameras(nullptr)
150 151
    , _connectionLost(false)
    , _connectionLostEnabled(true)
DonLakeFlyer's avatar
DonLakeFlyer committed
152
    , _initialPlanRequestComplete(false)
153
    , _missionManager(nullptr)
154
    , _missionManagerInitialRequestSent(false)
155
    , _geoFenceManager(nullptr)
156
    , _geoFenceManagerInitialRequestSent(false)
157
    , _rallyPointManager(nullptr)
158
    , _rallyPointManagerInitialRequestSent(false)
159
    , _parameterManager(nullptr)
160
#if defined(QGC_AIRMAP_ENABLED)
161
    , _airspaceVehicleManager(nullptr)
162
#endif
Don Gagne's avatar
Don Gagne committed
163 164 165
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
166
    , _nextSendMessageMultipleIndex(0)
167 168
    , _firmwarePluginManager(firmwarePluginManager)
    , _joystickManager(joystickManager)
dogmaphobic's avatar
dogmaphobic committed
169
    , _flowImageIndex(0)
Don Gagne's avatar
Don Gagne committed
170
    , _allLinksInactiveSent(false)
171 172 173 174 175 176
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
177 178 179
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
180 181 182
    , _firmwareCustomMajorVersion(versionNotSetValue)
    , _firmwareCustomMinorVersion(versionNotSetValue)
    , _firmwareCustomPatchVersion(versionNotSetValue)
183
    , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL)
184
    , _gitHash(versionNotSetValue)
Gus Grubba's avatar
Gus Grubba committed
185
    , _uid(0)
186
    , _lastAnnouncedLowBatteryPercent(100)
187
    , _priorityLinkCommanded(false)
188
    , _orbitActive(false)
189 190
    , _pidTuningTelemetryMode(false)
    , _pidTuningWaitingForRates(false)
Don Gagne's avatar
Don Gagne committed
191 192 193
    , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
    , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
    , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
DonLakeFlyer's avatar
DonLakeFlyer committed
194 195 196
    , _rollRateFact         (0, _rollRateFactName,          FactMetaData::valueTypeDouble)
    , _pitchRateFact        (0, _pitchRateFactName,         FactMetaData::valueTypeDouble)
    , _yawRateFact          (0, _yawRateFactName,           FactMetaData::valueTypeDouble)
Don Gagne's avatar
Don Gagne committed
197 198 199 200 201
    , _groundSpeedFact      (0, _groundSpeedFactName,       FactMetaData::valueTypeDouble)
    , _airSpeedFact         (0, _airSpeedFactName,          FactMetaData::valueTypeDouble)
    , _climbRateFact        (0, _climbRateFactName,         FactMetaData::valueTypeDouble)
    , _altitudeRelativeFact (0, _altitudeRelativeFactName,  FactMetaData::valueTypeDouble)
    , _altitudeAMSLFact     (0, _altitudeAMSLFactName,      FactMetaData::valueTypeDouble)
202
    , _flightDistanceFact   (0, _flightDistanceFactName,    FactMetaData::valueTypeDouble)
203
    , _flightTimeFact       (0, _flightTimeFactName,        FactMetaData::valueTypeElapsedTimeInSeconds)
204
    , _distanceToHomeFact   (0, _distanceToHomeFactName,    FactMetaData::valueTypeDouble)
205
    , _headingToHomeFact    (0, _headingToHomeFactName,     FactMetaData::valueTypeDouble)
206
    , _distanceToGCSFact    (0, _distanceToGCSFactName,     FactMetaData::valueTypeDouble)
207
    , _hobbsFact            (0, _hobbsFactName,             FactMetaData::valueTypeString)
208
    , _throttlePctFact      (0, _throttlePctFactName,       FactMetaData::valueTypeUint16)
Don Gagne's avatar
Don Gagne committed
209
    , _gpsFactGroup(this)
DonLakeFlyer's avatar
DonLakeFlyer committed
210 211
    , _battery1FactGroup(this)
    , _battery2FactGroup(this)
Don Gagne's avatar
Don Gagne committed
212
    , _windFactGroup(this)
213
    , _vibrationFactGroup(this)
214
    , _temperatureFactGroup(this)
dheideman's avatar
dheideman committed
215
    , _clockFactGroup(this)
216
    , _distanceSensorFactGroup(this)
217
    , _estimatorStatusFactGroup(this)
218
{
219 220
    connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings);
    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings);
Jacob Walser's avatar
Jacob Walser committed
221

222
    _mavlink = _toolbox->mavlinkProtocol();
223
    qCDebug(VehicleLog) << "Link started with Mavlink " << (_mavlink->getCurrentVersion() >= 200 ? "V2" : "V1");
dogmaphobic's avatar
dogmaphobic committed
224

225 226
    connect(_mavlink, &MAVLinkProtocol::messageReceived,        this, &Vehicle::_mavlinkMessageReceived);
    connect(_mavlink, &MAVLinkProtocol::mavlinkMessageStatus,   this, &Vehicle::_mavlinkMessageStatus);
227

228 229
    _addLink(link);

230
    connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
Don Gagne's avatar
Don Gagne committed
231
    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
232
    connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
233

234 235
    connect(_toolbox->multiVehicleManager(), &MultiVehicleManager::parameterReadyVehicleAvailableChanged, this, &Vehicle::_vehicleParamLoaded);

236
    _uas = new UAS(_mavlink, this, _firmwarePluginManager);
dogmaphobic's avatar
dogmaphobic committed
237

Don Gagne's avatar
Don Gagne committed
238 239
    connect(_uas, &UAS::imageReady,                     this, &Vehicle::_imageReady);
    connect(this, &Vehicle::remoteControlRSSIChanged,   this, &Vehicle::_remoteControlRSSIChanged);
dogmaphobic's avatar
dogmaphobic committed
240

241
    _commonInit();
Don Gagne's avatar
Don Gagne committed
242
    _autopilotPlugin = _firmwarePlugin->autopilotPlugin(this);
dogmaphobic's avatar
dogmaphobic committed
243

Jimmy Johnson's avatar
Jimmy Johnson committed
244
    // connect this vehicle to the follow me handle manager
245
    connect(this, &Vehicle::flightModeChanged,_toolbox->followMe(), &FollowMe::followMeHandleManager);
Jimmy Johnson's avatar
Jimmy Johnson committed
246

Don Gagne's avatar
Don Gagne committed
247 248 249 250 251
    // PreArm Error self-destruct timer
    connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
    _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
    _prearmErrorTimer.setSingleShot(true);

252 253
    // Send MAV_CMD ack timer
    _mavCommandAckTimer.setSingleShot(true);
DonLakeFlyer's avatar
DonLakeFlyer committed
254
    _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
255 256
    connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);

257
    _mav = uas();
258

259
    // Listen for system messages
260 261
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
262

263 264
    if (_highLatencyLink || link->isPX4Flow()) {
        // These links don't request information
265 266 267 268 269 270 271
        _setMaxProtoVersion(100);
        _setCapabilities(0);
        _initialPlanRequestComplete = true;
        _missionManagerInitialRequestSent = true;
        _geoFenceManagerInitialRequestSent = true;
        _rallyPointManagerInitialRequestSent = true;
    } else {
272
        sendMavCommand(MAV_COMP_ID_ALL,
273 274 275
                       MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
                       false,                                   // No error shown if fails
                       1);                                      // Request firmware version
276 277 278 279
        sendMavCommand(MAV_COMP_ID_ALL,
                       MAV_CMD_REQUEST_PROTOCOL_VERSION,
                       false,                                   // No error shown if fails
                       1);                                      // Request protocol version
280
    }
281

282
    _firmwarePlugin->initializeVehicle(this);
283 284 285
    for(auto& factName: factNames()) {
        _firmwarePlugin->adjustMetaData(vehicleType, getFact(factName)->metaData());
    }
dogmaphobic's avatar
dogmaphobic committed
286

287 288
    _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay);
    connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext);
dogmaphobic's avatar
dogmaphobic committed
289

290 291
    _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints);
    connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint);
292

293 294
    connect(&_orbitTelemetryTimer, &QTimer::timeout, this, &Vehicle::_orbitTelemetryTimeout);

295
    // Create camera manager instance
296
    _cameras = _firmwarePlugin->createCameraManager(this);
297
    emit dynamicCamerasChanged();
298

299 300 301
    connect(&_adsbTimer, &QTimer::timeout, this, &Vehicle::_adsbTimerTimeout);
    _adsbTimer.setSingleShot(false);
    _adsbTimer.start(1000);
302 303
}

304 305 306 307 308
// Disconnected Vehicle for offline editing
Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 QObject*                   parent)
309 310
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json", parent)
    , _id(0)
311
    , _defaultComponentId(MAV_COMP_ID_ALL)
312
    , _active(false)
313
    , _offlineEditingVehicle(true)
314 315
    , _firmwareType(firmwareType)
    , _vehicleType(vehicleType)
316 317 318 319
    , _firmwarePlugin(nullptr)
    , _firmwarePluginInstanceData(nullptr)
    , _autopilotPlugin(nullptr)
    , _mavlink(nullptr)
320
    , _soloFirmware(false)
321 322
    , _toolbox(qgcApp()->toolbox())
    , _settingsManager(_toolbox->settingsManager())
323 324
    , _joystickMode(JoystickModeRC)
    , _joystickEnabled(false)
325 326
    , _uas(nullptr)
    , _mav(nullptr)
327 328 329 330 331 332 333
    , _currentMessageCount(0)
    , _messageCount(0)
    , _currentErrorCount(0)
    , _currentWarningCount(0)
    , _currentNormalCount(0)
    , _currentMessageType(MessageNone)
    , _updateCount(0)
334
    , _rcRSSI(255)
335
    , _rcRSSIstore(255.)
336
    , _autoDisconnect(false)
337
    , _flying(false)
338
    , _landing(false)
339
    , _vtolInFwdFlight(false)
340 341 342 343
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
344 345
    , _gpsRawIntMessageAvailable(false)
    , _globalPositionIntMessageAvailable(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
346 347
    , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
    , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
348
    , _vehicleCapabilitiesKnown(true)
349
    , _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
350
    , _highLatencyLink(false)
351
    , _receivingAttitudeQuaternion(false)
352
    , _cameras(nullptr)
353 354
    , _connectionLost(false)
    , _connectionLostEnabled(true)
DonLakeFlyer's avatar
DonLakeFlyer committed
355
    , _initialPlanRequestComplete(false)
356
    , _missionManager(nullptr)
357
    , _missionManagerInitialRequestSent(false)
358
    , _geoFenceManager(nullptr)
359
    , _geoFenceManagerInitialRequestSent(false)
360
    , _rallyPointManager(nullptr)
361
    , _rallyPointManagerInitialRequestSent(false)
362
    , _parameterManager(nullptr)
363
#if defined(QGC_AIRMAP_ENABLED)
364
    , _airspaceVehicleManager(nullptr)
365
#endif
366 367 368 369
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
    , _nextSendMessageMultipleIndex(0)
370
    , _firmwarePluginManager(firmwarePluginManager)
371
    , _joystickManager(nullptr)
372 373 374 375 376 377 378 379
    , _flowImageIndex(0)
    , _allLinksInactiveSent(false)
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
380 381 382
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
383 384 385 386
    , _firmwareCustomMajorVersion(versionNotSetValue)
    , _firmwareCustomMinorVersion(versionNotSetValue)
    , _firmwareCustomPatchVersion(versionNotSetValue)
    , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL)
387
    , _gitHash(versionNotSetValue)
Gus Grubba's avatar
Gus Grubba committed
388
    , _uid(0)
389
    , _lastAnnouncedLowBatteryPercent(100)
390
    , _orbitActive(false)
391 392
    , _pidTuningTelemetryMode(false)
    , _pidTuningWaitingForRates(false)
393 394 395
    , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
    , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
    , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
DonLakeFlyer's avatar
DonLakeFlyer committed
396 397 398
    , _rollRateFact         (0, _rollRateFactName,          FactMetaData::valueTypeDouble)
    , _pitchRateFact        (0, _pitchRateFactName,         FactMetaData::valueTypeDouble)
    , _yawRateFact          (0, _yawRateFactName,           FactMetaData::valueTypeDouble)
399 400 401 402 403
    , _groundSpeedFact      (0, _groundSpeedFactName,       FactMetaData::valueTypeDouble)
    , _airSpeedFact         (0, _airSpeedFactName,          FactMetaData::valueTypeDouble)
    , _climbRateFact        (0, _climbRateFactName,         FactMetaData::valueTypeDouble)
    , _altitudeRelativeFact (0, _altitudeRelativeFactName,  FactMetaData::valueTypeDouble)
    , _altitudeAMSLFact     (0, _altitudeAMSLFactName,      FactMetaData::valueTypeDouble)
404 405
    , _flightDistanceFact   (0, _flightDistanceFactName,    FactMetaData::valueTypeDouble)
    , _flightTimeFact       (0, _flightTimeFactName,        FactMetaData::valueTypeElapsedTimeInSeconds)
406
    , _distanceToHomeFact   (0, _distanceToHomeFactName,    FactMetaData::valueTypeDouble)
407
    , _headingToHomeFact    (0, _headingToHomeFactName,     FactMetaData::valueTypeDouble)
408
    , _distanceToGCSFact    (0, _distanceToGCSFactName,     FactMetaData::valueTypeDouble)
409
    , _hobbsFact            (0, _hobbsFactName,             FactMetaData::valueTypeString)
410
    , _throttlePctFact      (0, _throttlePctFactName,       FactMetaData::valueTypeUint16)
411
    , _gpsFactGroup(this)
DonLakeFlyer's avatar
DonLakeFlyer committed
412 413
    , _battery1FactGroup(this)
    , _battery2FactGroup(this)
414 415
    , _windFactGroup(this)
    , _vibrationFactGroup(this)
dheideman's avatar
dheideman committed
416
    , _clockFactGroup(this)
417
    , _distanceSensorFactGroup(this)
418
{
419
    _commonInit();
420 421 422 423 424 425 426

    // Offline editing vehicle tracks settings changes for offline editing settings
    connect(_settingsManager->appSettings()->offlineEditingFirmwareType(),  &Fact::rawValueChanged, this, &Vehicle::_offlineFirmwareTypeSettingChanged);
    connect(_settingsManager->appSettings()->offlineEditingVehicleType(),   &Fact::rawValueChanged, this, &Vehicle::_offlineVehicleTypeSettingChanged);
    connect(_settingsManager->appSettings()->offlineEditingCruiseSpeed(),   &Fact::rawValueChanged, this, &Vehicle::_offlineCruiseSpeedSettingChanged);
    connect(_settingsManager->appSettings()->offlineEditingHoverSpeed(),    &Fact::rawValueChanged, this, &Vehicle::_offlineHoverSpeedSettingChanged);

427
    _firmwarePlugin->initializeVehicle(this);
428 429 430 431 432
}

void Vehicle::_commonInit(void)
{
    _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
433

434 435
    connect(_firmwarePlugin, &FirmwarePlugin::toolbarIndicatorsChanged, this, &Vehicle::toolBarIndicatorsChanged);

436
    connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceHeadingToHome);
437
    connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceToGCS);
438
    connect(this, &Vehicle::homePositionChanged,    this, &Vehicle::_updateDistanceHeadingToHome);
439
    connect(this, &Vehicle::hobbsMeterChanged,      this, &Vehicle::_updateHobbsMeter);
440

441 442
    connect(_toolbox->qgcPositionManager(), &QGCPositionManager::gcsPositionChanged, this, &Vehicle::_updateDistanceToGCS);

443
    _missionManager = new MissionManager(this);
444
    connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
445
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
446 447 448 449
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearCameraTriggerPoints);
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearTrajectoryPoints);
    connect(_missionManager, &MissionManager::sendComplete,             this, &Vehicle::_clearCameraTriggerPoints);
    connect(_missionManager, &MissionManager::sendComplete,             this, &Vehicle::_clearTrajectoryPoints);
450

451
    _parameterManager = new ParameterManager(this);
Don Gagne's avatar
Don Gagne committed
452
    connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
453

454
    // GeoFenceManager needs to access ParameterManager so make sure to create after
455
    _geoFenceManager = new GeoFenceManager(this);
456
    connect(_geoFenceManager, &GeoFenceManager::error,          this, &Vehicle::_geoFenceManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
457
    connect(_geoFenceManager, &GeoFenceManager::loadComplete,   this, &Vehicle::_geoFenceLoadComplete);
458

459
    _rallyPointManager = new RallyPointManager(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
460 461
    connect(_rallyPointManager, &RallyPointManager::error,          this, &Vehicle::_rallyPointManagerError);
    connect(_rallyPointManager, &RallyPointManager::loadComplete,   this, &Vehicle::_rallyPointLoadComplete);
462

463 464 465
    // Flight modes can differ based on advanced mode
    connect(_toolbox->corePlugin(), &QGCCorePlugin::showAdvancedUIChanged, this, &Vehicle::flightModesChanged);

466 467 468 469 470
    // Build FactGroup object model

    _addFact(&_rollFact,                _rollFactName);
    _addFact(&_pitchFact,               _pitchFactName);
    _addFact(&_headingFact,             _headingFactName);
DonLakeFlyer's avatar
DonLakeFlyer committed
471 472 473
    _addFact(&_rollRateFact,            _rollRateFactName);
    _addFact(&_pitchRateFact,           _pitchRateFactName);
    _addFact(&_yawRateFact,             _yawRateFactName);
474 475 476 477 478
    _addFact(&_groundSpeedFact,         _groundSpeedFactName);
    _addFact(&_airSpeedFact,            _airSpeedFactName);
    _addFact(&_climbRateFact,           _climbRateFactName);
    _addFact(&_altitudeRelativeFact,    _altitudeRelativeFactName);
    _addFact(&_altitudeAMSLFact,        _altitudeAMSLFactName);
479
    _addFact(&_flightDistanceFact,      _flightDistanceFactName);
480
    _addFact(&_flightTimeFact,          _flightTimeFactName);
481
    _addFact(&_distanceToHomeFact,      _distanceToHomeFactName);
482
    _addFact(&_headingToHomeFact,       _headingToHomeFactName);
483
    _addFact(&_distanceToGCSFact,       _distanceToGCSFactName);
484
    _addFact(&_throttlePctFact,         _throttlePctFactName);
485 486

    _hobbsFact.setRawValue(QVariant(QString("0000:00:00")));
487
    _addFact(&_hobbsFact,               _hobbsFactName);
488

489
    _addFactGroup(&_gpsFactGroup,               _gpsFactGroupName);
DonLakeFlyer's avatar
DonLakeFlyer committed
490 491
    _addFactGroup(&_battery1FactGroup,          _battery1FactGroupName);
    _addFactGroup(&_battery2FactGroup,          _battery2FactGroupName);
492 493 494 495 496
    _addFactGroup(&_windFactGroup,              _windFactGroupName);
    _addFactGroup(&_vibrationFactGroup,         _vibrationFactGroupName);
    _addFactGroup(&_temperatureFactGroup,       _temperatureFactGroupName);
    _addFactGroup(&_clockFactGroup,             _clockFactGroupName);
    _addFactGroup(&_distanceSensorFactGroup,    _distanceSensorFactGroupName);
497
    _addFactGroup(&_estimatorStatusFactGroup,   _estimatorStatusFactGroupName);
498

Jacob Walser's avatar
Jacob Walser committed
499
    // Add firmware-specific fact groups, if provided
500 501 502 503 504 505 506
    QMap<QString, FactGroup*>* fwFactGroups = _firmwarePlugin->factGroups();
    if (fwFactGroups) {
        QMapIterator<QString, FactGroup*> i(*fwFactGroups);
        while(i.hasNext()) {
            i.next();
            _addFactGroup(i.value(), i.key());
        }
507 508
    }

509
    _flightDistanceFact.setRawValue(0);
510
    _flightTimeFact.setRawValue(0);
511

512
    // Set video stream to udp if running ArduSub and Video is disabled
513
    if (sub() && _settingsManager->videoSettings()->videoSource()->rawValue() == VideoSettings::videoDisabled) {
Gus Grubba's avatar
Gus Grubba committed
514
        _settingsManager->videoSettings()->videoSource()->setRawValue(VideoSettings::videoSourceUDPH264);
515 516
    }

517 518 519 520 521 522 523 524 525 526
    //-- Airspace Management
#if defined(QGC_AIRMAP_ENABLED)
    AirspaceManager* airspaceManager = _toolbox->airspaceManager();
    if (airspaceManager) {
        _airspaceVehicleManager = airspaceManager->instantiateVehicle(*this);
        if (_airspaceVehicleManager) {
            connect(_airspaceVehicleManager, &AirspaceVehicleManager::trafficUpdate, this, &Vehicle::_trafficUpdate);
        }
    }
#endif
527 528

    _pidTuningMessages << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
529 530
}

531 532
Vehicle::~Vehicle()
{
Don Gagne's avatar
Don Gagne committed
533 534
    qCDebug(VehicleLog) << "~Vehicle" << this;

535
    delete _missionManager;
536
    _missionManager = nullptr;
537

538
    delete _autopilotPlugin;
539
    _autopilotPlugin = nullptr;
540

541
    delete _mav;
542
    _mav = nullptr;
dogmaphobic's avatar
dogmaphobic committed
543

544
#if defined(QGC_AIRMAP_ENABLED)
545 546
    if (_airspaceVehicleManager) {
        delete _airspaceVehicleManager;
547
    }
548
#endif
549 550
}

551 552 553 554
void Vehicle::prepareDelete()
{
    if(_cameras) {
        delete _cameras;
555
        _cameras = nullptr;
556 557 558 559 560
        emit dynamicCamerasChanged();
        qApp->processEvents();
    }
}

561 562 563
void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
    _firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
564
    _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
565
    emit firmwareTypeChanged();
566 567 568 569 570 571
    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        _capabilityBits = 0;
    } else {
        _capabilityBits = MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
    }
    emit capabilityBitsChanged(_capabilityBits);
572 573 574 575 576 577 578 579 580 581
}

void Vehicle::_offlineVehicleTypeSettingChanged(QVariant value)
{
    _vehicleType = static_cast<MAV_TYPE>(value.toInt());
    emit vehicleTypeChanged();
}

void Vehicle::_offlineCruiseSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
582 583
    _defaultCruiseSpeed = value.toDouble();
    emit defaultCruiseSpeedChanged(_defaultCruiseSpeed);
584 585 586 587
}

void Vehicle::_offlineHoverSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
588 589
    _defaultHoverSpeed = value.toDouble();
    emit defaultHoverSpeedChanged(_defaultHoverSpeed);
590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620
}

QString Vehicle::firmwareTypeString(void) const
{
    if (px4Firmware()) {
        return QStringLiteral("PX4 Pro");
    } else if (apmFirmware()) {
        return QStringLiteral("ArduPilot");
    } else {
        return tr("MAVLink Generic");
    }
}

QString Vehicle::vehicleTypeString(void) const
{
    if (fixedWing()) {
        return tr("Fixed Wing");
    } else if (multiRotor()) {
        return tr("Multi-Rotor");
    } else if (vtol()) {
        return tr("VTOL");
    } else if (rover()) {
        return tr("Rover");
    } else if (sub()) {
        return tr("Sub");
    } else {
        return tr("Unknown");
    }
}

void Vehicle::resetCounters()
621 622 623 624 625 626 627 628
{
    _messagesReceived   = 0;
    _messagesSent       = 0;
    _messagesLost       = 0;
    _messageSeq         = 0;
    _heardFrom          = false;
}

629 630
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
631
    // If the link is already running at Mavlink V2 set our max proto version to it.
DonLakeFlyer's avatar
DonLakeFlyer committed
632 633
    unsigned mavlinkVersion = _mavlink->getCurrentVersion();
    if (_maxProtoVersion != mavlinkVersion && mavlinkVersion >= 200) {
634 635
        _maxProtoVersion = mavlinkVersion;
        qCDebug(VehicleLog) << "Vehicle::_mavlinkMessageReceived Link already running Mavlink v2. Setting _maxProtoVersion" << _maxProtoVersion;
DonLakeFlyer's avatar
DonLakeFlyer committed
636 637
    }

Don Gagne's avatar
Don Gagne committed
638
    if (message.sysid != _id && message.sysid != 0) {
639 640 641 642
        // We allow RADIO_STATUS messages which come from a link the vehicle is using to pass through and be handled
        if (!(message.msgid == MAVLINK_MSG_ID_RADIO_STATUS && _containsLink(link))) {
            return;
        }
643
    }
644

645 646 647
    if (!_containsLink(link)) {
        _addLink(link);
    }
dogmaphobic's avatar
dogmaphobic committed
648

649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674
    //-- Check link status
    _messagesReceived++;
    emit messagesReceivedChanged();
    if(!_heardFrom) {
        if(message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
            _heardFrom = true;
            _compID = message.compid;
            _messageSeq = message.seq + 1;
        }
    } else {
        if(_compID == message.compid) {
            uint16_t seq_received = (uint16_t)message.seq;
            uint16_t packet_lost_count = 0;
            //-- Account for overflow during packet loss
            if(seq_received < _messageSeq) {
                packet_lost_count = (seq_received + 255) - _messageSeq;
            } else {
                packet_lost_count = seq_received - _messageSeq;
            }
            _messageSeq = message.seq + 1;
            _messagesLost += packet_lost_count;
            if(packet_lost_count)
                emit messagesLostChanged();
        }
    }

Don Gagne's avatar
Don Gagne committed
675
    // Give the plugin a change to adjust the message contents
676 677 678
    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
679

680 681 682 683 684
    // Give the Core Plugin access to all mavlink traffic
    if (!_toolbox->corePlugin()->mavlinkMessage(this, link, message)) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
685
    switch (message.msgid) {
Don Gagne's avatar
Don Gagne committed
686 687 688 689 690 691
    case MAVLINK_MSG_ID_HOME_POSITION:
        _handleHomePosition(message);
        break;
    case MAVLINK_MSG_ID_HEARTBEAT:
        _handleHeartbeat(message);
        break;
692 693 694
    case MAVLINK_MSG_ID_RADIO_STATUS:
        _handleRadioStatus(message);
        break;
Don Gagne's avatar
Don Gagne committed
695 696 697 698 699 700
    case MAVLINK_MSG_ID_RC_CHANNELS:
        _handleRCChannels(message);
        break;
    case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
        _handleRCChannelsRaw(message);
        break;
Don Gagne's avatar
Don Gagne committed
701 702 703 704 705 706
    case MAVLINK_MSG_ID_BATTERY_STATUS:
        _handleBatteryStatus(message);
        break;
    case MAVLINK_MSG_ID_SYS_STATUS:
        _handleSysStatus(message);
        break;
707 708 709 710 711 712 713 714 715 716 717 718
    case MAVLINK_MSG_ID_RAW_IMU:
        emit mavlinkRawImu(message);
        break;
    case MAVLINK_MSG_ID_SCALED_IMU:
        emit mavlinkScaledImu1(message);
        break;
    case MAVLINK_MSG_ID_SCALED_IMU2:
        emit mavlinkScaledImu2(message);
        break;
    case MAVLINK_MSG_ID_SCALED_IMU3:
        emit mavlinkScaledImu3(message);
        break;
719 720 721
    case MAVLINK_MSG_ID_VIBRATION:
        _handleVibration(message);
        break;
Don Gagne's avatar
Don Gagne committed
722 723 724
    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        _handleExtendedSysState(message);
        break;
725 726 727
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
728 729 730
    case MAVLINK_MSG_ID_COMMAND_LONG:
        _handleCommandLong(message);
        break;
731
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
Gus Grubba's avatar
Gus Grubba committed
732
        _handleAutopilotVersion(link, message);
733
        break;
734 735 736
    case MAVLINK_MSG_ID_PROTOCOL_VERSION:
        _handleProtocolVersion(link, message);
        break;
737 738 739
    case MAVLINK_MSG_ID_WIND_COV:
        _handleWindCov(message);
        break;
740 741 742
    case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
        _handleHilActuatorControls(message);
        break;
743 744 745 746 747 748
    case MAVLINK_MSG_ID_LOGGING_DATA:
        _handleMavlinkLoggingData(message);
        break;
    case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
        _handleMavlinkLoggingDataAcked(message);
        break;
749 750 751 752 753 754 755 756 757 758 759 760
    case MAVLINK_MSG_ID_GPS_RAW_INT:
        _handleGpsRawInt(message);
        break;
    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
        _handleGlobalPositionInt(message);
        break;
    case MAVLINK_MSG_ID_ALTITUDE:
        _handleAltitude(message);
        break;
    case MAVLINK_MSG_ID_VFR_HUD:
        _handleVfrHud(message);
        break;
761 762 763 764 765 766 767 768
    case MAVLINK_MSG_ID_SCALED_PRESSURE:
        _handleScaledPressure(message);
        break;
    case MAVLINK_MSG_ID_SCALED_PRESSURE2:
        _handleScaledPressure2(message);
        break;
    case MAVLINK_MSG_ID_SCALED_PRESSURE3:
        _handleScaledPressure3(message);
769
        break;
770 771
    case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
        _handleCameraImageCaptured(message);
772
        break;
773 774 775
    case MAVLINK_MSG_ID_ADSB_VEHICLE:
        _handleADSBVehicle(message);
        break;
776 777 778
    case MAVLINK_MSG_ID_HIGH_LATENCY2:
        _handleHighLatency2(message);
        break;
779 780 781
    case MAVLINK_MSG_ID_ATTITUDE:
        _handleAttitude(message);
        break;
782 783
    case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
        _handleAttitudeQuaternion(message);
DonLakeFlyer's avatar
DonLakeFlyer committed
784 785 786 787
        break;
    case MAVLINK_MSG_ID_ATTITUDE_TARGET:
        _handleAttitudeTarget(message);
        break;
788 789 790
    case MAVLINK_MSG_ID_DISTANCE_SENSOR:
        _handleDistanceSensor(message);
        break;
791 792 793
    case MAVLINK_MSG_ID_ESTIMATOR_STATUS:
        _handleEstimatorStatus(message);
        break;
794
    case MAVLINK_MSG_ID_STATUSTEXT:
Don Gagne's avatar
Don Gagne committed
795 796 797 798
        _handleStatusText(message, false /* longVersion */);
        break;
    case MAVLINK_MSG_ID_STATUSTEXT_LONG:
        _handleStatusText(message, true /* longVersion */);
799
        break;
800 801 802
    case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS:
        _handleOrbitExecutionStatus(message);
        break;
803 804 805
    case MAVLINK_MSG_ID_MESSAGE_INTERVAL:
        _handleMessageInterval(message);
        break;
DonLakeFlyer's avatar
DonLakeFlyer committed
806 807 808
    case MAVLINK_MSG_ID_PING:
        _handlePing(link, message);
        break;
Don Gagne's avatar
Don Gagne committed
809

810 811 812 813 814 815 816
    case MAVLINK_MSG_ID_SERIAL_CONTROL:
    {
        mavlink_serial_control_t ser;
        mavlink_msg_serial_control_decode(&message, &ser);
        emit mavlinkSerialControl(ser.device, ser.flags, ser.timeout, ser.baudrate, QByteArray(reinterpret_cast<const char*>(ser.data), ser.count));
    }
        break;
Don Gagne's avatar
Don Gagne committed
817

818
        // Following are ArduPilot dialect messages
819 820 821 822
#if !defined(NO_ARDUPILOT_DIALECT)
    case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
        _handleCameraFeedback(message);
        break;
Don Gagne's avatar
Don Gagne committed
823 824 825
    case MAVLINK_MSG_ID_WIND:
        _handleWind(message);
        break;
826
#endif
827
    }
dogmaphobic's avatar
dogmaphobic committed
828

829 830
    // This must be emitted after the vehicle processes the message. This way the vehicle state is up to date when anyone else
    // does processing.
Don Gagne's avatar
Don Gagne committed
831
    emit mavlinkMessageReceived(message);
dogmaphobic's avatar
dogmaphobic committed
832

833 834 835
    _uas->receiveMessage(message);
}

836
#if !defined(NO_ARDUPILOT_DIALECT)
837 838 839 840 841 842 843 844 845 846
void Vehicle::_handleCameraFeedback(const mavlink_message_t& message)
{
    mavlink_camera_feedback_t feedback;

    mavlink_msg_camera_feedback_decode(&message, &feedback);

    QGeoCoordinate imageCoordinate((double)feedback.lat / qPow(10.0, 7.0), (double)feedback.lng / qPow(10.0, 7.0), feedback.alt_msl);
    qCDebug(VehicleLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.img_idx;
    _cameraTriggerPoints.append(new QGCQGeoCoordinate(imageCoordinate, this));
}
847
#endif
848

849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884
void Vehicle::_handleOrbitExecutionStatus(const mavlink_message_t& message)
{
    mavlink_orbit_execution_status_t orbitStatus;

    mavlink_msg_orbit_execution_status_decode(&message, &orbitStatus);

    double newRadius =  qAbs(static_cast<double>(orbitStatus.radius));
    if (!qFuzzyCompare(_orbitMapCircle.radius()->rawValue().toDouble(), newRadius)) {
        _orbitMapCircle.radius()->setRawValue(newRadius);
    }

    bool newOrbitClockwise = orbitStatus.radius > 0 ? true : false;
    if (_orbitMapCircle.clockwiseRotation() != newOrbitClockwise) {
        _orbitMapCircle.setClockwiseRotation(newOrbitClockwise);
    }

    QGeoCoordinate newCenter(static_cast<double>(orbitStatus.x) / qPow(10.0, 7.0), static_cast<double>(orbitStatus.y) / qPow(10.0, 7.0));
    if (_orbitMapCircle.center() != newCenter) {
        _orbitMapCircle.setCenter(newCenter);
    }

    if (!_orbitActive) {
        _orbitActive = true;
        _orbitMapCircle.setShowRotation(true);
        emit orbitActiveChanged(true);
    }

    _orbitTelemetryTimer.start(_orbitTelemetryTimeoutMsecs);
}

void Vehicle::_orbitTelemetryTimeout(void)
{
    _orbitActive = false;
    emit orbitActiveChanged(false);
}

885 886 887 888 889 890 891 892 893 894 895 896 897
void Vehicle::_handleCameraImageCaptured(const mavlink_message_t& message)
{
    mavlink_camera_image_captured_t feedback;

    mavlink_msg_camera_image_captured_decode(&message, &feedback);

    QGeoCoordinate imageCoordinate((double)feedback.lat / qPow(10.0, 7.0), (double)feedback.lon / qPow(10.0, 7.0), feedback.alt);
    qCDebug(VehicleLog) << "_handleCameraFeedback coord:index" << imageCoordinate << feedback.image_index << feedback.capture_result;
    if (feedback.capture_result == 1) {
        _cameraTriggerPoints.append(new QGCQGeoCoordinate(imageCoordinate, this));
    }
}

Don Gagne's avatar
Don Gagne committed
898
void Vehicle::_handleStatusText(mavlink_message_t& message, bool longVersion)
899
{
Don Gagne's avatar
Don Gagne committed
900 901 902 903 904 905
    QByteArray  b;
    QString     messageText;
    int         severity;

    if (longVersion) {
        b.resize(MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN+1);
906 907 908 909
        mavlink_statustext_long_t statustextLong;
        mavlink_msg_statustext_long_decode(&message, &statustextLong);
        strncpy(b.data(), statustextLong.text, MAVLINK_MSG_STATUSTEXT_LONG_FIELD_TEXT_LEN);
        severity = statustextLong.severity;
Don Gagne's avatar
Don Gagne committed
910 911
    } else {
        b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
912 913 914 915
        mavlink_statustext_t statustext;
        mavlink_msg_statustext_decode(&message, &statustext);
        strncpy(b.data(), statustext.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
        severity = statustext.severity;
Don Gagne's avatar
Don Gagne committed
916
    }
917
    b[b.length()-1] = '\0';
Don Gagne's avatar
Don Gagne committed
918
    messageText = QString(b);
919 920

    bool skipSpoken = false;
Don Gagne's avatar
Don Gagne committed
921
    bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm"));
Don Gagne's avatar
Don Gagne committed
922
    bool px4Prearm = messageText.startsWith(QStringLiteral("preflight"), Qt::CaseInsensitive) && severity >= MAV_SEVERITY_CRITICAL;
Don Gagne's avatar
Don Gagne committed
923
    if (ardupilotPrearm || px4Prearm) {
924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940 941 942 943 944
        // Limit repeated PreArm message to once every 10 seconds
        if (_noisySpokenPrearmMap.contains(messageText) && _noisySpokenPrearmMap[messageText].msecsTo(QTime::currentTime()) < (10 * 1000)) {
            skipSpoken = true;
        } else {
            _noisySpokenPrearmMap[messageText] = QTime::currentTime();
            setPrearmError(messageText);
        }
    }


    // If the message is NOTIFY or higher severity, or starts with a '#',
    // then read it aloud.
    if (messageText.startsWith("#") || severity <= MAV_SEVERITY_NOTICE) {
        messageText.remove("#");
        if (!skipSpoken) {
            qgcApp()->toolbox()->audioOutput()->say(messageText);
        }
    }
    emit textMessageReceived(id(), message.compid, severity, messageText);
}

945 946 947 948 949 950 951 952
void Vehicle::_handleVfrHud(mavlink_message_t& message)
{
    mavlink_vfr_hud_t vfrHud;
    mavlink_msg_vfr_hud_decode(&message, &vfrHud);

    _airSpeedFact.setRawValue(qIsNaN(vfrHud.airspeed) ? 0 : vfrHud.airspeed);
    _groundSpeedFact.setRawValue(qIsNaN(vfrHud.groundspeed) ? 0 : vfrHud.groundspeed);
    _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb);
953
    _throttlePctFact.setRawValue(vfrHud.throttle);
954 955
}

956 957 958 959 960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008
void Vehicle::_handleEstimatorStatus(mavlink_message_t& message)
{
    mavlink_estimator_status_t estimatorStatus;
    mavlink_msg_estimator_status_decode(&message, &estimatorStatus);

    _estimatorStatusFactGroup.goodAttitudeEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_ATTITUDE));
    _estimatorStatusFactGroup.goodHorizVelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_VELOCITY_HORIZ));
    _estimatorStatusFactGroup.goodVertVelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_VELOCITY_VERT));
    _estimatorStatusFactGroup.goodHorizPosRelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_HORIZ_REL));
    _estimatorStatusFactGroup.goodHorizPosAbsEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_HORIZ_ABS));
    _estimatorStatusFactGroup.goodVertPosAbsEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_VERT_ABS));
    _estimatorStatusFactGroup.goodVertPosAGLEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_VERT_AGL));
    _estimatorStatusFactGroup.goodConstPosModeEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_CONST_POS_MODE));
    _estimatorStatusFactGroup.goodPredHorizPosRelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_PRED_POS_HORIZ_REL));
    _estimatorStatusFactGroup.goodPredHorizPosAbsEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_PRED_POS_HORIZ_ABS));
    _estimatorStatusFactGroup.gpsGlitch()->setRawValue(estimatorStatus.flags & ESTIMATOR_GPS_GLITCH ? true : false);
    _estimatorStatusFactGroup.accelError()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_ACCEL_ERROR));
    _estimatorStatusFactGroup.velRatio()->setRawValue(estimatorStatus.vel_ratio);
    _estimatorStatusFactGroup.horizPosRatio()->setRawValue(estimatorStatus.pos_horiz_ratio);
    _estimatorStatusFactGroup.vertPosRatio()->setRawValue(estimatorStatus.pos_vert_ratio);
    _estimatorStatusFactGroup.magRatio()->setRawValue(estimatorStatus.mag_ratio);
    _estimatorStatusFactGroup.haglRatio()->setRawValue(estimatorStatus.hagl_ratio);
    _estimatorStatusFactGroup.tasRatio()->setRawValue(estimatorStatus.tas_ratio);
    _estimatorStatusFactGroup.horizPosAccuracy()->setRawValue(estimatorStatus.pos_horiz_accuracy);
    _estimatorStatusFactGroup.vertPosAccuracy()->setRawValue(estimatorStatus.pos_vert_accuracy);

#if 0
    typedef enum ESTIMATOR_STATUS_FLAGS
    {
       ESTIMATOR_ATTITUDE=1, /* True if the attitude estimate is good | */
       ESTIMATOR_VELOCITY_HORIZ=2, /* True if the horizontal velocity estimate is good | */
       ESTIMATOR_VELOCITY_VERT=4, /* True if the  vertical velocity estimate is good | */
       ESTIMATOR_POS_HORIZ_REL=8, /* True if the horizontal position (relative) estimate is good | */
       ESTIMATOR_POS_HORIZ_ABS=16, /* True if the horizontal position (absolute) estimate is good | */
       ESTIMATOR_POS_VERT_ABS=32, /* True if the vertical position (absolute) estimate is good | */
       ESTIMATOR_POS_VERT_AGL=64, /* True if the vertical position (above ground) estimate is good | */
       ESTIMATOR_CONST_POS_MODE=128, /* True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) | */
       ESTIMATOR_PRED_POS_HORIZ_REL=256, /* True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate | */
       ESTIMATOR_PRED_POS_HORIZ_ABS=512, /* True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate | */
       ESTIMATOR_GPS_GLITCH=1024, /* True if the EKF has detected a GPS glitch | */
       ESTIMATOR_ACCEL_ERROR=2048, /* True if the EKF has detected bad accelerometer data | */

        typedef struct __mavlink_estimator_status_t {
         uint64_t time_usec; /*< Timestamp (micros since boot or Unix epoch)*/
         float vel_ratio; /*< Velocity innovation test ratio*/
         float pos_horiz_ratio; /*< Horizontal position innovation test ratio*/
         float pos_vert_ratio; /*< Vertical position innovation test ratio*/
         float mag_ratio; /*< Magnetometer innovation test ratio*/
         float hagl_ratio; /*< Height above terrain innovation test ratio*/
         float tas_ratio; /*< True airspeed innovation test ratio*/
         float pos_horiz_accuracy; /*< Horizontal position 1-STD accuracy relative to the EKF local origin (m)*/
         float pos_vert_accuracy; /*< Vertical position 1-STD accuracy relative to the EKF local origin (m)*/
         uint16_t flags; /*< Integer bitmask indicating which EKF outputs are valid. See definition for ESTIMATOR_STATUS_FLAGS.*/
1009 1010
        } mavlink_estimator_status_t;
    };
1011 1012 1013
#endif
}

1014 1015 1016 1017
void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
{
    mavlink_distance_sensor_t distanceSensor;

1018 1019 1020 1021
    mavlink_msg_distance_sensor_decode(&message, &distanceSensor);\

    if (!_distanceSensorFactGroup.idSet()) {
        _distanceSensorFactGroup.setIdSet(true);
1022
        _distanceSensorFactGroup.setId(distanceSensor.id);
1023 1024
    }

1025
    if (_distanceSensorFactGroup.id() != distanceSensor.id) {
1026 1027 1028
        // We can only handle a single sensor reporting
        return;
    }
1029 1030 1031 1032 1033 1034 1035 1036

    struct orientation2Fact_s {
        MAV_SENSOR_ORIENTATION  orientation;
        Fact*                   fact;
    };

    orientation2Fact_s rgOrientation2Fact[] =
    {
1037 1038 1039 1040 1041 1042 1043 1044 1045 1046
        { MAV_SENSOR_ROTATION_NONE,         _distanceSensorFactGroup.rotationNone() },
        { MAV_SENSOR_ROTATION_YAW_45,       _distanceSensorFactGroup.rotationYaw45() },
        { MAV_SENSOR_ROTATION_YAW_90,       _distanceSensorFactGroup.rotationYaw90() },
        { MAV_SENSOR_ROTATION_YAW_135,      _distanceSensorFactGroup.rotationYaw135() },
        { MAV_SENSOR_ROTATION_YAW_180,      _distanceSensorFactGroup.rotationYaw180() },
        { MAV_SENSOR_ROTATION_YAW_225,      _distanceSensorFactGroup.rotationYaw225() },
        { MAV_SENSOR_ROTATION_YAW_270,      _distanceSensorFactGroup.rotationYaw270() },
        { MAV_SENSOR_ROTATION_YAW_315,      _distanceSensorFactGroup.rotationYaw315() },
        { MAV_SENSOR_ROTATION_PITCH_90,     _distanceSensorFactGroup.rotationPitch90() },
        { MAV_SENSOR_ROTATION_PITCH_270,    _distanceSensorFactGroup.rotationPitch270() },
1047 1048 1049 1050 1051
    };

    for (size_t i=0; i<sizeof(rgOrientation2Fact)/sizeof(rgOrientation2Fact[0]); i++) {
        const orientation2Fact_s& orientation2Fact = rgOrientation2Fact[i];
        if (orientation2Fact.orientation == distanceSensor.orientation) {
1052
            orientation2Fact.fact->setRawValue(distanceSensor.current_distance / 100.0); // cm to meters
1053 1054 1055 1056
        }
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1057 1058 1059 1060 1061 1062 1063 1064 1065
void Vehicle::_handleAttitudeTarget(mavlink_message_t& message)
{
    mavlink_attitude_target_t attitudeTarget;

    mavlink_msg_attitude_target_decode(&message, &attitudeTarget);

    float roll, pitch, yaw;
    mavlink_quaternion_to_euler(attitudeTarget.q, &roll, &pitch, &yaw);

DonLakeFlyer's avatar
DonLakeFlyer committed
1066 1067 1068
    _setpointFactGroup.roll()->setRawValue(qRadiansToDegrees(roll));
    _setpointFactGroup.pitch()->setRawValue(qRadiansToDegrees(pitch));
    _setpointFactGroup.yaw()->setRawValue(qRadiansToDegrees(yaw));
DonLakeFlyer's avatar
DonLakeFlyer committed
1069 1070 1071 1072 1073 1074

    _setpointFactGroup.rollRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_roll_rate));
    _setpointFactGroup.pitchRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_pitch_rate));
    _setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate));
}

1075
void Vehicle::_handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians)
DonLakeFlyer's avatar
DonLakeFlyer committed
1076
{
1077
    double roll, pitch, yaw;
1078

1079 1080 1081
    roll = QGC::limitAngleToPMPIf(rollRadians);
    pitch = QGC::limitAngleToPMPIf(pitchRadians);
    yaw = QGC::limitAngleToPMPIf(yawRadians);
1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095

    roll = qRadiansToDegrees(roll);
    pitch = qRadiansToDegrees(pitch);
    yaw = qRadiansToDegrees(yaw);

    if (yaw < 0.0) {
        yaw += 360.0;
    }
    // truncate to integer so widget never displays 360
    yaw = trunc(yaw);

    _rollFact.setRawValue(roll);
    _pitchFact.setRawValue(pitch);
    _headingFact.setRawValue(yaw);
1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121
}

void Vehicle::_handleAttitude(mavlink_message_t& message)
{
    if (_receivingAttitudeQuaternion) {
        return;
    }

    mavlink_attitude_t attitude;
    mavlink_msg_attitude_decode(&message, &attitude);

    _handleAttitudeWorker(attitude.roll, attitude.pitch, attitude.yaw);
}

void Vehicle::_handleAttitudeQuaternion(mavlink_message_t& message)
{
    _receivingAttitudeQuaternion = true;

    mavlink_attitude_quaternion_t attitudeQuaternion;
    mavlink_msg_attitude_quaternion_decode(&message, &attitudeQuaternion);

    float roll, pitch, yaw;
    float q[] = { attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4 };
    mavlink_quaternion_to_euler(q, &roll, &pitch, &yaw);

    _handleAttitudeWorker(roll, pitch, yaw);
1122 1123 1124 1125

    rollRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.rollspeed));
    pitchRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.pitchspeed));
    yawRate()->setRawValue(qRadiansToDegrees(attitudeQuaternion.yawspeed));
DonLakeFlyer's avatar
DonLakeFlyer committed
1126 1127
}

1128 1129 1130 1131 1132
void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
{
    mavlink_gps_raw_int_t gpsRawInt;
    mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt);

1133 1134
    _gpsRawIntMessageAvailable = true;

1135
    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
1136
        if (!_globalPositionIntMessageAvailable) {
1137 1138 1139 1140 1141
            QGeoCoordinate newPosition(gpsRawInt.lat  / (double)1E7, gpsRawInt.lon / (double)1E7, gpsRawInt.alt  / 1000.0);
            if (newPosition != _coordinate) {
                _coordinate = newPosition;
                emit coordinateChanged(_coordinate);
            }
1142
            _altitudeAMSLFact.setRawValue(gpsRawInt.alt / 1000.0);
1143 1144 1145
        }
    }

1146 1147
    _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7);
    _gpsFactGroup.lon()->setRawValue(gpsRawInt.lon * 1e-7);
1148
    _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
1149 1150 1151
    _gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.eph / 100.0);
    _gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.epv / 100.0);
    _gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.cog / 100.0);
1152 1153 1154 1155 1156 1157 1158 1159
    _gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type);
}

void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
{
    mavlink_global_position_int_t globalPositionInt;
    mavlink_msg_global_position_int_decode(&message, &globalPositionInt);

1160 1161 1162 1163 1164
    _altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0);
    _altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0);

    // ArduPilot sends bogus GLOBAL_POSITION_INT messages with lat/lat 0/0 even when it has no gps signal
    // Apparently, this is in order to transport relative altitude information.
1165
    if (globalPositionInt.lat == 0 && globalPositionInt.lon == 0) {
1166 1167 1168
        return;
    }

1169
    _globalPositionIntMessageAvailable = true;
1170 1171 1172 1173 1174
    QGeoCoordinate newPosition(globalPositionInt.lat  / (double)1E7, globalPositionInt.lon / (double)1E7, globalPositionInt.alt  / 1000.0);
    if (newPosition != _coordinate) {
        _coordinate = newPosition;
        emit coordinateChanged(_coordinate);
    }
1175 1176
}

1177 1178 1179 1180 1181
void Vehicle::_handleHighLatency2(mavlink_message_t& message)
{
    mavlink_high_latency2_t highLatency2;
    mavlink_msg_high_latency2_decode(&message, &highLatency2);

1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199
    QString previousFlightMode;
    if (_base_mode != 0 || _custom_mode != 0){
        // Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about
        // bad modes while unit testing.
        previousFlightMode = flightMode();
    }
    _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
    _custom_mode = _firmwarePlugin->highLatencyCustomModeTo32Bits(highLatency2.custom_mode);
    if (previousFlightMode != flightMode()) {
        emit flightModeChanged(flightMode());
    }

    // Assume armed since we don't know
    if (_armed != true) {
        _armed = true;
        emit armedChanged(_armed);
    }

1200 1201 1202 1203 1204 1205 1206 1207 1208
    _coordinate.setLatitude(highLatency2.latitude  / (double)1E7);
    _coordinate.setLongitude(highLatency2.longitude / (double)1E7);
    _coordinate.setAltitude(highLatency2.altitude);
    emit coordinateChanged(_coordinate);

    _airSpeedFact.setRawValue((double)highLatency2.airspeed / 5.0);
    _groundSpeedFact.setRawValue((double)highLatency2.groundspeed / 5.0);
    _climbRateFact.setRawValue((double)highLatency2.climb_rate / 10.0);
    _headingFact.setRawValue((double)highLatency2.heading * 2.0);
DonLakeFlyer's avatar
DonLakeFlyer committed
1209 1210
    _altitudeRelativeFact.setRawValue(std::numeric_limits<double>::quiet_NaN());
    _altitudeAMSLFact.setRawValue(highLatency2.altitude);
1211 1212 1213 1214

    _windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0);
    _windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0);

DonLakeFlyer's avatar
DonLakeFlyer committed
1215
    _battery1FactGroup.percentRemaining()->setRawValue(highLatency2.battery);
1216 1217 1218 1219 1220 1221 1222 1223

    _temperatureFactGroup.temperature1()->setRawValue(highLatency2.temperature_air);

    _gpsFactGroup.lat()->setRawValue(highLatency2.latitude * 1e-7);
    _gpsFactGroup.lon()->setRawValue(highLatency2.longitude * 1e-7);
    _gpsFactGroup.count()->setRawValue(0);
    _gpsFactGroup.hdop()->setRawValue(highLatency2.eph == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.eph / 10.0);
    _gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.epv / 10.0);
1224 1225

    struct failure2Sensor_s {
1226
        HL_FAILURE_FLAG         failureBit;
1227 1228 1229 1230
        MAV_SYS_STATUS_SENSOR   sensorBit;
    };

    static const failure2Sensor_s rgFailure2Sensor[] = {
1231 1232 1233 1234 1235 1236
        { HL_FAILURE_FLAG_GPS,                      MAV_SYS_STATUS_SENSOR_GPS },
        { HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE,    MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE },
        { HL_FAILURE_FLAG_ABSOLUTE_PRESSURE,        MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE },
        { HL_FAILURE_FLAG_3D_ACCEL,                 MAV_SYS_STATUS_SENSOR_3D_ACCEL },
        { HL_FAILURE_FLAG_3D_GYRO,                  MAV_SYS_STATUS_SENSOR_3D_GYRO },
        { HL_FAILURE_FLAG_3D_MAG,                   MAV_SYS_STATUS_SENSOR_3D_MAG },
1237 1238 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253
    };

    // Map from MAV_FAILURE bits to standard SYS_STATUS message handling
    uint32_t newOnboardControlSensorsEnabled = 0;
    for (size_t i=0; i<sizeof(rgFailure2Sensor)/sizeof(failure2Sensor_s); i++) {
        const failure2Sensor_s* pFailure2Sensor = &rgFailure2Sensor[i];
        if (highLatency2.failure_flags & pFailure2Sensor->failureBit) {
            // Assume if reporting as unhealthy that is it present and enabled
            newOnboardControlSensorsEnabled |= pFailure2Sensor->sensorBit;
        }
    }
    if (newOnboardControlSensorsEnabled != _onboardControlSensorsEnabled) {
        _onboardControlSensorsEnabled = newOnboardControlSensorsEnabled;
        _onboardControlSensorsPresent = newOnboardControlSensorsEnabled;
        _onboardControlSensorsUnhealthy = 0;
        emit unhealthySensorsChanged();
    }
1254 1255
}

1256 1257 1258 1259 1260
void Vehicle::_handleAltitude(mavlink_message_t& message)
{
    mavlink_altitude_t altitude;
    mavlink_msg_altitude_decode(&message, &altitude);

1261 1262 1263 1264 1265 1266 1267
    // If data from GPS is available it takes precedence over ALTITUDE message
    if (!_globalPositionIntMessageAvailable) {
        _altitudeRelativeFact.setRawValue(altitude.altitude_relative);
        if (!