Vehicle.cc 182 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 "FlightPathSegment.h"
30
#include "QGCApplication.h"
dogmaphobic's avatar
dogmaphobic committed
31
#include "QGCImageProvider.h"
32
#include "MissionCommandTree.h"
33
#include "SettingsManager.h"
34
#include "QGCQGeoCoordinate.h"
35
#include "QGCCorePlugin.h"
36
#include "QGCOptions.h"
37
#include "ADSBVehicleManager.h"
38
#include "QGCCameraManager.h"
39 40
#include "VideoReceiver.h"
#include "VideoManager.h"
41
#include "VideoSettings.h"
42
#include "PositionManager.h"
Gus Grubba's avatar
Gus Grubba committed
43
#include "VehicleObjectAvoidance.h"
44
#include "TrajectoryPoints.h"
45
#include "QGCGeo.h"
46
#include "TerrainProtocolHandler.h"
47
#include "ParameterManager.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
    , _missionItemIndexFact (0, _missionItemIndexFactName,  FactMetaData::valueTypeUint16)
418
    , _headingToNextWPFact  (0, _headingToNextWPFactName,   FactMetaData::valueTypeDouble)
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::toolIndicatorsChanged, this, &Vehicle::toolIndicatorsChanged);
    connect(_firmwarePlugin, &FirmwarePlugin::modeIndicatorsChanged, this, &Vehicle::modeIndicatorsChanged);
451

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

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

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

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

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

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

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

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

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

487 488 489 490 491
    // Build FactGroup object model

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

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

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

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

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

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

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

    _pidTuningMessages << MAVLINK_MSG_ID_ATTITUDE << MAVLINK_MSG_ID_ATTITUDE_TARGET;
557 558
}

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

563
    delete _missionManager;
564
    _missionManager = nullptr;
565

566
    delete _autopilotPlugin;
567
    _autopilotPlugin = nullptr;
568

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

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

579 580 581
void Vehicle::prepareDelete()
{
    if(_cameras) {
582 583 584
        // 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;
585
        _cameras = nullptr;
586
        delete tmpCameras;
587 588 589 590 591
        emit dynamicCamerasChanged();
        qApp->processEvents();
    }
}

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

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

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

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

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

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

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

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

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

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

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

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

720 721 722 723 724 725 726 727 728 729
    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) {
730 731 732 733 734
            _capabilitiesRetryCount++;
            if (_capabilitiesRetryCount == 1) {
                _capabilitiesRetryElapsed.start();
            } else if (_capabilitiesRetryElapsed.elapsed() > 10000){
                qCDebug(VehicleLog) << "Giving up on getting AUTOPILOT_VERSION after 10 seconds";
735
                qgcApp()->showAppMessage(QStringLiteral("Vehicle failed to send AUTOPILOT_VERSION after waiting/retrying for 10 seconds"));
736 737 738
                _handleUnsupportedRequestAutopilotCapabilities();
            } else {
                // Vehicle never sent us AUTOPILOT_VERSION response. Try again.
739
                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());
740 741 742 743 744 745 746 747
                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
748
    switch (message.msgid) {
Don Gagne's avatar
Don Gagne committed
749 750 751 752 753 754
    case MAVLINK_MSG_ID_HOME_POSITION:
        _handleHomePosition(message);
        break;
    case MAVLINK_MSG_ID_HEARTBEAT:
        _handleHeartbeat(message);
        break;
755 756 757
    case MAVLINK_MSG_ID_RADIO_STATUS:
        _handleRadioStatus(message);
        break;
Don Gagne's avatar
Don Gagne committed
758 759 760 761 762 763
    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
764 765 766 767 768 769
    case MAVLINK_MSG_ID_BATTERY_STATUS:
        _handleBatteryStatus(message);
        break;
    case MAVLINK_MSG_ID_SYS_STATUS:
        _handleSysStatus(message);
        break;
770 771 772 773 774 775 776 777 778 779 780 781
    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;
782 783 784
    case MAVLINK_MSG_ID_VIBRATION:
        _handleVibration(message);
        break;
Don Gagne's avatar
Don Gagne committed
785 786 787
    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        _handleExtendedSysState(message);
        break;
788 789 790
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
791 792 793
    case MAVLINK_MSG_ID_COMMAND_LONG:
        _handleCommandLong(message);
        break;
794
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
Gus Grubba's avatar
Gus Grubba committed
795
        _handleAutopilotVersion(link, message);
796
        break;
797 798 799
    case MAVLINK_MSG_ID_PROTOCOL_VERSION:
        _handleProtocolVersion(link, message);
        break;
800 801 802
    case MAVLINK_MSG_ID_WIND_COV:
        _handleWindCov(message);
        break;
803 804 805 806 807 808
    case MAVLINK_MSG_ID_LOGGING_DATA:
        _handleMavlinkLoggingData(message);
        break;
    case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
        _handleMavlinkLoggingDataAcked(message);
        break;
809 810 811 812 813 814 815 816 817 818 819 820
    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;
821 822 823 824 825 826 827 828
    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);
829
        break;
830 831
    case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
        _handleCameraImageCaptured(message);
832
        break;
833 834 835
    case MAVLINK_MSG_ID_ADSB_VEHICLE:
        _handleADSBVehicle(message);
        break;
836 837 838
    case MAVLINK_MSG_ID_HIGH_LATENCY2:
        _handleHighLatency2(message);
        break;
839 840 841
    case MAVLINK_MSG_ID_ATTITUDE:
        _handleAttitude(message);
        break;
842 843
    case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
        _handleAttitudeQuaternion(message);
DonLakeFlyer's avatar
DonLakeFlyer committed
844 845 846 847
        break;
    case MAVLINK_MSG_ID_ATTITUDE_TARGET:
        _handleAttitudeTarget(message);
        break;
848 849 850
    case MAVLINK_MSG_ID_DISTANCE_SENSOR:
        _handleDistanceSensor(message);
        break;
851 852 853
    case MAVLINK_MSG_ID_ESTIMATOR_STATUS:
        _handleEstimatorStatus(message);
        break;
854
    case MAVLINK_MSG_ID_STATUSTEXT:
855
        _handleStatusText(message);
856
        break;
857 858 859
    case MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS:
        _handleOrbitExecutionStatus(message);
        break;
860 861 862
    case MAVLINK_MSG_ID_MESSAGE_INTERVAL:
        _handleMessageInterval(message);
        break;
DonLakeFlyer's avatar
DonLakeFlyer committed
863 864 865
    case MAVLINK_MSG_ID_PING:
        _handlePing(link, message);
        break;
Gus Grubba's avatar
Gus Grubba committed
866 867 868
    case MAVLINK_MSG_ID_MOUNT_ORIENTATION:
        _handleGimbalOrientation(message);
        break;
Gus Grubba's avatar
Gus Grubba committed
869 870 871
    case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
        _handleObstacleDistance(message);
        break;
Don Gagne's avatar
Don Gagne committed
872

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

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

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

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

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

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

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

961
void Vehicle::_chunkedStatusTextTimeout(void)
962
{
963 964 965 966 967
    // 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
968
    }
969 970 971 972 973 974 975 976 977 978 979 980 981 982 983 984 985 986 987 988
}

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

    bool skipSpoken = false;
Don Gagne's avatar
Don Gagne committed
991
    bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm"));
Don Gagne's avatar
Don Gagne committed
992
    bool px4Prearm = messageText.startsWith(QStringLiteral("preflight"), Qt::CaseInsensitive) && severity >= MAV_SEVERITY_CRITICAL;
Don Gagne's avatar
Don Gagne committed
993
    if (ardupilotPrearm || px4Prearm) {
994 995 996 997 998 999 1000 1001 1002 1003 1004
        // 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.
1005 1006 1007
    bool readAloud = false;

    if (messageText.startsWith("#")) {
1008
        messageText.remove(0, 1);
1009 1010 1011 1012 1013 1014 1015
        readAloud = true;
    }
    else if (severity <= MAV_SEVERITY_NOTICE) {
        readAloud = true;
    }

    if (readAloud) {
1016 1017 1018 1019
        if (!skipSpoken) {
            qgcApp()->toolbox()->audioOutput()->say(messageText);
        }
    }
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 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077
    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);
    }
1078 1079
}

1080 1081 1082 1083 1084 1085 1086 1087
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);
1088
    _throttlePctFact.setRawValue(static_cast<int16_t>(vfrHud.throttle));
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 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143
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.*/
1144 1145
        } mavlink_estimator_status_t;
    };
1146 1147 1148
#endif
}

1149 1150 1151 1152
void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
{
    mavlink_distance_sensor_t distanceSensor;

1153
    mavlink_msg_distance_sensor_decode(&message, &distanceSensor);
1154 1155 1156 1157 1158 1159 1160 1161

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

    orientation2Fact_s rgOrientation2Fact[] =
    {
1162 1163 1164 1165 1166 1167 1168 1169 1170 1171
        { 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() },
1172 1173 1174 1175 1176
    };

    for (size_t i=0; i<sizeof(rgOrientation2Fact)/sizeof(rgOrientation2Fact[0]); i++) {
        const orientation2Fact_s& orientation2Fact = rgOrientation2Fact[i];
        if (orientation2Fact.orientation == distanceSensor.orientation) {
1177
            orientation2Fact.fact->setRawValue(distanceSensor.current_distance / 100.0); // cm to meters
1178 1179 1180 1181
        }
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1182 1183 1184 1185 1186 1187 1188 1189 1190
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
1191 1192 1193
    _setpointFactGroup.roll()->setRawValue(qRadiansToDegrees(roll));
    _setpointFactGroup.pitch()->setRawValue(qRadiansToDegrees(pitch));
    _setpointFactGroup.yaw()->setRawValue(qRadiansToDegrees(yaw));
DonLakeFlyer's avatar
DonLakeFlyer committed
1194 1195 1196 1197 1198 1199

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

1200
void Vehicle::_handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians)
DonLakeFlyer's avatar
DonLakeFlyer committed
1201
{
1202
    double roll, pitch, yaw;
1203

1204 1205 1206
    roll = QGC::limitAngleToPMPIf(rollRadians);
    pitch = QGC::limitAngleToPMPIf(pitchRadians);
    yaw = QGC::limitAngleToPMPIf(yawRadians);
1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220

    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);
1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240 1241
}

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

1242 1243 1244 1245 1246 1247 1248
    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[