Vehicle.cc 181 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

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
#include <Eigen/Eigen>

17 18 19 20 21
#include "Vehicle.h"
#include "MAVLinkProtocol.h"
#include "FirmwarePluginManager.h"
#include "LinkManager.h"
#include "FirmwarePlugin.h"
22
#include "UAS.h"
23
#include "JoystickManager.h"
Don Gagne's avatar
Don Gagne committed
24
#include "MissionManager.h"
Don Gagne's avatar
Don Gagne committed
25
#include "MissionController.h"
26
#include "PlanMasterController.h"
27 28
#include "GeoFenceManager.h"
#include "RallyPointManager.h"
29
#include "CoordinateVector.h"
30
#include "ParameterManager.h"
31
#include "QGCApplication.h"
dogmaphobic's avatar
dogmaphobic committed
32
#include "QGCImageProvider.h"
33
#include "MissionCommandTree.h"
34
#include "SettingsManager.h"
35
#include "QGCQGeoCoordinate.h"
36
#include "QGCCorePlugin.h"
37
#include "QGCOptions.h"
38
#include "ADSBVehicleManager.h"
39
#include "QGCCameraManager.h"
40 41
#include "VideoReceiver.h"
#include "VideoManager.h"
42
#include "VideoSettings.h"
43
#include "PositionManager.h"
Gus Grubba's avatar
Gus Grubba committed
44
#include "VehicleObjectAvoidance.h"
45
#include "TrajectoryPoints.h"
46
#include "QGCGeo.h"
47
#include "TerrainProtocolHandler.h"
Gus Grubba's avatar
Gus Grubba committed
48

49 50 51 52
#if defined(QGC_AIRMAP_ENABLED)
#include "AirspaceVehicleManager.h"
#endif

53 54
QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")

55 56 57 58
#define UPDATE_TIMER 50
#define DEFAULT_LAT  38.965767f
#define DEFAULT_LON -120.083923f

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

61 62 63
const char* Vehicle::_settingsGroup =               "Vehicle%1";        // %1 replaced with mavlink system id
const char* Vehicle::_joystickModeSettingsKey =     "JoystickMode";
const char* Vehicle::_joystickEnabledSettingsKey =  "JoystickEnabled";
64

Don Gagne's avatar
Don Gagne committed
65 66 67
const char* Vehicle::_rollFactName =                "roll";
const char* Vehicle::_pitchFactName =               "pitch";
const char* Vehicle::_headingFactName =             "heading";
DonLakeFlyer's avatar
DonLakeFlyer committed
68 69 70
const char* Vehicle::_rollRateFactName =             "rollRate";
const char* Vehicle::_pitchRateFactName =           "pitchRate";
const char* Vehicle::_yawRateFactName =             "yawRate";
Don Gagne's avatar
Don Gagne committed
71 72 73 74 75
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";
76
const char* Vehicle::_flightDistanceFactName =      "flightDistance";
77
const char* Vehicle::_flightTimeFactName =          "flightTime";
78
const char* Vehicle::_distanceToHomeFactName =      "distanceToHome";
79
const char* Vehicle::_missionItemIndexFactName =    "missionItemIndex";
80
const char* Vehicle::_headingToNextWPFactName =     "headingToNextWP";
81
const char* Vehicle::_headingToHomeFactName =       "headingToHome";
82
const char* Vehicle::_distanceToGCSFactName =       "distanceToGCS";
83
const char* Vehicle::_hobbsFactName =               "hobbs";
84
const char* Vehicle::_throttlePctFactName =         "throttlePct";
Don Gagne's avatar
Don Gagne committed
85

86 87 88 89 90 91 92 93 94
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";
95
const char* Vehicle::_terrainFactGroupName =            "terrain";
Don Gagne's avatar
Don Gagne committed
96

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

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

233 234
    connect(_mavlink, &MAVLinkProtocol::messageReceived,        this, &Vehicle::_mavlinkMessageReceived);
    connect(_mavlink, &MAVLinkProtocol::mavlinkMessageStatus,   this, &Vehicle::_mavlinkMessageStatus);
235

236 237
    _addLink(link);

238
    connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
Don Gagne's avatar
Don Gagne committed
239
    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
240
    connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
241

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

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

Don Gagne's avatar
Don Gagne committed
246 247
    connect(_uas, &UAS::imageReady,                     this, &Vehicle::_imageReady);
    connect(this, &Vehicle::remoteControlRSSIChanged,   this, &Vehicle::_remoteControlRSSIChanged);
dogmaphobic's avatar
dogmaphobic committed
248

249
    _commonInit();
Don Gagne's avatar
Don Gagne committed
250
    _autopilotPlugin = _firmwarePlugin->autopilotPlugin(this);
dogmaphobic's avatar
dogmaphobic committed
251

Don Gagne's avatar
Don Gagne committed
252 253 254 255 256
    // PreArm Error self-destruct timer
    connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
    _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
    _prearmErrorTimer.setSingleShot(true);

257 258
    // Send MAV_CMD ack timer
    _mavCommandAckTimer.setSingleShot(true);
DonLakeFlyer's avatar
DonLakeFlyer committed
259
    _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
260 261
    connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);

262 263 264 265 266
    // Chunked status text timeout timer
    _chunkedStatusTextTimer.setSingleShot(true);
    _chunkedStatusTextTimer.setInterval(1000);
    connect(&_chunkedStatusTextTimer, &QTimer::timeout, this, &Vehicle::_chunkedStatusTextTimeout);

267
    _mav = uas();
268

269
    // Listen for system messages
270 271
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
272

273 274
    if (_highLatencyLink || link->isPX4Flow()) {
        // These links don't request information
275 276 277 278 279 280 281
        _setMaxProtoVersion(100);
        _setCapabilities(0);
        _initialPlanRequestComplete = true;
        _missionManagerInitialRequestSent = true;
        _geoFenceManagerInitialRequestSent = true;
        _rallyPointManagerInitialRequestSent = true;
    } else {
282
        sendMavCommand(MAV_COMP_ID_ALL,
283 284 285
                       MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
                       false,                                   // No error shown if fails
                       1);                                      // Request firmware version
286 287 288 289
        sendMavCommand(MAV_COMP_ID_ALL,
                       MAV_CMD_REQUEST_PROTOCOL_VERSION,
                       false,                                   // No error shown if fails
                       1);                                      // Request protocol version
290
    }
291

292
    _firmwarePlugin->initializeVehicle(this);
293 294 295
    for(auto& factName: factNames()) {
        _firmwarePlugin->adjustMetaData(vehicleType, getFact(factName)->metaData());
    }
dogmaphobic's avatar
dogmaphobic committed
296

297 298
    _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay);
    connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext);
dogmaphobic's avatar
dogmaphobic committed
299

300 301
    connect(&_orbitTelemetryTimer, &QTimer::timeout, this, &Vehicle::_orbitTelemetryTimeout);

302
    // Create camera manager instance
303
    _cameras = _firmwarePlugin->createCameraManager(this);
304
    emit dynamicCamerasChanged();
305

306 307 308
    // Start csv logger
    connect(&_csvLogTimer, &QTimer::timeout, this, &Vehicle::_writeCsvLine);
    _csvLogTimer.start(1000);
309
    _lastBatteryAnnouncement.start();
310 311
}

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

    // 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);

439
    // This add correct terrain capability bit
440
    _offlineFirmwareTypeSettingChanged(_firmwareType);
441

442
    _firmwarePlugin->initializeVehicle(this);
443 444
}

445
void Vehicle::_commonInit()
446 447
{
    _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
448

449 450
    connect(_firmwarePlugin, &FirmwarePlugin::toolbarIndicatorsChanged, this, &Vehicle::toolBarIndicatorsChanged);

451
    connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceHeadingToHome);
452
    connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceToGCS);
453
    connect(this, &Vehicle::homePositionChanged,    this, &Vehicle::_updateDistanceHeadingToHome);
454
    connect(this, &Vehicle::hobbsMeterChanged,      this, &Vehicle::_updateHobbsMeter);
455

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

458
    _missionManager = new MissionManager(this);
459
    connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
460
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
461 462
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearCameraTriggerPoints);
    connect(_missionManager, &MissionManager::sendComplete,             this, &Vehicle::_clearCameraTriggerPoints);
463
    connect(_missionManager, &MissionManager::currentIndexChanged,      this, &Vehicle::_updateHeadingToNextWP);
464
    connect(_missionManager, &MissionManager::currentIndexChanged,      this, &Vehicle::_updateMissionItemIndex);
465

466 467 468
    connect(_missionManager, &MissionManager::sendComplete,             _trajectoryPoints, &TrajectoryPoints::clear);
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, _trajectoryPoints, &TrajectoryPoints::clear);

469
    _parameterManager = new ParameterManager(this);
Don Gagne's avatar
Don Gagne committed
470
    connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
471

Gus Grubba's avatar
Gus Grubba committed
472 473
    _objectAvoidance = new VehicleObjectAvoidance(this, this);

474
    // GeoFenceManager needs to access ParameterManager so make sure to create after
475
    _geoFenceManager = new GeoFenceManager(this);
476
    connect(_geoFenceManager, &GeoFenceManager::error,          this, &Vehicle::_geoFenceManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
477
    connect(_geoFenceManager, &GeoFenceManager::loadComplete,   this, &Vehicle::_geoFenceLoadComplete);
478

479
    _rallyPointManager = new RallyPointManager(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
480 481
    connect(_rallyPointManager, &RallyPointManager::error,          this, &Vehicle::_rallyPointManagerError);
    connect(_rallyPointManager, &RallyPointManager::loadComplete,   this, &Vehicle::_rallyPointLoadComplete);
482

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

486 487 488 489 490
    // Build FactGroup object model

    _addFact(&_rollFact,                _rollFactName);
    _addFact(&_pitchFact,               _pitchFactName);
    _addFact(&_headingFact,             _headingFactName);
DonLakeFlyer's avatar
DonLakeFlyer committed
491 492 493
    _addFact(&_rollRateFact,            _rollRateFactName);
    _addFact(&_pitchRateFact,           _pitchRateFactName);
    _addFact(&_yawRateFact,             _yawRateFactName);
494 495 496 497 498
    _addFact(&_groundSpeedFact,         _groundSpeedFactName);
    _addFact(&_airSpeedFact,            _airSpeedFactName);
    _addFact(&_climbRateFact,           _climbRateFactName);
    _addFact(&_altitudeRelativeFact,    _altitudeRelativeFactName);
    _addFact(&_altitudeAMSLFact,        _altitudeAMSLFactName);
499
    _addFact(&_flightDistanceFact,      _flightDistanceFactName);
500
    _addFact(&_flightTimeFact,          _flightTimeFactName);
501
    _addFact(&_distanceToHomeFact,      _distanceToHomeFactName);
502
    _addFact(&_missionItemIndexFact,    _missionItemIndexFactName);
503
    _addFact(&_headingToNextWPFact,     _headingToNextWPFactName);
504
    _addFact(&_headingToHomeFact,       _headingToHomeFactName);
505
    _addFact(&_distanceToGCSFact,       _distanceToGCSFactName);
506
    _addFact(&_throttlePctFact,         _throttlePctFactName);
507 508

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

511
    _addFactGroup(&_gpsFactGroup,               _gpsFactGroupName);
DonLakeFlyer's avatar
DonLakeFlyer committed
512 513
    _addFactGroup(&_battery1FactGroup,          _battery1FactGroupName);
    _addFactGroup(&_battery2FactGroup,          _battery2FactGroupName);
514 515 516 517 518
    _addFactGroup(&_windFactGroup,              _windFactGroupName);
    _addFactGroup(&_vibrationFactGroup,         _vibrationFactGroupName);
    _addFactGroup(&_temperatureFactGroup,       _temperatureFactGroupName);
    _addFactGroup(&_clockFactGroup,             _clockFactGroupName);
    _addFactGroup(&_distanceSensorFactGroup,    _distanceSensorFactGroupName);
519
    _addFactGroup(&_estimatorStatusFactGroup,   _estimatorStatusFactGroupName);
520
    _addFactGroup(&_terrainFactGroup,           _terrainFactGroupName);
521

Jacob Walser's avatar
Jacob Walser committed
522
    // Add firmware-specific fact groups, if provided
523 524 525 526 527 528 529
    QMap<QString, FactGroup*>* fwFactGroups = _firmwarePlugin->factGroups();
    if (fwFactGroups) {
        QMapIterator<QString, FactGroup*> i(*fwFactGroups);
        while(i.hasNext()) {
            i.next();
            _addFactGroup(i.value(), i.key());
        }
530 531
    }

532
    _flightDistanceFact.setRawValue(0);
533
    _flightTimeFact.setRawValue(0);
DonLakeFlyer's avatar
DonLakeFlyer committed
534 535 536
    _flightTimeUpdater.setInterval(1000);
    _flightTimeUpdater.setSingleShot(false);
    connect(&_flightTimeUpdater, &QTimer::timeout, this, &Vehicle::_updateFlightTime);
537

538
    // Set video stream to udp if running ArduSub and Video is disabled
539
    if (sub() && _settingsManager->videoSettings()->videoSource()->rawValue() == VideoSettings::videoDisabled) {
Gus Grubba's avatar
Gus Grubba committed
540
        _settingsManager->videoSettings()->videoSource()->setRawValue(VideoSettings::videoSourceUDPH264);
541
        _settingsManager->videoSettings()->lowLatencyMode()->setRawValue(true);
542 543
    }

544 545 546 547 548 549 550 551 552 553
    //-- 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
554 555

    _pidTuningMessages << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
556 557
}

558 559
Vehicle::~Vehicle()
{
Don Gagne's avatar
Don Gagne committed
560 561
    qCDebug(VehicleLog) << "~Vehicle" << this;

562
    delete _missionManager;
563
    _missionManager = nullptr;
564

565
    delete _autopilotPlugin;
566
    _autopilotPlugin = nullptr;
567

568
    delete _mav;
569
    _mav = nullptr;
dogmaphobic's avatar
dogmaphobic committed
570

571
#if defined(QGC_AIRMAP_ENABLED)
572 573
    if (_airspaceVehicleManager) {
        delete _airspaceVehicleManager;
574
    }
575
#endif
576 577
}

578 579 580
void Vehicle::prepareDelete()
{
    if(_cameras) {
581 582 583
        // because of _cameras QML bindings check for nullptr won't work in the binding pipeline
        // the dangling pointer access will cause a runtime fault
        auto tmpCameras = _cameras;
584
        _cameras = nullptr;
585
        delete tmpCameras;
586 587 588 589 590
        emit dynamicCamerasChanged();
        qApp->processEvents();
    }
}

591 592 593
void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
    _firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
594
    _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
595 596 597 598 599
    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        _capabilityBits |= MAV_PROTOCOL_CAPABILITY_TERRAIN;
    } else {
        _capabilityBits &= ~MAV_PROTOCOL_CAPABILITY_TERRAIN;
    }
600
    emit firmwareTypeChanged();
601
    emit capabilityBitsChanged(_capabilityBits);
602 603 604 605 606 607 608 609 610 611
}

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

void Vehicle::_offlineCruiseSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
612 613
    _defaultCruiseSpeed = value.toDouble();
    emit defaultCruiseSpeedChanged(_defaultCruiseSpeed);
614 615 616 617
}

void Vehicle::_offlineHoverSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
618 619
    _defaultHoverSpeed = value.toDouble();
    emit defaultHoverSpeedChanged(_defaultHoverSpeed);
620 621
}

622
QString Vehicle::firmwareTypeString() const
623 624 625 626 627 628 629 630 631 632
{
    if (px4Firmware()) {
        return QStringLiteral("PX4 Pro");
    } else if (apmFirmware()) {
        return QStringLiteral("ArduPilot");
    } else {
        return tr("MAVLink Generic");
    }
}

633
QString Vehicle::vehicleTypeString() const
634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650
{
    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()
651 652 653 654 655 656 657 658
{
    _messagesReceived   = 0;
    _messagesSent       = 0;
    _messagesLost       = 0;
    _messageSeq         = 0;
    _heardFrom          = false;
}

659 660
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
661
    // If the link is already running at Mavlink V2 set our max proto version to it.
DonLakeFlyer's avatar
DonLakeFlyer committed
662 663
    unsigned mavlinkVersion = _mavlink->getCurrentVersion();
    if (_maxProtoVersion != mavlinkVersion && mavlinkVersion >= 200) {
664 665
        _maxProtoVersion = mavlinkVersion;
        qCDebug(VehicleLog) << "Vehicle::_mavlinkMessageReceived Link already running Mavlink v2. Setting _maxProtoVersion" << _maxProtoVersion;
DonLakeFlyer's avatar
DonLakeFlyer committed
666 667
    }

Don Gagne's avatar
Don Gagne committed
668
    if (message.sysid != _id && message.sysid != 0) {
669 670 671 672
        // 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;
        }
673
    }
674

675 676 677
    if (!_containsLink(link)) {
        _addLink(link);
    }
dogmaphobic's avatar
dogmaphobic committed
678

679 680 681 682 683 684 685 686 687 688 689
    //-- 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) {
Gus Grubba's avatar
Gus Grubba committed
690
            uint16_t seq_received = static_cast<uint16_t>(message.seq);
691 692 693 694 695 696 697 698 699 700 701 702 703 704
            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
705
    // Give the plugin a change to adjust the message contents
706 707 708
    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
709

710 711 712 713 714
    // Give the Core Plugin access to all mavlink traffic
    if (!_toolbox->corePlugin()->mavlinkMessage(this, link, message)) {
        return;
    }

715 716 717 718
    if (!_terrainProtocolHandler->mavlinkMessageReceived(message)) {
        return;
    }

719 720 721 722 723 724 725 726 727 728
    if (!_capabilityBitsKnown && message.msgid == MAVLINK_MSG_ID_HEARTBEAT && message.compid == MAV_COMP_ID_AUTOPILOT1) {
        // We want to try to get capabilities as fast as possible so we retry on heartbeats
        bool foundRequest = false;
        for (MavCommandQueueEntry_t& queuedCommand : _mavCommandQueue) {
            if (queuedCommand.command ==  MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
                foundRequest = true;
                break;
            }
        }
        if (!foundRequest) {
729 730 731 732 733
            _capabilitiesRetryCount++;
            if (_capabilitiesRetryCount == 1) {
                _capabilitiesRetryElapsed.start();
            } else if (_capabilitiesRetryElapsed.elapsed() > 10000){
                qCDebug(VehicleLog) << "Giving up on getting AUTOPILOT_VERSION after 10 seconds";
Don Gagne's avatar
Don Gagne committed
734
                qgcApp()->showMessage(QStringLiteral("Vehicle failed to send AUTOPILOT_VERSION after waiting/retrying for 10 seconds"));
735 736 737
                _handleUnsupportedRequestAutopilotCapabilities();
            } else {
                // Vehicle never sent us AUTOPILOT_VERSION response. Try again.
738
                qCDebug(VehicleLog) << QStringLiteral("Sending MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES again due to no response with AUTOPILOT_VERSION - _capabilitiesRetryCount(%1) elapsed(%2)").arg(_capabilitiesRetryCount).arg(_capabilitiesRetryElapsed.elapsed());
739 740 741 742 743 744 745 746
                sendMavCommand(MAV_COMP_ID_ALL,
                               MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
                               true,                                    // Show error on failure
                               1);                                      // Request firmware version
            }
        }
    }

Don Gagne's avatar
Don Gagne committed
747
    switch (message.msgid) {
Don Gagne's avatar
Don Gagne committed
748 749 750 751 752 753
    case MAVLINK_MSG_ID_HOME_POSITION:
        _handleHomePosition(message);
        break;
    case MAVLINK_MSG_ID_HEARTBEAT:
        _handleHeartbeat(message);
        break;
754 755 756
    case MAVLINK_MSG_ID_RADIO_STATUS:
        _handleRadioStatus(message);
        break;
Don Gagne's avatar
Don Gagne committed
757 758 759 760 761 762
    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
763 764 765 766 767 768
    case MAVLINK_MSG_ID_BATTERY_STATUS:
        _handleBatteryStatus(message);
        break;
    case MAVLINK_MSG_ID_SYS_STATUS:
        _handleSysStatus(message);
        break;
769 770 771 772 773 774 775 776 777 778 779 780
    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;
781 782 783
    case MAVLINK_MSG_ID_VIBRATION:
        _handleVibration(message);
        break;
Don Gagne's avatar
Don Gagne committed
784 785 786
    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        _handleExtendedSysState(message);
        break;
787 788 789
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
790 791 792
    case MAVLINK_MSG_ID_COMMAND_LONG:
        _handleCommandLong(message);
        break;
793
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
Gus Grubba's avatar
Gus Grubba committed
794
        _handleAutopilotVersion(link, message);
795
        break;
796 797 798
    case MAVLINK_MSG_ID_PROTOCOL_VERSION:
        _handleProtocolVersion(link, message);
        break;
799 800 801
    case MAVLINK_MSG_ID_WIND_COV:
        _handleWindCov(message);
        break;
802 803 804 805 806 807
    case MAVLINK_MSG_ID_LOGGING_DATA:
        _handleMavlinkLoggingData(message);
        break;
    case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
        _handleMavlinkLoggingDataAcked(message);
        break;
808 809 810 811 812 813 814 815 816 817 818 819
    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;
820 821 822 823 824 825 826 827
    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);
828
        break;
829 830
    case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
        _handleCameraImageCaptured(message);
831
        break;
832 833 834
    case MAVLINK_MSG_ID_ADSB_VEHICLE:
        _handleADSBVehicle(message);
        break;
835 836 837
    case MAVLINK_MSG_ID_HIGH_LATENCY2:
        _handleHighLatency2(message);
        break;
838 839 840
    case MAVLINK_MSG_ID_ATTITUDE:
        _handleAttitude(message);
        break;
841 842
    case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
        _handleAttitudeQuaternion(message);
DonLakeFlyer's avatar
DonLakeFlyer committed
843 844 845 846
        break;
    case MAVLINK_MSG_ID_ATTITUDE_TARGET:
        _handleAttitudeTarget(message);
        break;
847 848 849
    case MAVLINK_MSG_ID_DISTANCE_SENSOR:
        _handleDistanceSensor(message);
        break;
850 851 852
    case MAVLINK_MSG_ID_ESTIMATOR_STATUS:
        _handleEstimatorStatus(message);
        break;
853
    case MAVLINK_MSG_ID_STATUSTEXT:
854
        _handleStatusText(message);
855
        break;
856 857 858
    case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS:
        _handleOrbitExecutionStatus(message);
        break;
859 860 861
    case MAVLINK_MSG_ID_MESSAGE_INTERVAL:
        _handleMessageInterval(message);
        break;
DonLakeFlyer's avatar
DonLakeFlyer committed
862 863 864
    case MAVLINK_MSG_ID_PING:
        _handlePing(link, message);
        break;
Gus Grubba's avatar
Gus Grubba committed
865 866 867
    case MAVLINK_MSG_ID_MOUNT_ORIENTATION:
        _handleGimbalOrientation(message);
        break;
Gus Grubba's avatar
Gus Grubba committed
868 869 870
    case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
        _handleObstacleDistance(message);
        break;
Don Gagne's avatar
Don Gagne committed
871

872 873 874 875 876 877 878
    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
879

880
        // Following are ArduPilot dialect messages
881 882 883 884
#if !defined(NO_ARDUPILOT_DIALECT)
    case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
        _handleCameraFeedback(message);
        break;
Don Gagne's avatar
Don Gagne committed
885 886 887
    case MAVLINK_MSG_ID_WIND:
        _handleWind(message);
        break;
888
#endif
889
    }
dogmaphobic's avatar
dogmaphobic committed
890

891 892
    // 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
893
    emit mavlinkMessageReceived(message);
dogmaphobic's avatar
dogmaphobic committed
894

895 896 897
    _uas->receiveMessage(message);
}

898
#if !defined(NO_ARDUPILOT_DIALECT)
899 900 901 902 903 904 905 906 907 908
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));
}
909
#endif
910

911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937 938 939 940
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);
}

941
void Vehicle::_orbitTelemetryTimeout()
942 943 944 945 946
{
    _orbitActive = false;
    emit orbitActiveChanged(false);
}

947 948 949 950 951 952 953 954 955 956 957 958 959
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));
    }
}

960
void Vehicle::_chunkedStatusTextTimeout(void)
961
{
962 963 964 965 966
    // Spit out all incomplete chunks
    QList<uint8_t> rgCompId = _chunkedStatusTextInfoMap.keys();
    for (uint8_t compId : rgCompId) {
        _chunkedStatusTextInfoMap[compId].rgMessageChunks.append(QString());
        _chunkedStatusTextCompleted(compId);
Don Gagne's avatar
Don Gagne committed
967
    }
968 969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987
}

void Vehicle::_chunkedStatusTextCompleted(uint8_t compId)
{
    ChunkedStatusTextInfo_t&    chunkedInfo =   _chunkedStatusTextInfoMap[compId];
    uint8_t                     severity =      chunkedInfo.severity;
    QStringList&                rgChunks =      chunkedInfo.rgMessageChunks;

    // Build up message from chunks
    QString messageText;
    for (const QString& chunk : rgChunks) {
        if (chunk.isEmpty()) {
            // Indicates missing chunk
            messageText += tr(" ... ", "Indicates missing chunk from chunked STATUS_TEXT");
        } else {
            messageText += chunk;
        }
    }

    _chunkedStatusTextInfoMap.remove(compId);
988 989

    bool skipSpoken = false;
Don Gagne's avatar
Don Gagne committed
990
    bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm"));
Don Gagne's avatar
Don Gagne committed
991
    bool px4Prearm = messageText.startsWith(QStringLiteral("preflight"), Qt::CaseInsensitive) && severity >= MAV_SEVERITY_CRITICAL;
Don Gagne's avatar
Don Gagne committed
992
    if (ardupilotPrearm || px4Prearm) {
993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009
        // 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);
        }
    }
1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067
    emit textMessageReceived(id(), compId, severity, messageText);
}

void Vehicle::_handleStatusText(mavlink_message_t& message)
{
    QByteArray  b;
    QString     messageText;

    mavlink_statustext_t statustext;
    mavlink_msg_statustext_decode(&message, &statustext);

    uint8_t compId = message.compid;

    b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
    strncpy(b.data(), statustext.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
    b[b.length()-1] = '\0';
    messageText = QString(b);
    bool includesNullTerminator = messageText.length() < MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;

    if (_chunkedStatusTextInfoMap.contains(compId) && _chunkedStatusTextInfoMap[compId].chunkId != statustext.id) {
        // We have an incomplete chunked status still pending
        _chunkedStatusTextInfoMap[compId].rgMessageChunks.append(QString());
        _chunkedStatusTextCompleted(compId);
    }

    if (statustext.id == 0) {
        // Non-chunked status text. We still use common chunked text output mechanism.
        ChunkedStatusTextInfo_t chunkedInfo;
        chunkedInfo.chunkId = 0;
        chunkedInfo.severity = statustext.severity;
        chunkedInfo.rgMessageChunks.append(messageText);
        _chunkedStatusTextInfoMap[compId] = chunkedInfo;
    } else {
        if (_chunkedStatusTextInfoMap.contains(compId)) {
            // A chunk sequence is in progress
            QStringList& chunks = _chunkedStatusTextInfoMap[compId].rgMessageChunks;
            if (statustext.chunk_seq > chunks.size()) {
                // We are missing some chunks in between, fill them in as missing
                for (int i=chunks.size(); i<statustext.chunk_seq; i++) {
                    chunks.append(QString());
                }
            }
            chunks.append(messageText);
        } else {
            // Starting a new chunk sequence
            ChunkedStatusTextInfo_t chunkedInfo;
            chunkedInfo.chunkId = statustext.id;
            chunkedInfo.severity = statustext.severity;
            chunkedInfo.rgMessageChunks.append(messageText);
            _chunkedStatusTextInfoMap[compId] = chunkedInfo;
        }
        _chunkedStatusTextTimer.start();
    }

    if (statustext.id == 0 || includesNullTerminator) {
        _chunkedStatusTextTimer.stop();
        _chunkedStatusTextCompleted(message.compid);
    }
1068 1069
}

1070 1071 1072 1073 1074 1075 1076 1077
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);
1078
    _throttlePctFact.setRawValue(static_cast<int16_t>(vfrHud.throttle));
1079 1080
}

1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 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 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133
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.*/
1134 1135
        } mavlink_estimator_status_t;
    };
1136 1137 1138
#endif
}

1139 1140 1141 1142
void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
{
    mavlink_distance_sensor_t distanceSensor;

1143
    mavlink_msg_distance_sensor_decode(&message, &distanceSensor);
1144 1145 1146 1147 1148 1149 1150 1151

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

    orientation2Fact_s rgOrientation2Fact[] =
    {
1152 1153 1154 1155 1156 1157 1158 1159 1160 1161
        { 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() },
1162 1163 1164 1165 1166
    };

    for (size_t i=0; i<sizeof(rgOrientation2Fact)/sizeof(rgOrientation2Fact[0]); i++) {
        const orientation2Fact_s& orientation2Fact = rgOrientation2Fact[i];
        if (orientation2Fact.orientation == distanceSensor.orientation) {
1167
            orientation2Fact.fact->setRawValue(distanceSensor.current_distance / 100.0); // cm to meters
1168 1169 1170 1171
        }
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1172 1173 1174 1175 1176 1177 1178 1179 1180
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
1181 1182 1183
    _setpointFactGroup.roll()->setRawValue(qRadiansToDegrees(roll));
    _setpointFactGroup.pitch()->setRawValue(qRadiansToDegrees(pitch));
    _setpointFactGroup.yaw()->setRawValue(qRadiansToDegrees(yaw));
DonLakeFlyer's avatar
DonLakeFlyer committed
1184 1185 1186 1187 1188 1189

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

1190
void Vehicle::_handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians)
DonLakeFlyer's avatar
DonLakeFlyer committed
1191
{
1192
    double roll, pitch, yaw;
1193

1194 1195 1196
    roll = QGC::limitAngleToPMPIf(rollRadians);
    pitch = QGC::limitAngleToPMPIf(pitchRadians);
    yaw = QGC::limitAngleToPMPIf(yawRadians);
1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210

    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);
1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231
}

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);

1232 1233 1234 1235 1236 1237 1238
    Eigen::Quaternionf quat(attitudeQuaternion.q1, attitudeQuaternion.q2, attitudeQuaternion.q3, attitudeQuaternion.q4);
    Eigen::Vector3f rates(attitudeQuaternion.rollspeed, attitudeQuaternion.pitchspeed, attitudeQuaternion.yawspeed);
    Eigen::Quaternionf repr_offset(attitudeQuaternion.repr_offset_q[0], attitudeQuaternion.repr_offset_q[1], attitudeQuaternion.repr_offset_q[2], attitudeQuaternion.repr_offset_q[3]);

    // if repr_offset is valid, rotate attitude and rates
    if (repr_offset.norm() >= 0.5f) {
        quat = quat * repr_offset;
1239
        rates = repr_offset * rates;
1240 1241
    }

1242
    float roll, pitch, yaw;
1243
    float q[] = { quat.w(), quat.x(), quat.y(), quat.z() };
1244 1245 1246
    mavlink_quaternion_to_euler(q, &roll,</