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

10 11 12 13 14 15

#include "Vehicle.h"
#include "MAVLinkProtocol.h"
#include "FirmwarePluginManager.h"
#include "LinkManager.h"
#include "FirmwarePlugin.h"
16
#include "UAS.h"
17
#include "JoystickManager.h"
Don Gagne's avatar
Don Gagne committed
18
#include "MissionManager.h"
Don Gagne's avatar
Don Gagne committed
19
#include "MissionController.h"
20
#include "PlanMasterController.h"
21 22
#include "GeoFenceManager.h"
#include "RallyPointManager.h"
23
#include "CoordinateVector.h"
24
#include "ParameterManager.h"
25
#include "QGCApplication.h"
dogmaphobic's avatar
dogmaphobic committed
26
#include "QGCImageProvider.h"
27
#include "GAudioOutput.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
28
#include "FollowMe.h"
29
#include "MissionCommandTree.h"
30
#include "QGroundControlQmlGlobal.h"
31
#include "SettingsManager.h"
32
#include "QGCQGeoCoordinate.h"
33
#include "QGCCorePlugin.h"
34
#include "ADSBVehicle.h"
35 36 37

QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")

38 39 40 41
#define UPDATE_TIMER 50
#define DEFAULT_LAT  38.965767f
#define DEFAULT_LON -120.083923f

42 43
extern const char* guided_mode_not_supported_by_vehicle;

44 45 46
const char* Vehicle::_settingsGroup =               "Vehicle%1";        // %1 replaced with mavlink system id
const char* Vehicle::_joystickModeSettingsKey =     "JoystickMode";
const char* Vehicle::_joystickEnabledSettingsKey =  "JoystickEnabled";
47

Don Gagne's avatar
Don Gagne committed
48 49 50 51 52 53 54 55
const char* Vehicle::_rollFactName =                "roll";
const char* Vehicle::_pitchFactName =               "pitch";
const char* Vehicle::_headingFactName =             "heading";
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";
56
const char* Vehicle::_flightDistanceFactName =      "flightDistance";
57
const char* Vehicle::_flightTimeFactName =          "flightTime";
Don Gagne's avatar
Don Gagne committed
58 59 60

const char* Vehicle::_gpsFactGroupName =        "gps";
const char* Vehicle::_batteryFactGroupName =    "battery";
Don Gagne's avatar
Don Gagne committed
61
const char* Vehicle::_windFactGroupName =       "wind";
62
const char* Vehicle::_vibrationFactGroupName =  "vibration";
63
const char* Vehicle::_temperatureFactGroupName = "temperature";
Don Gagne's avatar
Don Gagne committed
64

65 66
Vehicle::Vehicle(LinkInterface*             link,
                 int                        vehicleId,
67
                 int                        defaultComponentId,
68 69 70 71
                 MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 JoystickManager*           joystickManager)
Don Gagne's avatar
Don Gagne committed
72 73
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json")
    , _id(vehicleId)
74
    , _defaultComponentId(defaultComponentId)
75
    , _active(false)
76
    , _offlineEditingVehicle(false)
77
    , _firmwareType(firmwareType)
78
    , _vehicleType(vehicleType)
79
    , _firmwarePlugin(NULL)
80
    , _firmwarePluginInstanceData(NULL)
81
    , _autopilotPlugin(NULL)
82 83
    , _mavlink(NULL)
    , _soloFirmware(false)
84 85
    , _toolbox(qgcApp()->toolbox())
    , _settingsManager(_toolbox->settingsManager())
86
    , _joystickMode(JoystickModeRC)
87
    , _joystickEnabled(false)
88
    , _uas(NULL)
89 90 91 92 93 94 95 96
    , _mav(NULL)
    , _currentMessageCount(0)
    , _messageCount(0)
    , _currentErrorCount(0)
    , _currentWarningCount(0)
    , _currentNormalCount(0)
    , _currentMessageType(MessageNone)
    , _updateCount(0)
97 98
    , _rcRSSI(255)
    , _rcRSSIstore(255)
Don Gagne's avatar
Don Gagne committed
99
    , _autoDisconnect(false)
Don Gagne's avatar
Don Gagne committed
100
    , _flying(false)
101
    , _landing(false)
102 103 104 105
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
106 107
    , _gpsRawIntMessageAvailable(false)
    , _globalPositionIntMessageAvailable(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
108 109
    , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
    , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
Gus Grubba's avatar
Gus Grubba committed
110 111 112 113 114 115 116
    , _telemetryRRSSI(0)
    , _telemetryLRSSI(0)
    , _telemetryRXErrors(0)
    , _telemetryFixed(0)
    , _telemetryTXBuffer(0)
    , _telemetryLNoise(0)
    , _telemetryRNoise(0)
117
    , _maxProtoVersion(0)
118 119
    , _vehicleCapabilitiesKnown(false)
    , _supportsMissionItemInt(false)
120 121
    , _connectionLost(false)
    , _connectionLostEnabled(true)
DonLakeFlyer's avatar
DonLakeFlyer committed
122
    , _initialPlanRequestComplete(false)
123
    , _missionManager(NULL)
124
    , _missionManagerInitialRequestSent(false)
125
    , _geoFenceManager(NULL)
126 127 128
    , _geoFenceManagerInitialRequestSent(false)
    , _rallyPointManager(NULL)
    , _rallyPointManagerInitialRequestSent(false)
129
    , _parameterManager(NULL)
Don Gagne's avatar
Don Gagne committed
130 131 132
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
133
    , _nextSendMessageMultipleIndex(0)
134 135
    , _firmwarePluginManager(firmwarePluginManager)
    , _joystickManager(joystickManager)
dogmaphobic's avatar
dogmaphobic committed
136
    , _flowImageIndex(0)
Don Gagne's avatar
Don Gagne committed
137
    , _allLinksInactiveSent(false)
138 139 140 141 142 143
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
144 145 146
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
147 148 149
    , _firmwareCustomMajorVersion(versionNotSetValue)
    , _firmwareCustomMinorVersion(versionNotSetValue)
    , _firmwareCustomPatchVersion(versionNotSetValue)
150
    , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL)
151
    , _gitHash(versionNotSetValue)
152
    , _lastAnnouncedLowBatteryPercent(100)
Don Gagne's avatar
Don Gagne committed
153 154 155 156 157 158 159 160
    , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
    , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
    , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
    , _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)
161
    , _flightDistanceFact   (0, _flightDistanceFactName,    FactMetaData::valueTypeDouble)
162
    , _flightTimeFact       (0, _flightTimeFactName,        FactMetaData::valueTypeElapsedTimeInSeconds)
Don Gagne's avatar
Don Gagne committed
163
    , _gpsFactGroup(this)
Don Gagne's avatar
Don Gagne committed
164
    , _batteryFactGroup(this)
Don Gagne's avatar
Don Gagne committed
165
    , _windFactGroup(this)
166
    , _vibrationFactGroup(this)
167
    , _temperatureFactGroup(this)
168 169
{
    _addLink(link);
dogmaphobic's avatar
dogmaphobic committed
170

Jacob Walser's avatar
Jacob Walser committed
171 172
    connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_activeJoystickChanged);

173
    _mavlink = _toolbox->mavlinkProtocol();
dogmaphobic's avatar
dogmaphobic committed
174

dogmaphobic's avatar
dogmaphobic committed
175
    connect(_mavlink, &MAVLinkProtocol::messageReceived,     this, &Vehicle::_mavlinkMessageReceived);
176

177
    connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
Don Gagne's avatar
Don Gagne committed
178
    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
179
    connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
180

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

Don Gagne's avatar
Don Gagne committed
183 184
    connect(_uas, &UAS::imageReady,                     this, &Vehicle::_imageReady);
    connect(this, &Vehicle::remoteControlRSSIChanged,   this, &Vehicle::_remoteControlRSSIChanged);
dogmaphobic's avatar
dogmaphobic committed
185

186
    _commonInit();
Don Gagne's avatar
Don Gagne committed
187
    _autopilotPlugin = _firmwarePlugin->autopilotPlugin(this);
dogmaphobic's avatar
dogmaphobic committed
188

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

Don Gagne's avatar
Don Gagne committed
192 193 194 195 196
    // PreArm Error self-destruct timer
    connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
    _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
    _prearmErrorTimer.setSingleShot(true);

197 198
    // Connection Lost timer
    _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs);
199 200 201
    _connectionLostTimer.setSingleShot(false);
    _connectionLostTimer.start();
    connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout);
dogmaphobic's avatar
dogmaphobic committed
202

203 204 205 206 207
    // Send MAV_CMD ack timer
    _mavCommandAckTimer.setSingleShot(true);
    _mavCommandAckTimer.setInterval(_mavCommandAckTimeoutMSecs);
    connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);

208
    _mav = uas();
209

210
    // Listen for system messages
211 212
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
213 214 215
    // Now connect the new UAS
    connect(_mav, SIGNAL(attitudeChanged                    (UASInterface*,double,double,double,quint64)),              this, SLOT(_updateAttitude(UASInterface*, double, double, double, quint64)));
    connect(_mav, SIGNAL(attitudeChanged                    (UASInterface*,int,double,double,double,quint64)),          this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
216

217
    _loadSettings();
dogmaphobic's avatar
dogmaphobic committed
218

219 220 221 222 223 224
    // Ask the vehicle for protocol version info.
    sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
                    MAV_CMD_REQUEST_PROTOCOL_VERSION,
                   false,                                   // No error shown if fails
                    1);                                     // Request protocol version

225 226 227 228 229
    // Ask the vehicle for firmware version info.
    sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
                    MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES,
                   false,                                   // No error shown if fails
                    1);                                     // Request firmware version
230

231
    _firmwarePlugin->initializeVehicle(this);
dogmaphobic's avatar
dogmaphobic committed
232

233 234
    _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay);
    connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext);
dogmaphobic's avatar
dogmaphobic committed
235

236 237
    _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints);
    connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint);
238 239
}

240 241 242 243 244
// Disconnected Vehicle for offline editing
Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 QObject*                   parent)
245 246
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json", parent)
    , _id(0)
247
    , _defaultComponentId(MAV_COMP_ID_ALL)
248
    , _active(false)
249
    , _offlineEditingVehicle(true)
250 251
    , _firmwareType(firmwareType)
    , _vehicleType(vehicleType)
252
    , _firmwarePlugin(NULL)
253
    , _firmwarePluginInstanceData(NULL)
254
    , _autopilotPlugin(NULL)
255 256
    , _mavlink(NULL)
    , _soloFirmware(false)
257 258
    , _toolbox(qgcApp()->toolbox())
    , _settingsManager(_toolbox->settingsManager())
259 260 261 262 263 264 265 266 267 268 269
    , _joystickMode(JoystickModeRC)
    , _joystickEnabled(false)
    , _uas(NULL)
    , _mav(NULL)
    , _currentMessageCount(0)
    , _messageCount(0)
    , _currentErrorCount(0)
    , _currentWarningCount(0)
    , _currentNormalCount(0)
    , _currentMessageType(MessageNone)
    , _updateCount(0)
270 271
    , _rcRSSI(255)
    , _rcRSSIstore(255)
272
    , _autoDisconnect(false)
273
    , _flying(false)
274
    , _landing(false)
275 276 277 278
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
279 280
    , _gpsRawIntMessageAvailable(false)
    , _globalPositionIntMessageAvailable(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
281 282
    , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
    , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
283 284
    , _vehicleCapabilitiesKnown(true)
    , _supportsMissionItemInt(false)
285 286
    , _connectionLost(false)
    , _connectionLostEnabled(true)
DonLakeFlyer's avatar
DonLakeFlyer committed
287
    , _initialPlanRequestComplete(false)
288
    , _missionManager(NULL)
289
    , _missionManagerInitialRequestSent(false)
290
    , _geoFenceManager(NULL)
291 292 293
    , _geoFenceManagerInitialRequestSent(false)
    , _rallyPointManager(NULL)
    , _rallyPointManagerInitialRequestSent(false)
294
    , _parameterManager(NULL)
295 296 297 298
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
    , _nextSendMessageMultipleIndex(0)
299
    , _firmwarePluginManager(firmwarePluginManager)
300 301 302 303 304 305 306 307 308
    , _joystickManager(NULL)
    , _flowImageIndex(0)
    , _allLinksInactiveSent(false)
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
309 310 311
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
312 313 314 315
    , _firmwareCustomMajorVersion(versionNotSetValue)
    , _firmwareCustomMinorVersion(versionNotSetValue)
    , _firmwareCustomPatchVersion(versionNotSetValue)
    , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL)
316
    , _gitHash(versionNotSetValue)
317
    , _lastAnnouncedLowBatteryPercent(100)
318 319 320 321 322 323 324 325
    , _rollFact             (0, _rollFactName,              FactMetaData::valueTypeDouble)
    , _pitchFact            (0, _pitchFactName,             FactMetaData::valueTypeDouble)
    , _headingFact          (0, _headingFactName,           FactMetaData::valueTypeDouble)
    , _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)
326 327
    , _flightDistanceFact   (0, _flightDistanceFactName,    FactMetaData::valueTypeDouble)
    , _flightTimeFact       (0, _flightTimeFactName,        FactMetaData::valueTypeElapsedTimeInSeconds)
328 329 330 331 332
    , _gpsFactGroup(this)
    , _batteryFactGroup(this)
    , _windFactGroup(this)
    , _vibrationFactGroup(this)
{
333
    _commonInit();
334
    _firmwarePlugin->initializeVehicle(this);
335 336 337 338 339
}

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

    _missionManager = new MissionManager(this);
342
    connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
343
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
344 345 346 347
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearCameraTriggerPoints);
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_clearTrajectoryPoints);
    connect(_missionManager, &MissionManager::sendComplete,             this, &Vehicle::_clearCameraTriggerPoints);
    connect(_missionManager, &MissionManager::sendComplete,             this, &Vehicle::_clearTrajectoryPoints);
348

349
    _parameterManager = new ParameterManager(this);
Don Gagne's avatar
Don Gagne committed
350
    connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
351

352
    // GeoFenceManager needs to access ParameterManager so make sure to create after
353
    _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
354
    connect(_geoFenceManager, &GeoFenceManager::error,          this, &Vehicle::_geoFenceManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
355
    connect(_geoFenceManager, &GeoFenceManager::loadComplete,   this, &Vehicle::_geoFenceLoadComplete);
356

357
    _rallyPointManager = _firmwarePlugin->newRallyPointManager(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
358 359
    connect(_rallyPointManager, &RallyPointManager::error,          this, &Vehicle::_rallyPointManagerError);
    connect(_rallyPointManager, &RallyPointManager::loadComplete,   this, &Vehicle::_rallyPointLoadComplete);
360

361
    // Offline editing vehicle tracks settings changes for offline editing settings
362 363 364 365
    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);
366

367 368 369 370 371 372 373 374 375 376
    // Build FactGroup object model

    _addFact(&_rollFact,                _rollFactName);
    _addFact(&_pitchFact,               _pitchFactName);
    _addFact(&_headingFact,             _headingFactName);
    _addFact(&_groundSpeedFact,         _groundSpeedFactName);
    _addFact(&_airSpeedFact,            _airSpeedFactName);
    _addFact(&_climbRateFact,           _climbRateFactName);
    _addFact(&_altitudeRelativeFact,    _altitudeRelativeFactName);
    _addFact(&_altitudeAMSLFact,        _altitudeAMSLFactName);
377
    _addFact(&_flightDistanceFact,      _flightDistanceFactName);
378
    _addFact(&_flightTimeFact,          _flightTimeFactName);
379 380 381 382 383

    _addFactGroup(&_gpsFactGroup,       _gpsFactGroupName);
    _addFactGroup(&_batteryFactGroup,   _batteryFactGroupName);
    _addFactGroup(&_windFactGroup,      _windFactGroupName);
    _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
384
    _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
385

Jacob Walser's avatar
Jacob Walser committed
386
    // Add firmware-specific fact groups, if provided
387 388 389 390 391 392 393
    QMap<QString, FactGroup*>* fwFactGroups = _firmwarePlugin->factGroups();
    if (fwFactGroups) {
        QMapIterator<QString, FactGroup*> i(*fwFactGroups);
        while(i.hasNext()) {
            i.next();
            _addFactGroup(i.value(), i.key());
        }
394 395
    }

396
    _flightDistanceFact.setRawValue(0);
397
    _flightTimeFact.setRawValue(0);
398 399
}

400 401
Vehicle::~Vehicle()
{
Don Gagne's avatar
Don Gagne committed
402 403
    qCDebug(VehicleLog) << "~Vehicle" << this;

404 405
    delete _missionManager;
    _missionManager = NULL;
406

407 408 409
    delete _autopilotPlugin;
    _autopilotPlugin = NULL;

410 411
    delete _mav;
    _mav = NULL;
dogmaphobic's avatar
dogmaphobic committed
412

413 414
}

415 416 417 418 419 420 421 422 423 424 425 426 427 428
void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
    _firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
    emit firmwareTypeChanged();
}

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

void Vehicle::_offlineCruiseSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
429 430
    _defaultCruiseSpeed = value.toDouble();
    emit defaultCruiseSpeedChanged(_defaultCruiseSpeed);
431 432 433 434
}

void Vehicle::_offlineHoverSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
435 436
    _defaultHoverSpeed = value.toDouble();
    emit defaultHoverSpeedChanged(_defaultHoverSpeed);
437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467
}

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

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

void Vehicle::resetCounters()
468 469 470 471 472 473 474 475
{
    _messagesReceived   = 0;
    _messagesSent       = 0;
    _messagesLost       = 0;
    _messageSeq         = 0;
    _heardFrom          = false;
}

476 477
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
Don Gagne's avatar
Don Gagne committed
478
    if (message.sysid != _id && message.sysid != 0) {
479 480 481 482
        // 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;
        }
483
    }
484

485 486
    if (!_containsLink(link)) {
        _addLink(link);
487 488 489 490 491 492

        // if the minimum supported version of MAVLink is already 2.0
        // set our max proto version to it.
        if (_mavlink->getCurrentVersion() >= 200) {
            _maxProtoVersion = _mavlink->getCurrentVersion();
        }
493
    }
dogmaphobic's avatar
dogmaphobic committed
494

495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520
    //-- Check link status
    _messagesReceived++;
    emit messagesReceivedChanged();
    if(!_heardFrom) {
        if(message.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
            _heardFrom = true;
            _compID = message.compid;
            _messageSeq = message.seq + 1;
        }
    } else {
        if(_compID == message.compid) {
            uint16_t seq_received = (uint16_t)message.seq;
            uint16_t packet_lost_count = 0;
            //-- Account for overflow during packet loss
            if(seq_received < _messageSeq) {
                packet_lost_count = (seq_received + 255) - _messageSeq;
            } else {
                packet_lost_count = seq_received - _messageSeq;
            }
            _messageSeq = message.seq + 1;
            _messagesLost += packet_lost_count;
            if(packet_lost_count)
                emit messagesLostChanged();
        }
    }

521

522 523 524 525 526
    // Mark this vehicle as active - but only if the traffic is coming from
    // the actual vehicle
    if (message.sysid == _id) {
        _connectionActive();
    }
527

Don Gagne's avatar
Don Gagne committed
528
    // Give the plugin a change to adjust the message contents
529 530 531
    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
532

533 534 535 536 537
    // Give the Core Plugin access to all mavlink traffic
    if (!_toolbox->corePlugin()->mavlinkMessage(this, link, message)) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
538
    switch (message.msgid) {
Don Gagne's avatar
Don Gagne committed
539 540 541 542 543 544
    case MAVLINK_MSG_ID_HOME_POSITION:
        _handleHomePosition(message);
        break;
    case MAVLINK_MSG_ID_HEARTBEAT:
        _handleHeartbeat(message);
        break;
545 546 547
    case MAVLINK_MSG_ID_RADIO_STATUS:
        _handleRadioStatus(message);
        break;
Don Gagne's avatar
Don Gagne committed
548 549 550 551 552 553
    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
554 555 556 557 558 559
    case MAVLINK_MSG_ID_BATTERY_STATUS:
        _handleBatteryStatus(message);
        break;
    case MAVLINK_MSG_ID_SYS_STATUS:
        _handleSysStatus(message);
        break;
560 561 562 563 564 565 566 567 568 569 570 571
    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;
572 573 574
    case MAVLINK_MSG_ID_VIBRATION:
        _handleVibration(message);
        break;
Don Gagne's avatar
Don Gagne committed
575 576 577
    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        _handleExtendedSysState(message);
        break;
578 579 580
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
581 582 583
    case MAVLINK_MSG_ID_COMMAND_LONG:
        _handleCommandLong(message);
        break;
584
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
Gus Grubba's avatar
Gus Grubba committed
585
        _handleAutopilotVersion(link, message);
586
        break;
587 588 589
    case MAVLINK_MSG_ID_PROTOCOL_VERSION:
        _handleProtocolVersion(link, message);
        break;
590 591 592
    case MAVLINK_MSG_ID_WIND_COV:
        _handleWindCov(message);
        break;
593 594 595
    case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
        _handleHilActuatorControls(message);
        break;
596 597 598 599 600 601
    case MAVLINK_MSG_ID_LOGGING_DATA:
        _handleMavlinkLoggingData(message);
        break;
    case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
        _handleMavlinkLoggingDataAcked(message);
        break;
602 603 604 605 606 607 608 609 610 611 612 613
    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;
614 615 616 617 618 619 620 621
    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);
622 623 624 625 626 627
        break;        
    case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
        _handleCameraFeedback(message);
        break;
    case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
        _handleCameraImageCaptured(message);
628
        break;
629 630 631
    case MAVLINK_MSG_ID_ADSB_VEHICLE:
        _handleADSBVehicle(message);
        break;
Don Gagne's avatar
Don Gagne committed
632

633 634 635 636 637 638 639
    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
640 641 642 643 644
    // Following are ArduPilot dialect messages

    case MAVLINK_MSG_ID_WIND:
        _handleWind(message);
        break;
645
    }
dogmaphobic's avatar
dogmaphobic committed
646

Don Gagne's avatar
Don Gagne committed
647
    emit mavlinkMessageReceived(message);
dogmaphobic's avatar
dogmaphobic committed
648

649 650 651
    _uas->receiveMessage(message);
}

652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676

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

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

677 678 679 680 681 682 683 684 685 686 687 688 689 690 691
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);
}

void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
{
    mavlink_gps_raw_int_t gpsRawInt;
    mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt);

692 693
    _gpsRawIntMessageAvailable = true;

694
    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
695
        if (!_globalPositionIntMessageAvailable) {
696 697 698 699 700
            //-- Set these here and emit a single signal instead of 3 for the same variable (_coordinate)
            _coordinate.setLatitude(gpsRawInt.lat  / (double)1E7);
            _coordinate.setLongitude(gpsRawInt.lon / (double)1E7);
            _coordinate.setAltitude(gpsRawInt.alt  / 1000.0);
            emit coordinateChanged(_coordinate);
701
            _altitudeAMSLFact.setRawValue(gpsRawInt.alt / 1000.0);
702 703 704
        }
    }

705 706
    _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7);
    _gpsFactGroup.lon()->setRawValue(gpsRawInt.lon * 1e-7);
707
    _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
708 709 710
    _gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.eph / 100.0);
    _gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.epv / 100.0);
    _gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? std::numeric_limits<double>::quiet_NaN() : gpsRawInt.cog / 100.0);
711 712 713 714 715 716 717 718
    _gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type);
}

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

719 720
    // ArduPilot sends bogus GLOBAL_POSITION_INT messages with lat/lat 0/0 even when it has not gps signal
    if (globalPositionInt.lat == 0 && globalPositionInt.lon == 0) {
721 722 723
        return;
    }

724
    _globalPositionIntMessageAvailable = true;
725 726 727 728 729
    //-- Set these here and emit a single signal instead of 3 for the same variable (_coordinate)
    _coordinate.setLatitude(globalPositionInt.lat  / (double)1E7);
    _coordinate.setLongitude(globalPositionInt.lon / (double)1E7);
    _coordinate.setAltitude(globalPositionInt.alt  / 1000.0);
    emit coordinateChanged(_coordinate);
730 731
    _altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0);
    _altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0);
732 733 734 735 736 737 738
}

void Vehicle::_handleAltitude(mavlink_message_t& message)
{
    mavlink_altitude_t altitude;
    mavlink_msg_altitude_decode(&message, &altitude);

739 740 741 742 743 744 745
    // If data from GPS is available it takes precedence over ALTITUDE message
    if (!_globalPositionIntMessageAvailable) {
        _altitudeRelativeFact.setRawValue(altitude.altitude_relative);
        if (!_gpsRawIntMessageAvailable) {
            _altitudeAMSLFact.setRawValue(altitude.altitude_amsl);
        }
    }
746 747
}

DonLakeFlyer's avatar
DonLakeFlyer committed
748 749 750 751 752 753
void Vehicle::_setCapabilities(uint64_t capabilityBits)
{
    if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT) {
        _supportsMissionItemInt = true;
    }
    _vehicleCapabilitiesKnown = true;
754
    emit capabilitiesKnownChanged(true);
DonLakeFlyer's avatar
DonLakeFlyer committed
755

756 757 758 759 760 761 762
    // This should potentially be turned into a user-facing warning
    // if the general experience after deployment is that users want MAVLink 2
    // but forget to upgrade their radio firmware
    if (capabilityBits & MAV_PROTOCOL_CAPABILITY_MAVLINK2 && maxProtoVersion() < 200) {
        qCDebug(VehicleLog) << QString("Vehicle does support MAVLink 2 but the link does not allow for it.");
    }

DonLakeFlyer's avatar
DonLakeFlyer committed
763 764 765
    qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_supportsMissionItemInt ? QStringLiteral("supports") : QStringLiteral("does not support"));
}

Gus Grubba's avatar
Gus Grubba committed
766
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
767
{
768 769
    Q_UNUSED(link);

770 771 772 773 774 775 776 777 778 779 780
    mavlink_autopilot_version_t autopilotVersion;
    mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);

    if (autopilotVersion.flight_sw_version != 0) {
        int majorVersion, minorVersion, patchVersion;
        FIRMWARE_VERSION_TYPE versionType;

        majorVersion = (autopilotVersion.flight_sw_version >> (8*3)) & 0xFF;
        minorVersion = (autopilotVersion.flight_sw_version >> (8*2)) & 0xFF;
        patchVersion = (autopilotVersion.flight_sw_version >> (8*1)) & 0xFF;
        versionType = (FIRMWARE_VERSION_TYPE)((autopilotVersion.flight_sw_version >> (8*0)) & 0xFF);
Don Gagne's avatar
Don Gagne committed
781
        setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
782
    }
783

Don Gagne's avatar
Don Gagne committed
784 785 786 787 788 789 790 791
    if (px4Firmware()) {
        // Lower 3 bytes is custom version
        int majorVersion, minorVersion, patchVersion;
        majorVersion = autopilotVersion.flight_custom_version[2];
        minorVersion = autopilotVersion.flight_custom_version[1];
        patchVersion = autopilotVersion.flight_custom_version[0];
        setFirmwareCustomVersion(majorVersion, minorVersion, patchVersion);

792
        // PX4 Firmware stores the first 16 characters of the git hash as binary, with the individual bytes in reverse order
Don Gagne's avatar
Don Gagne committed
793 794 795 796
        _gitHash = "";
        QByteArray array((char*)autopilotVersion.flight_custom_version, 8);
        for (int i = 7; i >= 0; i--) {
            _gitHash.append(QString("%1").arg(autopilotVersion.flight_custom_version[i], 2, 16, QChar('0')));
797
        }
Don Gagne's avatar
Don Gagne committed
798 799 800
    } else {
        // APM Firmware stores the first 8 characters of the git hash as an ASCII character string
        _gitHash = QString::fromUtf8((char*)autopilotVersion.flight_custom_version, 8);
801
    }
Don Gagne's avatar
Don Gagne committed
802
    emit gitHashChanged(_gitHash);
803

DonLakeFlyer's avatar
DonLakeFlyer committed
804 805
    _setCapabilities(autopilotVersion.capabilities);
    _startPlanRequest();
806 807
}

808 809 810 811 812 813 814 815 816 817 818
void Vehicle::_handleProtocolVersion(LinkInterface *link, mavlink_message_t& message)
{
    Q_UNUSED(link);

    mavlink_protocol_version_t protoVersion;
    mavlink_msg_protocol_version_decode(&message, &protoVersion);

    _setMaxProtoVersion(protoVersion.max_version);
}

void Vehicle::_setMaxProtoVersion(unsigned version) {
819 820 821 822 823 824

    // Set only once or if we need to reduce the max version
    if (_maxProtoVersion == 0 || version < _maxProtoVersion) {
        _maxProtoVersion = version;
        emit requestProtocolVersion(_maxProtoVersion);
    }
825 826
}

827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850
void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
{
    mavlink_hil_actuator_controls_t hil;
    mavlink_msg_hil_actuator_controls_decode(&message, &hil);
    emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
                                    hil.controls[0],
                                    hil.controls[1],
                                    hil.controls[2],
                                    hil.controls[3],
                                    hil.controls[4],
                                    hil.controls[5],
                                    hil.controls[6],
                                    hil.controls[7],
                                    hil.controls[8],
                                    hil.controls[9],
                                    hil.controls[10],
                                    hil.controls[11],
                                    hil.controls[12],
                                    hil.controls[13],
                                    hil.controls[14],
                                    hil.controls[15],
                                    hil.mode);
}

851 852
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
853
    bool showError = false;
854

855 856 857
    mavlink_command_ack_t ack;
    mavlink_msg_command_ack_decode(&message, &ack);

858 859 860
    if (ack.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES && ack.result != MAV_RESULT_ACCEPTED) {
        // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
        qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error. Starting mission request.";
DonLakeFlyer's avatar
DonLakeFlyer committed
861
        _setCapabilities(0);
DonLakeFlyer's avatar
DonLakeFlyer committed
862
        _startPlanRequest();
863 864
    }

865 866 867
    if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION && ack.result != MAV_RESULT_ACCEPTED) {
        // The autopilot does not understand the request and consequently is likely handling only
        // MAVLink 1
868 869 870
        if (_maxProtoVersion == 0) {
            _setMaxProtoVersion(100);
        }
871 872
    }

873
    if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
Don Gagne's avatar
Don Gagne committed
874
        _mavCommandAckTimer.stop();
875 876 877 878
        showError = _mavCommandQueue[0].showError;
        _mavCommandQueue.removeFirst();
    }

Don Gagne's avatar
Don Gagne committed
879
    emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
880 881

    if (showError) {
882
        QString commandName = _toolbox->missionCommandTree()->friendlyName((MAV_CMD)ack.command);
883

884 885 886 887 888 889 890 891 892 893 894 895 896
        switch (ack.result) {
        case MAV_RESULT_TEMPORARILY_REJECTED:
            qgcApp()->showMessage(tr("%1 command temporarily rejected").arg(commandName));
            break;
        case MAV_RESULT_DENIED:
            qgcApp()->showMessage(tr("%1 command denied").arg(commandName));
            break;
        case MAV_RESULT_UNSUPPORTED:
            qgcApp()->showMessage(tr("%1 command not supported").arg(commandName));
            break;
        case MAV_RESULT_FAILED:
            qgcApp()->showMessage(tr("%1 command failed").arg(commandName));
            break;
897
        default:
898
            // Do nothing
899
            break;
900
        }
901 902
    }

903
    _sendNextQueuedMavCommand();
904 905
}

906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928
void Vehicle::_handleCommandLong(mavlink_message_t& message)
{
    mavlink_command_long_t cmd;
    mavlink_msg_command_long_decode(&message, &cmd);

    switch (cmd.command) {
        // Other component on the same system
        // requests that QGC frees up the serial port of the autopilot
        case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
            if (cmd.param6 > 0) {
                // disconnect the USB link if its a direct connection to a Pixhawk
                for (int i = 0; i < _links.length(); i++) {
                    SerialLink *sl = qobject_cast<SerialLink*>(_links.at(i));
                    if (sl && sl->getSerialConfig()->usbDirect()) {
                        qDebug() << "Disconnecting:" << _links.at(i)->getName();
                        qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i));
                    }
                }
            }
        break;
    }
}

Don Gagne's avatar
Don Gagne committed
929 930 931 932 933 934 935
void Vehicle::_handleExtendedSysState(mavlink_message_t& message)
{
    mavlink_extended_sys_state_t extendedState;
    mavlink_msg_extended_sys_state_decode(&message, &extendedState);

    switch (extendedState.landed_state) {
    case MAV_LANDED_STATE_ON_GROUND:
936 937
        _setFlying(false);
        _setLanding(false);
Don Gagne's avatar
Don Gagne committed
938
        break;
939
    case MAV_LANDED_STATE_TAKEOFF:
Don Gagne's avatar
Don Gagne committed
940
    case MAV_LANDED_STATE_IN_AIR:
941 942 943 944 945 946 947 948 949
        _setFlying(true);
        _setLanding(false);
        break;
    case MAV_LANDED_STATE_LANDING:
        _setFlying(true);
        _setLanding(true);
        break;
    default:
        break;
Don Gagne's avatar
Don Gagne committed
950 951 952
    }
}

953 954 955 956 957 958 959 960 961 962 963 964 965
void Vehicle::_handleVibration(mavlink_message_t& message)
{
    mavlink_vibration_t vibration;
    mavlink_msg_vibration_decode(&message, &vibration);

    _vibrationFactGroup.xAxis()->setRawValue(vibration.vibration_x);
    _vibrationFactGroup.yAxis()->setRawValue(vibration.vibration_y);
    _vibrationFactGroup.zAxis()->setRawValue(vibration.vibration_z);
    _vibrationFactGroup.clipCount1()->setRawValue(vibration.clipping_0);
    _vibrationFactGroup.clipCount2()->setRawValue(vibration.clipping_1);
    _vibrationFactGroup.clipCount3()->setRawValue(vibration.clipping_2);
}

966 967 968 969 970 971 972 973 974 975 976 977 978
void Vehicle::_handleWindCov(mavlink_message_t& message)
{
    mavlink_wind_cov_t wind;
    mavlink_msg_wind_cov_decode(&message, &wind);

    float direction = qRadiansToDegrees(qAtan2(wind.wind_y, wind.wind_x));
    float speed = qSqrt(qPow(wind.wind_x, 2) + qPow(wind.wind_y, 2));

    _windFactGroup.direction()->setRawValue(direction);
    _windFactGroup.speed()->setRawValue(speed);
    _windFactGroup.verticalSpeed()->setRawValue(0);
}

Don Gagne's avatar
Don Gagne committed
979 980 981 982 983 984 985 986 987 988
void Vehicle::_handleWind(mavlink_message_t& message)
{
    mavlink_wind_t wind;
    mavlink_msg_wind_decode(&message, &wind);

    _windFactGroup.direction()->setRawValue(wind.direction);
    _windFactGroup.speed()->setRawValue(wind.speed);
    _windFactGroup.verticalSpeed()->setRawValue(wind.speed_z);
}

Don Gagne's avatar
Don Gagne committed
989 990 991 992 993 994 995 996
void Vehicle::_handleSysStatus(mavlink_message_t& message)
{
    mavlink_sys_status_t sysStatus;
    mavlink_msg_sys_status_decode(&message, &sysStatus);

    if (sysStatus.current_battery == -1) {
        _batteryFactGroup.current()->setRawValue(VehicleBatteryFactGroup::_currentUnavailable);
    } else {
997
        // Current is in Amps, current_battery is 10 * milliamperes (1 = 10 milliampere)
998
        _batteryFactGroup.current()->setRawValue((float)sysStatus.current_battery / 100.0f);
Don Gagne's avatar
Don Gagne committed
999 1000 1001 1002 1003 1004 1005
    }
    if (sysStatus.voltage_battery == UINT16_MAX) {
        _batteryFactGroup.voltage()->setRawValue(VehicleBatteryFactGroup::_voltageUnavailable);
    } else {
        _batteryFactGroup.voltage()->setRawValue((double)sysStatus.voltage_battery / 1000.0);
    }
    _batteryFactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining);
1006

1007 1008 1009 1010 1011
    if (sysStatus.battery_remaining > 0 &&
            sysStatus.battery_remaining < _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt() &&
            sysStatus.battery_remaining < _lastAnnouncedLowBatteryPercent) {
        _lastAnnouncedLowBatteryPercent = sysStatus.battery_remaining;
        _say(QString("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining));
1012
    }
1013 1014 1015 1016 1017 1018 1019 1020 1021 1022

    _onboardControlSensorsPresent = sysStatus.onboard_control_sensors_present;
    _onboardControlSensorsEnabled = sysStatus.onboard_control_sensors_enabled;
    _onboardControlSensorsHealth = sysStatus.onboard_control_sensors_health;

    uint32_t newSensorsUnhealthy = _onboardControlSensorsEnabled & ~_onboardControlSensorsHealth;
    if (newSensorsUnhealthy != _onboardControlSensorsUnhealthy) {
        _onboardControlSensorsUnhealthy = newSensorsUnhealthy;
        emit unhealthySensorsChanged();
    }
Don Gagne's avatar
Don Gagne committed
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
}

void Vehicle::_handleBatteryStatus(mavlink_message_t& message)
{
    mavlink_battery_status_t bat_status;
    mavlink_msg_battery_status_decode(&message, &bat_status);

    if (bat_status.temperature == INT16_MAX) {
        _batteryFactGroup.temperature()->setRawValue(VehicleBatteryFactGroup::_temperatureUnavailable);
    } else {
        _batteryFactGroup.temperature()->setRawValue((double)bat_status.temperature / 100.0);
    }
    if (bat_status.current_consumed == -1) {
        _batteryFactGroup.mahConsumed()->setRawValue(VehicleBatteryFactGroup::_mahConsumedUnavailable);
    } else {
        _batteryFactGroup.mahConsumed()->setRawValue(bat_status.current_consumed);
    }

    int cellCount = 0;
    for (int i=0; i<10; i++) {
        if (bat_status.voltages[i] != UINT16_MAX) {
            cellCount++;
        }
    }
    if (cellCount == 0) {
        cellCount = -1;
    }

    _batteryFactGroup.cellCount()->setRawValue(cellCount);
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1054 1055 1056 1057 1058 1059 1060 1061
void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord)
{
    if (homeCoord != _homePosition) {
        _homePosition = homeCoord;
        emit homePositionChanged(_homePosition);
    }
}

Don Gagne's avatar
Don Gagne committed
1062 1063 1064
void Vehicle::_handleHomePosition(mavlink_message_t& message)
{
    mavlink_home_position_t homePos;
dogmaphobic's avatar
dogmaphobic committed
1065

Don Gagne's avatar
Don Gagne committed
1066
    mavlink_msg_home_position_decode(&message, &homePos);
Don Gagne's avatar
Don Gagne committed
1067 1068 1069 1070

    QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
                                    homePos.longitude / 10000000.0,
                                    homePos.altitude / 1000.0);
DonLakeFlyer's avatar
DonLakeFlyer committed
1071
    _setHomePosition(newHomePosition);
Don Gagne's avatar
Don Gagne committed
1072 1073 1074 1075
}

void Vehicle::_handleHeartbeat(mavlink_message_t& message)
{
1076 1077 1078 1079
    if (message.compid != _defaultComponentId) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
1080
    mavlink_heartbeat_t heartbeat;
dogmaphobic's avatar
dogmaphobic committed
1081

Don Gagne's avatar
Don Gagne committed
1082
    mavlink_msg_heartbeat_decode(&message, &heartbeat);
dogmaphobic's avatar
dogmaphobic committed
1083

Don Gagne's avatar
Don Gagne committed
1084
    bool newArmed = heartbeat.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY;
dogmaphobic's avatar
dogmaphobic committed
1085

Don Gagne's avatar
Don Gagne committed
1086 1087 1088
    if (_armed != newArmed) {
        _armed = newArmed;
        emit armedChanged(_armed);
dogmaphobic's avatar
dogmaphobic committed
1089

1090 1091 1092
        // We are transitioning to the armed state, begin tracking trajectory points for the map
        if (_armed) {
            _mapTrajectoryStart();
1093
            _clearCameraTriggerPoints();
1094 1095 1096
        } else {
            _mapTrajectoryStop();
        }
Don Gagne's avatar
Don Gagne committed
1097 1098 1099
    }

    if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) {
1100 1101 1102 1103 1104 1105
        QString previousFlightMode;
        if (_base_mode != 0 || _custom_mode != 0){
            // Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about
            // bad modes while unit testing.
            previousFlightMode = flightMode();
        }
Don Gagne's avatar
Don Gagne committed
1106 1107
        _base_mode = heartbeat.base_mode;
        _custom_mode = heartbeat.custom_mode;
1108 1109 1110
        if (previousFlightMode != flightMode()) {
            emit flightModeChanged(flightMode());
        }
Don Gagne's avatar
Don Gagne committed
1111 1112 1113
    }
}

1114 1115
void Vehicle::_handleRadioStatus(mavlink_message_t& message)
{
1116

1117 1118 1119
    //-- Process telemetry status message
    mavlink_radio_status_t rstatus;
    mavlink_msg_radio_status_decode(&message, &rstatus);
1120

1121 1122
    int rssi    = rstatus.rssi;
    int remrssi = rstatus.remrssi;
1123 1124
    int lnoise = (int)(int8_t)rstatus.noise;
    int rnoise = (int)(int8_t)rstatus.remnoise;
1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138
    //-- 3DR Si1k radio needs rssi fields to be converted to dBm
    if (message.sysid == '3' && message.compid == 'D') {
        /* Per the Si1K datasheet figure 23.25 and SI AN474 code
         * samples the relationship between the RSSI register
         * and received power is as follows:
         *
         *                       10
         * inputPower = rssi * ------ 127
         *                       19
         *
         * Additionally limit to the only realistic range [-120,0] dBm
         */
        rssi    = qMin(qMax(qRound(static_cast<qreal>(rssi)    / 1.9 - 127.0), - 120), 0);
        remrssi = qMin(qMax(qRound(static_cast<qreal>(remrssi) / 1.9 - 127.0), - 120), 0);
1139 1140 1141
    } else {
        rssi    = (int)(int8_t)rstatus.rssi;
        remrssi = (int)(int8_t)rstatus.remrssi;
1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163
    }
    //-- Check for changes
    if(_telemetryLRSSI != rssi) {
        _telemetryLRSSI = rssi;
        emit telemetryLRSSIChanged(_telemetryLRSSI);
    }
    if(_telemetryRRSSI != remrssi) {
        _telemetryRRSSI = remrssi;
        emit telemetryRRSSIChanged(_telemetryRRSSI);
    }
    if(_telemetryRXErrors != rstatus.rxerrors) {
        _telemetryRXErrors = rstatus.rxerrors;
        emit telemetryRXErrorsChanged(_telemetryRXErrors);
    }
    if(_telemetryFixed != rstatus.fixed) {
        _telemetryFixed = rstatus.fixed;
        emit telemetryFixedChanged(_telemetryFixed);
    }
    if(_telemetryTXBuffer != rstatus.txbuf) {
        _telemetryTXBuffer = rstatus.txbuf;
        emit telemetryTXBufferChanged(_telemetryTXBuffer);
    }
1164 1165
    if(_telemetryLNoise != lnoise) {
        _telemetryLNoise = lnoise;
1166 1167
        emit telemetryLNoiseChanged(_telemetryLNoise);
    }
1168 1169
    if(_telemetryRNoise != rnoise) {
        _telemetryRNoise = rnoise;
1170 1171 1172 1173
        emit telemetryRNoiseChanged(_telemetryRNoise);
    }
}

Don Gagne's avatar
Don Gagne committed
1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238
void Vehicle::_handleRCChannels(mavlink_message_t& message)
{
    mavlink_rc_channels_t channels;

    mavlink_msg_rc_channels_decode(&message, &channels);

    uint16_t* _rgChannelvalues[cMaxRcChannels] = {
        &channels.chan1_raw,
        &channels.chan2_raw,
        &channels.chan3_raw,
        &channels.chan4_raw,
        &channels.chan5_raw,
        &channels.chan6_raw,
        &channels.chan7_raw,
        &channels.chan8_raw,
        &channels.chan9_raw,
        &channels.chan10_raw,
        &channels.chan11_raw,
        &channels.chan12_raw,
        &channels.chan13_raw,
        &channels.chan14_raw,
        &channels.chan15_raw,
        &channels.chan16_raw,
        &channels.chan17_raw,
        &channels.chan18_raw,
    };
    int pwmValues[cMaxRcChannels];

    for (int i=0; i<cMaxRcChannels; i++) {
        uint16_t channelValue = *_rgChannelvalues[i];

        if (i < channels.chancount) {
            pwmValues[i] = channelValue == UINT16_MAX ? -1 : channelValue;
        } else {
            pwmValues[i] = -1;
        }
    }

    emit remoteControlRSSIChanged(channels.rssi);
    emit rcChannelsChanged(channels.chancount, pwmValues);
}

void Vehicle::_handleRCChannelsRaw(mavlink_message_t& message)
{
    // We handle both RC_CHANNLES and RC_CHANNELS_RAW since different firmware will only
    // send one or the other.

    mavlink_rc_channels_raw_t channels;

    mavlink_msg_rc_channels_raw_decode(&message, &channels);

    uint16_t* _rgChannelvalues[cMaxRcChannels] = {
        &channels.chan1_raw,
        &channels.chan2_raw,
        &channels.chan3_raw,
        &channels.chan4_raw,
        &channels.chan5_raw,
        &channels.chan6_raw,
        &channels.chan7_raw,
        &channels.chan8_raw,
    };

    int pwmValues[cMaxRcChannels];
    int channelCount = 0;

Don Gagne's avatar
Don Gagne committed
1239 1240 1241 1242
    for (int i=0; i<cMaxRcChannels; i++) {
        pwmValues[i] = -1;
    }

Don Gagne's avatar
Don Gagne committed
1243 1244 1245 1246 1247 1248
    for (int i=0; i<8; i++) {
        uint16_t channelValue = *_rgChannelvalues[i];

        if (channelValue == UINT16_MAX) {
            pwmValues[i] = -1;
        } else {
Don Gagne's avatar
Don Gagne committed
1249
            channelCount = i + 1;
Don Gagne's avatar
Don Gagne committed
1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260
            pwmValues[i] = channelValue;
        }
    }
    for (int i=9; i<18; i++) {
        pwmValues[i] = -1;
    }

    emit remoteControlRSSIChanged(channels.rssi);
    emit rcChannelsChanged(channelCount, pwmValues);
}

1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278
void Vehicle::_handleScaledPressure(mavlink_message_t& message) {
    mavlink_scaled_pressure_t pressure;
    mavlink_msg_scaled_pressure_decode(&message, &pressure);
    _temperatureFactGroup.temperature1()->setRawValue(pressure.temperature / 100.0);
}

void Vehicle::_handleScaledPressure2(mavlink_message_t& message) {
    mavlink_scaled_pressure2_t pressure;
    mavlink_msg_scaled_pressure2_decode(&message, &pressure);
    _temperatureFactGroup.temperature2()->setRawValue(pressure.temperature / 100.0);
}

void Vehicle::_handleScaledPressure3(mavlink_message_t& message) {
    mavlink_scaled_pressure3_t pressure;
    mavlink_msg_scaled_pressure3_decode(&message, &pressure);
    _temperatureFactGroup.temperature3()->setRawValue(pressure.temperature / 100.0);
}

1279 1280
bool Vehicle::_containsLink(LinkInterface* link)
{
Don Gagne's avatar
Don Gagne committed
1281
    return _links.contains(link);
1282 1283 1284 1285 1286 1287
}

void Vehicle::_addLink(LinkInterface* link)
{
    if (!_containsLink(link)) {
        qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
1288 1289
        _links += link;
        _updatePriorityLink();
1290 1291
        connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
        connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
1292 1293 1294
    }
}

Don Gagne's avatar
Don Gagne committed
1295
void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
1296
{
Don Gagne's avatar
Don Gagne committed
1297
    qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
dogmaphobic's avatar
dogmaphobic committed
1298

Don Gagne's avatar
Don Gagne committed
1299
    _links.removeOne(link);
1300
    _updatePriorityLink();
dogmaphobic's avatar
dogmaphobic committed
1301

Don Gagne's avatar
Don Gagne committed
1302
    if (_links.count() == 0 && !_allLinksInactiveSent) {
Don Gagne's avatar
Don Gagne committed
1303
        qCDebug(VehicleLog) << "All links inactive";
Don Gagne's avatar
Don Gagne committed
1304 1305 1306
        // Make sure to not send this more than one time
        _allLinksInactiveSent = true;
        emit allLinksInactive(this);
1307 1308 1309
    }
}

1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322
bool Vehicle::sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
{
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return false;
    }

    emit _sendMessageOnLinkOnThread(link, message);

    return true;
}

void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
{
1323 1324 1325 1326 1327
    // Make sure this is still a good link
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return;
    }

1328 1329 1330 1331 1332 1333
#if 0
    // Leaving in for ease in Mav 2.0 testing
    mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(link->mavlinkChannel());
    qDebug() << "_sendMessageOnLink" << mavlinkStatus << link->mavlinkChannel() << mavlinkStatus->flags << message.magic;
#endif

1334
    // Give the plugin a chance to adjust
1335
    _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message);
Don Gagne's avatar
Don Gagne committed
1336 1337 1338 1339 1340 1341

    // Write message into buffer, prepending start sign
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int len = mavlink_msg_to_send_buffer(buffer, &message);

    link->writeBytesSafe((const char*)buffer, len);
1342 1343
    _messagesSent++;
    emit messagesSentChanged();
1344 1345
}

1346
void Vehicle::_updatePriorityLink(void)
1347
{
1348 1349
    LinkInterface* newPriorityLink = NULL;

1350
#ifndef NO_SERIAL_LINK
1351 1352 1353 1354 1355
    // Note that this routine specificallty does not clear _priorityLink when there are no links remaining.
    // By doing this we hold a reference on the last link as the Vehicle shuts down. Thus preventing shutdown
    // ordering NULL pointer crashes where priorityLink() is still called during shutdown sequence.
    for (int i=0; i<_links.count(); i++) {
        LinkInterface* link = _links[i];
1356 1357 1358
        if (link->isConnected()) {
            SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
            if (pSerialLink) {
1359 1360 1361
                LinkConfiguration* config = pSerialLink->getLinkConfiguration();
                if (config) {
                    SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(config);
1362
                    if (pSerialConfig && pSerialConfig->usbDirect()) {
1363 1364 1365 1366 1367
                        if (_priorityLink.data() != link) {
                            newPriorityLink = link;
                            break;
                        }
                        return;
1368 1369 1370
                    }
                }
            }
1371 1372
        }
    }
1373
#endif
1374 1375 1376 1377 1378 1379

    if (!newPriorityLink && !_priorityLink.data() && _links.count()) {
        newPriorityLink = _links[0];
    }

    if (newPriorityLink) {
1380
        _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
1381
    }
1382 1383
}

1384 1385
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
1386
    if (qIsInf(roll)) {
Don Gagne's avatar
Don Gagne committed
1387
        _rollFact.setRawValue(0);
1388
    } else {
Don Gagne's avatar
Don Gagne committed
1389
        _rollFact.setRawValue(roll * (180.0 / M_PI));
1390
    }
1391
    if (qIsInf(pitch)) {
Don Gagne's avatar
Don Gagne committed
1392
        _pitchFact.setRawValue(0);
1393
    } else {
Don Gagne's avatar
Don Gagne committed
1394
        _pitchFact.setRawValue(pitch * (180.0 / M_PI));
1395
    }
1396
    if (qIsInf(yaw)) {
Don Gagne's avatar
Don Gagne committed
1397