Vehicle.cc 104 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";
58
const char* Vehicle::_distanceToHomeFactName =      "distanceToHome";
Don Gagne's avatar
Don Gagne committed
59 60 61

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

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

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

175
    _mavlink = _toolbox->mavlinkProtocol();
dogmaphobic's avatar
dogmaphobic committed
176

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

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

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

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

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

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

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

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

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

210
    _mav = uas();
211

212
    // Listen for system messages
213 214
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
215 216 217
    // 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)));
218

219
    _loadSettings();
dogmaphobic's avatar
dogmaphobic committed
220

221 222 223 224 225 226
    // 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

227 228 229 230 231
    // 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
232

233
    _firmwarePlugin->initializeVehicle(this);
dogmaphobic's avatar
dogmaphobic committed
234

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

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

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

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

344 345 346
    connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceToHome);
    connect(this, &Vehicle::homePositionChanged,    this, &Vehicle::_updateDistanceToHome);

347
    _missionManager = new MissionManager(this);
348
    connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
349
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
350 351 352 353
    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);
354

355
    _parameterManager = new ParameterManager(this);
Don Gagne's avatar
Don Gagne committed
356
    connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
357

358
    // GeoFenceManager needs to access ParameterManager so make sure to create after
359
    _geoFenceManager = new GeoFenceManager(this);
360
    connect(_geoFenceManager, &GeoFenceManager::error,          this, &Vehicle::_geoFenceManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
361
    connect(_geoFenceManager, &GeoFenceManager::loadComplete,   this, &Vehicle::_geoFenceLoadComplete);
362

363
    _rallyPointManager = new RallyPointManager(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
364 365
    connect(_rallyPointManager, &RallyPointManager::error,          this, &Vehicle::_rallyPointManagerError);
    connect(_rallyPointManager, &RallyPointManager::loadComplete,   this, &Vehicle::_rallyPointLoadComplete);
366

367
    // Offline editing vehicle tracks settings changes for offline editing settings
368 369 370 371
    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);
372

373 374 375 376 377 378 379 380 381 382
    // 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);
383
    _addFact(&_flightDistanceFact,      _flightDistanceFactName);
384
    _addFact(&_flightTimeFact,          _flightTimeFactName);
385
    _addFact(&_distanceToHomeFact,      _distanceToHomeFactName);
386 387 388 389 390

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

Jacob Walser's avatar
Jacob Walser committed
393
    // Add firmware-specific fact groups, if provided
394 395 396 397 398 399 400
    QMap<QString, FactGroup*>* fwFactGroups = _firmwarePlugin->factGroups();
    if (fwFactGroups) {
        QMapIterator<QString, FactGroup*> i(*fwFactGroups);
        while(i.hasNext()) {
            i.next();
            _addFactGroup(i.value(), i.key());
        }
401 402
    }

403
    _flightDistanceFact.setRawValue(0);
404
    _flightTimeFact.setRawValue(0);
405 406
}

407 408
Vehicle::~Vehicle()
{
Don Gagne's avatar
Don Gagne committed
409 410
    qCDebug(VehicleLog) << "~Vehicle" << this;

411 412
    delete _missionManager;
    _missionManager = NULL;
413

414 415 416
    delete _autopilotPlugin;
    _autopilotPlugin = NULL;

417 418
    delete _mav;
    _mav = NULL;
dogmaphobic's avatar
dogmaphobic committed
419

420 421
}

422 423 424 425
void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
    _firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
    emit firmwareTypeChanged();
426 427 428 429 430 431
    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        _capabilityBits = 0;
    } else {
        _capabilityBits = MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
    }
    emit capabilityBitsChanged(_capabilityBits);
432 433 434 435 436 437 438 439 440 441
}

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

void Vehicle::_offlineCruiseSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
442 443
    _defaultCruiseSpeed = value.toDouble();
    emit defaultCruiseSpeedChanged(_defaultCruiseSpeed);
444 445 446 447
}

void Vehicle::_offlineHoverSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
448 449
    _defaultHoverSpeed = value.toDouble();
    emit defaultHoverSpeedChanged(_defaultHoverSpeed);
450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480
}

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()
481 482 483 484 485 486 487 488
{
    _messagesReceived   = 0;
    _messagesSent       = 0;
    _messagesLost       = 0;
    _messageSeq         = 0;
    _heardFrom          = false;
}

489 490
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
Don Gagne's avatar
Don Gagne committed
491
    if (message.sysid != _id && message.sysid != 0) {
492 493 494 495
        // 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;
        }
496
    }
497

498 499
    if (!_containsLink(link)) {
        _addLink(link);
500 501 502 503 504 505

        // 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();
        }
506
    }
dogmaphobic's avatar
dogmaphobic committed
507

508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533
    //-- 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();
        }
    }

534

535 536 537 538 539
    // Mark this vehicle as active - but only if the traffic is coming from
    // the actual vehicle
    if (message.sysid == _id) {
        _connectionActive();
    }
540

Don Gagne's avatar
Don Gagne committed
541
    // Give the plugin a change to adjust the message contents
542 543 544
    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
545

546 547 548 549 550
    // Give the Core Plugin access to all mavlink traffic
    if (!_toolbox->corePlugin()->mavlinkMessage(this, link, message)) {
        return;
    }

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

646 647 648 649 650 651 652
    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
653 654 655 656 657
    // Following are ArduPilot dialect messages

    case MAVLINK_MSG_ID_WIND:
        _handleWind(message);
        break;
658
    }
dogmaphobic's avatar
dogmaphobic committed
659

Don Gagne's avatar
Don Gagne committed
660
    emit mavlinkMessageReceived(message);
dogmaphobic's avatar
dogmaphobic committed
661

662 663 664
    _uas->receiveMessage(message);
}

665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689

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

690 691 692 693 694 695 696 697 698 699 700 701 702 703 704
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);

705 706
    _gpsRawIntMessageAvailable = true;

707
    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
708
        if (!_globalPositionIntMessageAvailable) {
709 710 711 712 713
            //-- 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);
714
            _altitudeAMSLFact.setRawValue(gpsRawInt.alt / 1000.0);
715 716 717
        }
    }

718 719
    _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7);
    _gpsFactGroup.lon()->setRawValue(gpsRawInt.lon * 1e-7);
720
    _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
721 722 723
    _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);
724 725 726 727 728 729 730 731
    _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);

732 733 734 735 736
    _altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0);
    _altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0);

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

741
    _globalPositionIntMessageAvailable = true;
742 743 744 745 746
    //-- 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);
747 748 749 750 751 752 753
}

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

754 755 756 757 758 759 760
    // 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);
        }
    }
761 762
}

DonLakeFlyer's avatar
DonLakeFlyer committed
763 764
void Vehicle::_setCapabilities(uint64_t capabilityBits)
{
765
    _capabilityBits = capabilityBits;
DonLakeFlyer's avatar
DonLakeFlyer committed
766
    _vehicleCapabilitiesKnown = true;
767
    emit capabilitiesKnownChanged(true);
768
    emit capabilityBitsChanged(_capabilityBits);
DonLakeFlyer's avatar
DonLakeFlyer committed
769

770 771 772 773 774 775 776
    // 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.");
    }

777 778 779 780 781 782 783
    QString supports("supports");
    QString doesNotSupport("does not support");

    qCDebug(VehicleLog) << QString("Vehicle %1 Mavlink 2.0").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MAVLINK2 ? supports : doesNotSupport);
    qCDebug(VehicleLog) << QString("Vehicle %1 MISSION_ITEM_INT").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_INT ? supports : doesNotSupport);
    qCDebug(VehicleLog) << QString("Vehicle %1 GeoFence").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_FENCE ? supports : doesNotSupport);
    qCDebug(VehicleLog) << QString("Vehicle %1 RallyPoints").arg(_capabilityBits & MAV_PROTOCOL_CAPABILITY_MISSION_RALLY ? supports : doesNotSupport);
DonLakeFlyer's avatar
DonLakeFlyer committed
784 785
}

Gus Grubba's avatar
Gus Grubba committed
786
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
787
{
788 789
    Q_UNUSED(link);

790 791 792 793 794 795 796 797 798 799 800
    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
801
        setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
802
    }
803

Don Gagne's avatar
Don Gagne committed
804 805 806 807 808 809 810 811
    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);

812
        // 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
813 814 815 816
        _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')));
817
        }
Don Gagne's avatar
Don Gagne committed
818 819 820
    } 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);
821
    }
Don Gagne's avatar
Don Gagne committed
822
    emit gitHashChanged(_gitHash);
823

DonLakeFlyer's avatar
DonLakeFlyer committed
824 825
    _setCapabilities(autopilotVersion.capabilities);
    _startPlanRequest();
826 827
}

828 829 830 831 832 833 834 835 836 837 838
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) {
839 840 841 842 843

    // Set only once or if we need to reduce the max version
    if (_maxProtoVersion == 0 || version < _maxProtoVersion) {
        _maxProtoVersion = version;
        emit requestProtocolVersion(_maxProtoVersion);
844 845 846 847 848

        // Now that the protocol version is known, the mission load is safe
        // as it will use the right MAVLink version to enable all features
        // the vehicle supports
        _startPlanRequest();
849
    }
850 851
}

852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875
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);
}

876 877
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
878
    bool showError = false;
879

880 881 882
    mavlink_command_ack_t ack;
    mavlink_msg_command_ack_decode(&message, &ack);

883 884 885
    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
886
        _setCapabilities(0);
887 888
    }

889 890 891
    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
892 893 894
        if (_maxProtoVersion == 0) {
            _setMaxProtoVersion(100);
        }
895 896
    }

897
    if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
Don Gagne's avatar
Don Gagne committed
898
        _mavCommandAckTimer.stop();
899 900 901 902
        showError = _mavCommandQueue[0].showError;
        _mavCommandQueue.removeFirst();
    }

Don Gagne's avatar
Don Gagne committed
903
    emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
904 905

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

908 909 910 911 912 913 914 915 916 917 918 919 920
        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;
921
        default:
922
            // Do nothing
923
            break;
924
        }
925 926
    }

927
    _sendNextQueuedMavCommand();
928 929
}

930 931
void Vehicle::_handleCommandLong(mavlink_message_t& message)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
932 933 934 935
#ifdef __ios__
    Q_UNUSED(message)
    // iOS has no serial links
#else
936 937 938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954
    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;
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
955
#endif
956 957
}

Don Gagne's avatar
Don Gagne committed
958 959 960 961 962 963 964
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:
965 966
        _setFlying(false);
        _setLanding(false);
Don Gagne's avatar
Don Gagne committed
967
        break;
968
    case MAV_LANDED_STATE_TAKEOFF:
Don Gagne's avatar
Don Gagne committed
969
    case MAV_LANDED_STATE_IN_AIR:
970 971 972 973 974 975 976 977 978
        _setFlying(true);
        _setLanding(false);
        break;
    case MAV_LANDED_STATE_LANDING:
        _setFlying(true);
        _setLanding(true);
        break;
    default:
        break;
Don Gagne's avatar
Don Gagne committed
979 980 981
    }
}

982 983 984 985 986 987 988 989 990 991 992 993 994
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);
}

995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007
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
1008 1009 1010 1011 1012 1013 1014 1015 1016 1017
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
1018 1019 1020 1021 1022 1023 1024 1025
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 {
1026
        // Current is in Amps, current_battery is 10 * milliamperes (1 = 10 milliampere)
1027
        _batteryFactGroup.current()->setRawValue((float)sysStatus.current_battery / 100.0f);
Don Gagne's avatar
Don Gagne committed
1028 1029 1030 1031 1032 1033 1034
    }
    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);
1035

1036 1037 1038 1039 1040
    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));
1041
    }
1042 1043 1044 1045 1046 1047 1048 1049 1050 1051

    _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
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 1078 1079 1080 1081 1082
}

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
1083 1084 1085 1086 1087 1088 1089 1090
void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord)
{
    if (homeCoord != _homePosition) {
        _homePosition = homeCoord;
        emit homePositionChanged(_homePosition);
    }
}

Don Gagne's avatar
Don Gagne committed
1091 1092 1093
void Vehicle::_handleHomePosition(mavlink_message_t& message)
{
    mavlink_home_position_t homePos;
dogmaphobic's avatar
dogmaphobic committed
1094

Don Gagne's avatar
Don Gagne committed
1095
    mavlink_msg_home_position_decode(&message, &homePos);
Don Gagne's avatar
Don Gagne committed
1096 1097 1098 1099

    QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
                                    homePos.longitude / 10000000.0,
                                    homePos.altitude / 1000.0);
DonLakeFlyer's avatar
DonLakeFlyer committed
1100
    _setHomePosition(newHomePosition);
Don Gagne's avatar
Don Gagne committed
1101 1102 1103 1104
}

void Vehicle::_handleHeartbeat(mavlink_message_t& message)
{
1105 1106 1107 1108
    if (message.compid != _defaultComponentId) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
1109
    mavlink_heartbeat_t heartbeat;
dogmaphobic's avatar
dogmaphobic committed
1110

Don Gagne's avatar
Don Gagne committed
1111
    mavlink_msg_heartbeat_decode(&message, &heartbeat);
dogmaphobic's avatar
dogmaphobic committed
1112

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

Don Gagne's avatar
Don Gagne committed
1115 1116 1117
    if (_armed != newArmed) {
        _armed = newArmed;
        emit armedChanged(_armed);
dogmaphobic's avatar
dogmaphobic committed
1118

1119 1120 1121
        // We are transitioning to the armed state, begin tracking trajectory points for the map
        if (_armed) {
            _mapTrajectoryStart();
1122
            _clearCameraTriggerPoints();
1123 1124 1125
        } else {
            _mapTrajectoryStop();
        }
Don Gagne's avatar
Don Gagne committed
1126 1127 1128
    }

    if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) {
1129 1130 1131 1132 1133 1134
        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
1135 1136
        _base_mode = heartbeat.base_mode;
        _custom_mode = heartbeat.custom_mode;
1137 1138 1139
        if (previousFlightMode != flightMode()) {
            emit flightModeChanged(flightMode());
        }
Don Gagne's avatar
Don Gagne committed
1140 1141 1142
    }
}

1143 1144
void Vehicle::_handleRadioStatus(mavlink_message_t& message)
{
1145

1146 1147 1148
    //-- Process telemetry status message
    mavlink_radio_status_t rstatus;
    mavlink_msg_radio_status_decode(&message, &rstatus);
1149

1150 1151
    int rssi    = rstatus.rssi;
    int remrssi = rstatus.remrssi;
1152 1153
    int lnoise = (int)(int8_t)rstatus.noise;
    int rnoise = (int)(int8_t)rstatus.remnoise;
1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167
    //-- 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);
1168 1169 1170
    } else {
        rssi    = (int)(int8_t)rstatus.rssi;
        remrssi = (int)(int8_t)rstatus.remrssi;
1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192
    }
    //-- 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);
    }
1193 1194
    if(_telemetryLNoise != lnoise) {
        _telemetryLNoise = lnoise;
1195 1196
        emit telemetryLNoiseChanged(_telemetryLNoise);
    }
1197 1198
    if(_telemetryRNoise != rnoise) {
        _telemetryRNoise = rnoise;
1199 1200 1201 1202
        emit telemetryRNoiseChanged(_telemetryRNoise);
    }
}

Don Gagne's avatar
Don Gagne committed
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 1239 1240 1241 1242 1243 1244 1245 1246 1247 1248 1249 1250 1251 1252 1253 1254 1255 1256 1257 1258 1259 1260 1261 1262 1263 1264 1265 1266 1267
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
1268 1269 1270 1271
    for (int i=0; i<cMaxRcChannels; i++) {
        pwmValues[i] = -1;
    }

Don Gagne's avatar
Don Gagne committed
1272 1273 1274 1275 1276 1277
    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
1278
            channelCount = i + 1;
Don Gagne's avatar
Don Gagne committed
1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289
            pwmValues[i] = channelValue;
        }
    }
    for (int i=9; i<18; i++) {
        pwmValues[i] = -1;
    }

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

1290 1291 1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305 1306 1307
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);
}

1308 1309
bool Vehicle::_containsLink(LinkInterface* link)
{
Don Gagne's avatar
Don Gagne committed
1310
    return _links.contains(link);
1311 1312 1313 1314 1315 1316
}

void Vehicle::_addLink(LinkInterface* link)
{
    if (!_containsLink(link)) {
        qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
1317 1318
        _links += link;
        _updatePriorityLink();
1319 1320
        connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
        connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
1321 1322 1323
    }
}

Don Gagne's avatar
Don Gagne committed
1324
void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
1325
{
Don Gagne's avatar
Don Gagne committed
1326
    qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
dogmaphobic's avatar
dogmaphobic committed
1327

Don Gagne's avatar
Don Gagne committed
1328
    _links.removeOne(link);
1329
    _updatePriorityLink();
dogmaphobic's avatar
dogmaphobic committed
1330

Don Gagne's avatar
Don Gagne committed
1331
    if (_links.count() == 0 && !_allLinksInactiveSent) {
Don Gagne's avatar
Don Gagne committed
1332
        qCDebug(VehicleLog) << "All links inactive";
Don Gagne's avatar
Don Gagne committed
1333 1334 1335
        // Make sure to not send this more than one time
        _allLinksInactiveSent = true;
        emit allLinksInactive(this);
1336 1337 1338
    }
}

1339 1340 1341 1342 1343 1344 1345 1346 1347 1348 1349 1350 1351
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)
{
1352 1353 1354 1355 1356
    // Make sure this is still a good link
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return;
    }

1357 1358 1359 1360 1361 1362
#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

1363
    // Give the plugin a chance to adjust
1364
    _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message);
Don Gagne's avatar
Don Gagne committed
1365 1366 1367 1368 1369 1370

    // 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);
1371 1372
    _messagesSent++;
    emit messagesSentChanged();
1373 1374
}

1375
void Vehicle::_updatePriorityLink(void)
1376
{
1377 1378
    LinkInterface* newPriorityLink = NULL;

1379
#ifndef NO_SERIAL_LINK
1380 1381 1382 1383 1384
    // 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];
1385 1386 1387
        if (link->isConnected()) {
            SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
            if (pSerialLink) {
1388 1389 1390
                LinkConfiguration* config = pSerialLink->getLinkConfiguration();
                if (config) {
                    SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(config);
1391
                    if (pSerialConfig && pSerialConfig->usbDirect()) {
1392 1393 1394 1395 1396
                        if (_priorityLink.data() != link) {
                            newPriorityLink = link;
                            break;
                        }
                        return;
1397 1398 1399
                    }
                }
            }
1400 1401
        }
    }
1402
#endif
1403 1404 1405 1406 1407 1408

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

    if (newPriorityLink) {
1409
        _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
1410
    }
1411 1412
}

1413 1414
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
1415
    if (qIsInf(roll)) {
Don Gagne's avatar
Don Gagne committed
1416
        _rollFact.setRawValue(0);
1417
    } else {
Don Gagne's avatar
Don Gagne committed
1418
        _rollFact.setRawValue(roll * (180.0 / M_PI));
1419
    }
1420
    if (qIsInf(pitch)) {
Don Gagne's avatar
Don Gagne committed
1421
        _pitchFact.setRawValue(0);
1422
    } else {
Don Gagne's avatar
Don Gagne committed
1423
        _pitchFact.setRawValue(pitch * (180.0 / M_PI));
1424
    }
1425
    if (qIsInf(yaw)) {
Don Gagne's avatar
Don Gagne committed
1426
        _headingFact.setRawValue(0);
1427
    } else {
Don Gagne's avatar
Don Gagne committed
1428
        yaw = yaw * (180.0 / M_PI);
1429
        if (yaw < 0) yaw += 360;
Don Gagne's avatar
Don Gagne committed
1430
        _headingFact.setRawValue(yaw);
1431 1432 1433 1434 1435 1436 1437 1438
    }
}

void Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch, double yaw, quint64 timestamp)
{
    _updateAttitude(uas, roll, pitch, yaw, timestamp);
}

Don Gagne's avatar
Don Gagne committed
1439 1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465 1466 1467 1468 1469
int Vehicle::motorCount(void)
{
    switch (_vehicleType) {
    case MAV_TYPE_HELICOPTER:
        return 1;
    case MAV_TYPE_VTOL_DUOROTOR:
        return 2;
    case MAV_TYPE_TRICOPTER:
        return 3;
    case MAV_TYPE_QUADROTOR:
    case MAV_TYPE_VTOL_QUADROTOR:
        return 4;
    case MAV_TYPE_HEXAROTOR:
        return 6;
    case MAV_TYPE_OCTOROTOR:
        return 8;
    default:
        return -1;
    }
}

bool Vehicle::coaxialMotors(void)
{
    return _firmwarePlugin->multiRotorCoaxialMotors(this);
}

bool Vehicle::xConfigMotors(void)
{
    return _firmwarePlugin->multiRotorXConfig(this);
}

1470 1471 1472 1473 1474 1475 1476 1477 1478 1479 1480 1481 1482
/*
 * Internal
 */

QString Vehicle::getMavIconColor()
{
    // TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette
    if(_mav)
        return _mav->getColor().name();
    else
        return QString("black");
}

dogmaphobic's avatar
dogmaphobic committed
1483 1484 1485
QString Vehicle::formatedMessages()
{
    QString messages;
1486
    foreach(UASMessage* message, _toolbox->uasMessageHandler()->messages()) {
dogmaphobic's avatar
dogmaphobic committed
1487 1488 1489 1490 1491
        messages += message->getFormatedText();
    }
    return messages;
}

dogmaphobic's avatar
dogmaphobic committed
1492 1493
void Vehicle::clearMessages()
{
1494
    _toolbox->uasMessageHandler()->clearMessages();
dogmaphobic's avatar
dogmaphobic committed
1495 1496
}

dogmaphobic's avatar
dogmaphobic committed
1497 1498 1499 1500 1501 1502 1503 1504 1505
void Vehicle::_handletextMessageReceived(UASMessage* message)
{
    if(message)
    {
        _formatedMessage = message->getFormatedText();
        emit formatedMessageChanged();
    }
}

1506 1507 1508 1509 1510 1511 1512 1513 1514 1515 1516 1517 1518 1519 1520
void Vehicle::_handleTextMessage(int newCount)
{
    // Reset?
    if(!newCount) {
        _currentMessageCount = 0;
        _currentNormalCount  = 0;
        _currentWarningCount = 0;
        _currentErrorCount   = 0;
        _messageCount        = 0;
        _currentMessageType  = MessageNone;
        emit newMessageCountChanged();
        emit messageTypeChanged();
        emit messageCountChanged();
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1521

1522
    UASMessageHandler* pMh = _toolbox->uasMessageHandler();
1523 1524 1525 1526 1527 1528 1529 1530 1531 1532 1533 1534 1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547 1548 1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564 1565 1566 1567 1568 1569 1570 1571 1572 1573 1574 1575 1576 1577 1578 1579 1580 1581 1582 1583 1584 1585 1586 1587
    MessageType_t type = newCount ? _currentMessageType : MessageNone;
    int errorCount     = _currentErrorCount;
    int warnCount      = _currentWarningCount;
    int normalCount    = _currentNormalCount;
    //-- Add current message counts
    errorCount  += pMh->getErrorCount();
    warnCount   += pMh->getWarningCount();
    normalCount += pMh->getNormalCount();
    //-- See if we have a higher level
    if(errorCount != _currentErrorCount) {
        _currentErrorCount = errorCount;
        type = MessageError;
    }
    if(warnCount != _currentWarningCount) {
        _currentWarningCount = warnCount;
        if(_currentMessageType != MessageError) {
            type = MessageWarning;
        }
    }
    if(normalCount != _currentNormalCount) {
        _currentNormalCount = normalCount;
        if(_currentMessageType != MessageError && _currentMessageType != MessageWarning) {
            type = MessageNormal;
        }
    }
    int count = _currentErrorCount + _currentWarningCount + _currentNormalCount;
    if(count != _currentMessageCount) {
        _currentMessageCount = count;
        // Display current total new messages count
        emit newMessageCountChanged();
    }
    if(type != _currentMessageType) {
        _currentMessageType = type;
        // Update message level
        emit messageTypeChanged();
    }
    // Update message count (all messages)
    if(newCount != _messageCount) {
        _messageCount = newCount;
        emit messageCountChanged();
    }
    QString errMsg = pMh->getLatestError();
    if(errMsg != _latestError) {
        _latestError = errMsg;
        emit latestErrorChanged();
    }
}

void Vehicle::resetMessages()
{
    // Reset Counts
    int count = _currentMessageCount;
    MessageType_t type = _currentMessageType;
    _currentErrorCount   = 0;
    _currentWarningCount = 0;
    _currentNormalCount  = 0;
    _currentMessageCount = 0;
    _currentMessageType = MessageNone;
    if(count != _currentMessageCount) {
        emit newMessageCountChanged();
    }
    if(type != _currentMessageType) {
        emit messageTypeChanged();
    }
}
1588 1589 1590 1591 1592 1593 1594 1595 1596

int Vehicle::manualControlReservedButtonCount(void)
{
    return _firmwarePlugin->manualControlReservedButtonCount();
}

void Vehicle::_loadSettings(void)
{
    QSettings settings;
dogmaphobic's avatar
dogmaphobic committed
1597

1598
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1599

1600
    bool convertOk;
dogmaphobic's avatar
dogmaphobic committed
1601

1602 1603 1604 1605
    _joystickMode = (JoystickMode_t)settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk);
    if (!convertOk) {
        _joystickMode = JoystickModeRC;
    }
dogmaphobic's avatar
dogmaphobic committed
1606

1607
    // Joystick enabled is a global setting so first make sure there are any joysticks connected
1608
    if (_toolbox->joystickManager()->joysticks().count()) {
1609
        setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool());
1610
    }
1611 1612 1613 1614 1615
}

void Vehicle::_saveSettings(void)
{
    QSettings settings;
dogmaphobic's avatar
dogmaphobic committed
1616

1617
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1618

1619
    settings.setValue(_joystickModeSettingsKey, _joystickMode);
1620 1621 1622

    // The joystick enabled setting should only be changed if a joystick is present
    // since the checkbox can only be clicked if one is present
1623
    if (_toolbox->joystickManager()->joysticks().count()) {
1624 1625
        settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled);
    }
1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637 1638
}

int Vehicle::joystickMode(void)
{
    return _joystickMode;
}

void Vehicle::setJoystickMode(int mode)
{
    if (mode < 0 || mode >= JoystickModeMax) {
        qCWarning(VehicleLog) << "Invalid joystick mode" << mode;
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
1639

1640 1641 1642 1643 1644 1645 1646 1647
    _joystickMode = (JoystickMode_t)mode;
    _saveSettings();
    emit joystickModeChanged(mode);
}

QStringList Vehicle::joystickModes(void)
{
    QStringList list;
dogmaphobic's avatar
dogmaphobic committed
1648

1649
    list << "Normal" << "Attitude" << "Position" << "Force" << "Velocity";
dogmaphobic's avatar
dogmaphobic committed
1650

1651 1652
    return list;
}
1653

Jacob Walser's avatar
Jacob Walser committed
1654 1655
void Vehicle::_activeJoystickChanged(void)
{
Gus Grubba's avatar
Gus Grubba committed
1656 1657
    _loadSettings();
    _startJoystick(true);
Jacob Walser's avatar
Jacob Walser committed
1658 1659
}

1660 1661 1662 1663 1664 1665 1666
bool Vehicle::joystickEnabled(void)
{
    return _joystickEnabled;
}

void Vehicle::setJoystickEnabled(bool enabled)
{
1667
    _joystickEnabled = enabled;
1668 1669
    _startJoystick(_joystickEnabled);
    _saveSettings();
1670
    emit joystickEnabledChanged(_joystickEnabled);
1671 1672 1673 1674
}

void Vehicle::_startJoystick(bool start)
{
1675
    Joystick* joystick = _joystickManager->activeJoystick();
1676 1677 1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691 1692 1693
    if (joystick) {
        if (start) {
            if (_joystickEnabled) {
                joystick->startPolling(this);
            }
        } else {
            joystick->stopPolling();
        }
    }
}

bool Vehicle::active(void)
{
    return _active;
}

void Vehicle::setActive(bool active)
{
1694 1695 1696 1697
    if (_active != active) {
        _active = active;
        emit activeChanged(_active);
    }
dogmaphobic's avatar
dogmaphobic committed
1698

1699 1700
    _startJoystick(_active);
}
1701

1702 1703 1704 1705
QGeoCoordinate Vehicle::homePosition(void)
{
    return _homePosition;
}
Don Gagne's avatar
Don Gagne committed
1706 1707 1708 1709

void Vehicle::setArmed(bool armed)
{
    // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks.
1710
    sendMavCommand(_defaultComponentId,
1711 1712 1713
                   MAV_CMD_COMPONENT_ARM_DISARM,
                   true,    // show error if fails
                   armed ? 1.0f : 0.0f);
Don Gagne's avatar
Don Gagne committed
1714 1715 1716 1717
}

bool Vehicle::flightModeSetAvailable(void)
{
1718
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
Don Gagne's avatar
Don Gagne committed
1719 1720 1721 1722
}

QStringList Vehicle::flightModes(void)
{
Daniel Agar's avatar
Daniel Agar committed
1723
    return _firmwarePlugin->flightModes(this);
Don Gagne's avatar
Don Gagne committed
1724 1725
}

Don Gagne's avatar
Don Gagne committed
1726
QString Vehicle::flightMode(void) const
Don Gagne's avatar
Don Gagne committed
1727 1728 1729 1730 1731 1732 1733 1734 1735
{
    return _firmwarePlugin->flightMode(_base_mode, _custom_mode);
}

void Vehicle::setFlightMode(const QString& flightMode)
{
    uint8_t     base_mode;
    uint32_t    custom_mode;

1736 1737
    qDebug() << flightMode;

Don Gagne's avatar
Don Gagne committed
1738 1739 1740 1741 1742 1743 1744
    if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) {
        // setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing
        // states.
        uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE;
        newBaseMode |= base_mode;

        mavlink_message_t msg;
1745 1746 1747 1748 1749 1750 1751 1752
        mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
                                       _mavlink->getComponentId(),
                                       priorityLink()->mavlinkChannel(),
                                       &msg,
                                       id(),
                                       newBaseMode,
                                       custom_mode);
        sendMessageOnLink(priorityLink(), msg);
Don Gagne's avatar
Don Gagne committed
1753
    } else {
Don Gagne's avatar
Don Gagne committed
1754
        qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;
Don Gagne's avatar
Don Gagne committed
1755 1756 1757 1758 1759 1760 1761 1762 1763 1764 1765 1766 1767 1768 1769 1770 1771
    }
}

bool Vehicle::hilMode(void)
{
    return _base_mode & MAV_MODE_FLAG_HIL_ENABLED;
}

void Vehicle::setHilMode(bool hilMode)
{
    mavlink_message_t msg;

    uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_HIL;
    if (hilMode) {
        newBaseMode |= MAV_MODE_FLAG_HIL_ENABLED;
    }

1772 1773 1774 1775 1776 1777 1778 1779
    mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
                                   _mavlink->getComponentId(),
                                   priorityLink()->mavlinkChannel(),
                                   &msg,
                                   id(),
                                   newBaseMode,
                                   _custom_mode);
    sendMessageOnLink(priorityLink(), msg);
Don Gagne's avatar
Don Gagne committed
1780
}
1781

1782
void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
1783 1784 1785
{
    mavlink_message_t               msg;
    mavlink_request_data_stream_t   dataStream;
dogmaphobic's avatar
dogmaphobic committed
1786

Don Gagne's avatar
Don Gagne committed
1787 1788
    memset(&dataStream, 0, sizeof(dataStream));

1789 1790 1791 1792
    dataStream.req_stream_id = stream;
    dataStream.req_message_rate = rate;
    dataStream.start_stop = 1;  // start
    dataStream.target_system = id();
1793
    dataStream.target_component = _defaultComponentId;
dogmaphobic's avatar
dogmaphobic committed
1794

1795 1796 1797 1798 1799
    mavlink_msg_request_data_stream_encode_chan(_mavlink->getSystemId(),
                                                _mavlink->getComponentId(),
                                                priorityLink()->mavlinkChannel(),
                                                &msg,
                                                &dataStream);
1800

1801 1802 1803 1804
    if (sendMultiple) {
        // We use sendMessageMultiple since we really want these to make it to the vehicle
        sendMessageMultiple(msg);
    } else {
1805
        sendMessageOnLink(priorityLink(), msg);
1806
    }
1807 1808 1809 1810 1811 1812
}

void Vehicle::_sendMessageMultipleNext(void)
{
    if (_nextSendMessageMultipleIndex < _sendMessageMultipleList.count()) {
        qCDebug(VehicleLog) << "_sendMessageMultipleNext:" << _sendMessageMultipleList[_nextSendMessageMultipleIndex].message.msgid;
dogmaphobic's avatar
dogmaphobic committed
1813

1814
        sendMessageOnLink(priorityLink(), _sendMessageMultipleList[_nextSendMessageMultipleIndex].message);
dogmaphobic's avatar
dogmaphobic committed
1815

1816 1817 1818 1819 1820 1821
        if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) {
            _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex);
        } else {
            _nextSendMessageMultipleIndex++;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
1822

1823 1824 1825 1826 1827 1828 1829 1830
    if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) {
        _nextSendMessageMultipleIndex = 0;
    }
}

void Vehicle::sendMessageMultiple(mavlink_message_t message)
{
    SendMessageMultipleInfo_t   info;
dogmaphobic's avatar
dogmaphobic committed
1831

1832 1833
    info.message =      message;
    info.retryCount =   _sendMessageMultipleRetries;
dogmaphobic's avatar
dogmaphobic committed
1834

1835 1836
    _sendMessageMultipleList.append(info);
}
1837 1838 1839 1840

void Vehicle::_missionManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
1841
    qgcApp()->showMessage(QString("Error during Mission communication with Vehicle: %1").arg(errorMsg));
1842
}
1843

1844 1845 1846
void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
1847 1848 1849 1850 1851 1852 1853
    qgcApp()->showMessage(QString("Error during GeoFence communication with Vehicle: %1").arg(errorMsg));
}

void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
    qgcApp()->showMessage(QString("Error during Rally Point communication with Vehicle: %1").arg(errorMsg));
1854 1855
}

1856 1857 1858
void Vehicle::_addNewMapTrajectoryPoint(void)
{
    if (_mapTrajectoryHaveFirstCoordinate) {
1859 1860
        // Keep three minutes of trajectory on mobile due to perf impact of lines
#ifdef __mobile__
1861 1862 1863
        if (_mapTrajectoryList.count() * _mapTrajectoryMsecsBetweenPoints > 3 * 1000 * 60) {
            _mapTrajectoryList.removeAt(0)->deleteLater();
        }
1864
#endif
1865
        _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this));
1866
        _flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + _mapTrajectoryLastCoordinate.distanceTo(_coordinate));
1867 1868
    }
    _mapTrajectoryHaveFirstCoordinate = true;
1869
    _mapTrajectoryLastCoordinate = _coordinate;
1870
    _flightTimeFact.setRawValue((double)_flightTimer.elapsed() / 1000.0);
1871 1872
}

1873 1874 1875 1876 1877 1878 1879 1880
void Vehicle::_clearTrajectoryPoints(void)
{
    _mapTrajectoryList.clearAndDeleteContents();
}

void Vehicle::_clearCameraTriggerPoints(void)
{
    _cameraTriggerPoints.clearAndDeleteContents();
1881 1882 1883 1884 1885
}

void Vehicle::_mapTrajectoryStart(void)
{
    _mapTrajectoryHaveFirstCoordinate = false;
1886
    _clearTrajectoryPoints();
1887
    _mapTrajectoryTimer.start();
1888
    _flightTimer.start();
1889
    _flightDistanceFact.setRawValue(0);
1890
    _flightTimeFact.setRawValue(0);
1891 1892 1893 1894 1895 1896
}

void Vehicle::_mapTrajectoryStop()
{
    _mapTrajectoryTimer.stop();
}
1897

DonLakeFlyer's avatar
DonLakeFlyer committed
1898
void Vehicle::_startPlanRequest(void)
1899
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1900 1901 1902 1903 1904
    if (_missionManagerInitialRequestSent) {
        return;
    }

    if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1905
        qCDebug(VehicleLog) << "_startPlanRequest";
1906
        _missionManagerInitialRequestSent = true;
1907 1908 1909 1910
        if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) {
            QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionSavePath();
            if (!missionAutoLoadDirPath.isEmpty()) {
                QDir missionAutoLoadDir(missionAutoLoadDirPath);
1911 1912
                QString autoloadFilename = missionAutoLoadDir.absoluteFilePath(tr("AutoLoad%1.%2").arg(_id).arg(AppSettings::planFileExtension));
                if (QFile(autoloadFilename).exists()) {
1913
                    _initialPlanRequestComplete = true; // We aren't going to load from the vehicle, so we are done
1914
                    PlanMasterController::sendPlanToVehicle(this, autoloadFilename);
1915 1916
                    return;
                }
Don Gagne's avatar
Don Gagne committed
1917 1918
            }
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
1919
        _missionManager->loadFromVehicle();
1920 1921
    } else {
        if (!_parameterManager->parametersReady()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1922
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready";
1923
        } else if (!_vehicleCapabilitiesKnown) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1924
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known";
1925 1926 1927 1928
        }
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
1929 1930 1931 1932 1933
void Vehicle::_missionLoadComplete(void)
{
    // After the initial mission request completes we ask for the geofence
    if (!_geoFenceManagerInitialRequestSent) {
        _geoFenceManagerInitialRequestSent = true;
1934 1935 1936 1937 1938 1939 1940
        if (_geoFenceManager->supported()) {
            qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence";
            _geoFenceManager->loadFromVehicle();
        } else {
            qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping";
            _geoFenceLoadComplete();
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
1941 1942 1943 1944 1945 1946 1947 1948
    }
}

void Vehicle::_geoFenceLoadComplete(void)
{
    // After geofence request completes we ask for the rally points
    if (!_rallyPointManagerInitialRequestSent) {
        _rallyPointManagerInitialRequestSent = true;
1949 1950 1951 1952 1953 1954 1955
        if (_rallyPointManager->supported()) {
            qCDebug(VehicleLog) << "_missionLoadComplete requesting Rally Points";
            _rallyPointManager->loadFromVehicle();
        } else {
            qCDebug(VehicleLog) << "_missionLoadComplete Rally Points not supported skipping";
            _rallyPointLoadComplete();
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
1956 1957 1958 1959 1960
    }
}

void Vehicle::_rallyPointLoadComplete(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
1961
    qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
1962 1963 1964 1965
    if (!_initialPlanRequestComplete) {
        _initialPlanRequestComplete = true;
        emit initialPlanRequestCompleted();
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
1966 1967
}

1968 1969 1970
void Vehicle::_parametersReady(bool parametersReady)
{
    if (parametersReady) {
1971
        _setupAutoDisarmSignalling();
DonLakeFlyer's avatar
DonLakeFlyer committed
1972
        _startPlanRequest();
Lorenz Meier's avatar
Lorenz Meier committed
1973 1974
        setJoystickEnabled(_joystickEnabled);
    }
1975
}
1976

Don Gagne's avatar
Don Gagne committed
1977
void Vehicle::disconnectInactiveVehicle(void)
1978
{
Don Gagne's avatar
Don Gagne committed
1979
    // Vehicle is no longer communicating with us, disconnect all links
1980

1981

1982
    LinkManager* linkMgr = _toolbox->linkManager();
1983
    for (int i=0; i<_links.count(); i++) {
1984 1985
        // FIXME: This linkInUse check is a hack fix for multiple vehicles on the same link.
        // The real fix requires significant restructuring which will come later.
1986
        if (!_toolbox->multiVehicleManager()->linkInUse(_links[i], this)) {
1987 1988
            linkMgr->disconnectLink(_links[i]);
        }
1989 1990
    }
}
1991

dogmaphobic's avatar
dogmaphobic committed
1992 1993 1994 1995 1996
void Vehicle::_imageReady(UASInterface*)
{
    if(_uas)
    {
        QImage img = _uas->getImage();
1997
        _toolbox->imageProvider()->setImage(&img, _id);
dogmaphobic's avatar
dogmaphobic committed
1998 1999 2000 2001
        _flowImageIndex++;
        emit flowImageIndexChanged();
    }
}
Don Gagne's avatar
Don Gagne committed
2002 2003 2004

void Vehicle::_remoteControlRSSIChanged(uint8_t rssi)
{
2005 2006 2007 2008
    if (_rcRSSIstore < 0 || _rcRSSIstore > 100) {
        _rcRSSIstore = rssi;
    }

Don Gagne's avatar
Don Gagne committed
2009 2010 2011 2012 2013 2014 2015 2016 2017 2018 2019
    // Low pass to git rid of jitter
    _rcRSSIstore = (_rcRSSIstore * 0.9f) + ((float)rssi * 0.1);
    uint8_t filteredRSSI = (uint8_t)ceil(_rcRSSIstore);
    if(_rcRSSIstore < 0.1) {
        filteredRSSI = 0;
    }
    if(_rcRSSI != filteredRSSI) {
        _rcRSSI = filteredRSSI;
        emit rcRSSIChanged(_rcRSSI);
    }
}
Don Gagne's avatar
Don Gagne committed
2020 2021 2022

void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust)
{
2023 2024 2025 2026
    // The following if statement prevents the virtualTabletJoystick from sending values if the standard joystick is enabled
    if ( !_joystickEnabled ) {
        _uas->setExternalControlSetpoint(roll, pitch, yaw, thrust, 0, JoystickModeRC);
    }
Don Gagne's avatar
Don Gagne committed
2027
}
2028 2029 2030 2031 2032 2033 2034 2035 2036 2037 2038 2039 2040

void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled)
{
    if (_connectionLostEnabled != connectionLostEnabled) {
        _connectionLostEnabled = connectionLostEnabled;
        emit connectionLostEnabledChanged(_connectionLostEnabled);
    }
}

void Vehicle::_connectionLostTimeout(void)
{
    if (_connectionLostEnabled && !_connectionLost) {
        _connectionLost = true;
2041
        _heardFrom = false;
2042
        _maxProtoVersion = 0;
2043
        emit connectionLostChanged(true);
2044
        _say(QString("%1 communication lost").arg(_vehicleIdSpeech()));
Don Gagne's avatar
Don Gagne committed
2045
        if (_autoDisconnect) {
2046 2047 2048 2049 2050

            // Reset link state
            for (int i = 0; i < _links.length(); i++) {
                _mavlink->resetMetadataForLink(_links.at(i));
            }
Don Gagne's avatar
Don Gagne committed
2051 2052
            disconnectInactiveVehicle();
        }
2053 2054 2055 2056 2057 2058 2059 2060 2061
    }
}

void Vehicle::_connectionActive(void)
{
    _connectionLostTimer.start();
    if (_connectionLost) {
        _connectionLost = false;
        emit connectionLostChanged(false);
Don Gagne's avatar
Don Gagne committed
2062
        _say(QString("%1 communication regained").arg(_vehicleIdSpeech()));
2063 2064 2065 2066 2067 2068

        // Re-negotiate protocol version for the link
        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
2069 2070 2071
    }
}

2072
void Vehicle::_say(const QString& text)
2073
{
2074
    _toolbox->audioOutput()->say(text.toLower());
2075
}
2076 2077 2078

bool Vehicle::fixedWing(void) const
{
2079
    return QGCMAVLink::isFixedWing(vehicleType());
2080 2081
}

Don Gagne's avatar
Don Gagne committed
2082 2083
bool Vehicle::rover(void) const
{
2084
    return QGCMAVLink::isRover(vehicleType());
Don Gagne's avatar
Don Gagne committed
2085 2086
}

2087 2088
bool Vehicle::sub(void) const
{
2089
    return QGCMAVLink::isSub(vehicleType());
2090 2091
}

2092 2093
bool Vehicle::multiRotor(void) const
{
2094
    return QGCMAVLink::isMultiRotor(vehicleType());
2095
}
Don Gagne's avatar
Don Gagne committed
2096

Don Gagne's avatar
Don Gagne committed
2097 2098 2099 2100 2101 2102 2103 2104 2105 2106 2107 2108 2109 2110 2111 2112
bool Vehicle::vtol(void) const
{
    switch (vehicleType()) {
    case MAV_TYPE_VTOL_DUOROTOR:
    case MAV_TYPE_VTOL_QUADROTOR:
    case MAV_TYPE_VTOL_TILTROTOR:
    case MAV_TYPE_VTOL_RESERVED2:
    case MAV_TYPE_VTOL_RESERVED3:
    case MAV_TYPE_VTOL_RESERVED4:
    case MAV_TYPE_VTOL_RESERVED5:
        return true;
    default:
        return false;
    }
}

2113 2114
bool Vehicle::supportsManualControl(void) const
{
2115
    return _firmwarePlugin->supportsManualControl();
2116 2117
}

2118 2119 2120 2121 2122
bool Vehicle::supportsThrottleModeCenterZero(void) const
{
    return _firmwarePlugin->supportsThrottleModeCenterZero();
}

2123 2124 2125 2126 2127
bool Vehicle::supportsRadio(void) const
{
    return _firmwarePlugin->supportsRadio();
}

2128 2129 2130 2131 2132
bool Vehicle::supportsJSButton(void) const
{
    return _firmwarePlugin->supportsJSButton();
}

2133 2134 2135 2136 2137
bool Vehicle::supportsMotorInterference(void) const
{
    return _firmwarePlugin->supportsMotorInterference();
}

2138 2139 2140 2141 2142 2143 2144 2145 2146 2147 2148 2149 2150 2151 2152 2153 2154 2155 2156 2157 2158 2159 2160 2161 2162 2163 2164 2165 2166 2167 2168 2169 2170 2171
QString Vehicle::vehicleTypeName() const {
    static QMap<int, QString> typeNames = {
        { MAV_TYPE_GENERIC,         tr("Generic micro air vehicle" )},
        { MAV_TYPE_FIXED_WING,      tr("Fixed wing aircraft")},
        { MAV_TYPE_QUADROTOR,       tr("Quadrotor")},
        { MAV_TYPE_COAXIAL,         tr("Coaxial helicopter")},
        { MAV_TYPE_HELICOPTER,      tr("Normal helicopter with tail rotor.")},
        { MAV_TYPE_ANTENNA_TRACKER, tr("Ground installation")},
        { MAV_TYPE_GCS,             tr("Operator control unit / ground control station")},
        { MAV_TYPE_AIRSHIP,         tr("Airship, controlled")},
        { MAV_TYPE_FREE_BALLOON,    tr("Free balloon, uncontrolled")},
        { MAV_TYPE_ROCKET,          tr("Rocket")},
        { MAV_TYPE_GROUND_ROVER,    tr("Ground rover")},
        { MAV_TYPE_SURFACE_BOAT,    tr("Surface vessel, boat, ship")},
        { MAV_TYPE_SUBMARINE,       tr("Submarine")},
        { MAV_TYPE_HEXAROTOR,       tr("Hexarotor")},
        { MAV_TYPE_OCTOROTOR,       tr("Octorotor")},
        { MAV_TYPE_TRICOPTER,       tr("Octorotor")},
        { MAV_TYPE_FLAPPING_WING,   tr("Flapping wing")},
        { MAV_TYPE_KITE,            tr("Flapping wing")},
        { MAV_TYPE_ONBOARD_CONTROLLER, tr("Onboard companion controller")},
        { MAV_TYPE_VTOL_DUOROTOR,   tr("Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter")},
        { MAV_TYPE_VTOL_QUADROTOR,  tr("Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter")},
        { MAV_TYPE_VTOL_TILTROTOR,  tr("Tiltrotor VTOL")},
        { MAV_TYPE_VTOL_RESERVED2,  tr("VTOL reserved 2")},
        { MAV_TYPE_VTOL_RESERVED3,  tr("VTOL reserved 3")},
        { MAV_TYPE_VTOL_RESERVED4,  tr("VTOL reserved 4")},
        { MAV_TYPE_VTOL_RESERVED5,  tr("VTOL reserved 5")},
        { MAV_TYPE_GIMBAL,          tr("Onboard gimbal")},
        { MAV_TYPE_ADSB,            tr("Onboard ADSB peripheral")},
    };
    return typeNames[_vehicleType];
}

2172 2173 2174
/// Returns the string to speak to identify the vehicle
QString Vehicle::_vehicleIdSpeech(void)
{
2175
    if (_toolbox->multiVehicleManager()->vehicles()->count() > 1) {
2176 2177
        return QString("vehicle %1").arg(id());
    } else {
2178
        return QString();
2179 2180 2181
    }
}

Don Gagne's avatar
Don Gagne committed
2182
void Vehicle::_handleFlightModeChanged(const QString& flightMode)
2183
{
2184
    _say(QString("%1 %2 flight mode").arg(_vehicleIdSpeech()).arg(flightMode));
Don Gagne's avatar
Don Gagne committed
2185
    emit guidedModeChanged(_firmwarePlugin->isGuidedMode(this));
2186 2187 2188 2189 2190
}

void Vehicle::_announceArmedChanged(bool armed)
{
    _say(QString("%1 %2").arg(_vehicleIdSpeech()).arg(armed ? QStringLiteral("armed") : QStringLiteral("disarmed")));
2191 2192
}

2193
void Vehicle::_setFlying(bool flying)
Don Gagne's avatar
Don Gagne committed
2194
{
2195
    if (_flying != flying) {
Don Gagne's avatar
Don Gagne committed
2196 2197 2198 2199 2200
        _flying = flying;
        emit flyingChanged(flying);
    }
}

2201 2202 2203 2204 2205 2206 2207 2208
void Vehicle::_setLanding(bool landing)
{
    if (armed() && _landing != landing) {
        _landing = landing;
        emit landingChanged(landing);
    }
}

Don Gagne's avatar
Don Gagne committed
2209 2210
bool Vehicle::guidedModeSupported(void) const
{
2211
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability);
Don Gagne's avatar
Don Gagne committed
2212 2213 2214 2215
}

bool Vehicle::pauseVehicleSupported(void) const
{
2216 2217 2218 2219 2220 2221
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::PauseVehicleCapability);
}

bool Vehicle::orbitModeSupported() const
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
Don Gagne's avatar
Don Gagne committed
2222 2223 2224 2225 2226
}

void Vehicle::guidedModeRTL(void)
{
    if (!guidedModeSupported()) {
2227
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2228 2229 2230 2231 2232 2233 2234 2235
        return;
    }
    _firmwarePlugin->guidedModeRTL(this);
}

void Vehicle::guidedModeLand(void)
{
    if (!guidedModeSupported()) {
2236
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2237 2238 2239 2240 2241
        return;
    }
    _firmwarePlugin->guidedModeLand(this);
}

2242
void Vehicle::guidedModeTakeoff(void)
Don Gagne's avatar
Don Gagne committed
2243 2244
{
    if (!guidedModeSupported()) {
2245
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2246 2247 2248
        return;
    }
    setGuidedMode(true);
2249 2250 2251 2252 2253 2254
    _firmwarePlugin->guidedModeTakeoff(this);
}

void Vehicle::startMission(void)
{
    _firmwarePlugin->startMission(this);
Don Gagne's avatar
Don Gagne committed
2255 2256 2257 2258 2259
}

void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
{
    if (!guidedModeSupported()) {
2260
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2261 2262
        return;
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
2263 2264 2265 2266 2267 2268 2269 2270
    if (!coordinate().isValid()) {
        return;
    }
    double maxDistance = 1000.0;
    if (coordinate().distanceTo(gotoCoord) > maxDistance) {
        qgcApp()->showMessage(QString("New location is too far. Must be less than %1 %2").arg(qRound(FactMetaData::metersToAppSettingsDistanceUnits(maxDistance).toDouble())).arg(FactMetaData::appSettingsDistanceUnitsString()));
        return;
    }
Don Gagne's avatar
Don Gagne committed
2271 2272 2273
    _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
}

2274
void Vehicle::guidedModeChangeAltitude(double altitudeChange)
Don Gagne's avatar
Don Gagne committed
2275 2276
{
    if (!guidedModeSupported()) {
2277
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2278 2279
        return;
    }
2280
    _firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange);
Don Gagne's avatar
Don Gagne committed
2281 2282
}

2283 2284
void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
{
2285 2286
    if (!orbitModeSupported()) {
        qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
2287 2288 2289 2290 2291
        return;
    }
    _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude);
}

Don Gagne's avatar
Don Gagne committed
2292 2293 2294 2295 2296 2297 2298 2299 2300
void Vehicle::pauseVehicle(void)
{
    if (!pauseVehicleSupported()) {
        qgcApp()->showMessage(QStringLiteral("Pause not supported by vehicle."));
        return;
    }
    _firmwarePlugin->pauseVehicle(this);
}

2301
void Vehicle::abortLanding(double climbOutAltitude)
2302 2303 2304 2305
{
    sendMavCommand(defaultComponentId(),
                   MAV_CMD_DO_GO_AROUND,
                   true,        // show error if fails
2306
                   climbOutAltitude);
2307 2308
}

Don Gagne's avatar
Don Gagne committed
2309 2310 2311 2312 2313 2314 2315 2316 2317 2318 2319 2320
bool Vehicle::guidedMode(void) const
{
    return _firmwarePlugin->isGuidedMode(this);
}

void Vehicle::setGuidedMode(bool guidedMode)
{
    return _firmwarePlugin->setGuidedMode(this, guidedMode);
}

void Vehicle::emergencyStop(void)
{
2321
    sendMavCommand(_defaultComponentId,
2322 2323 2324 2325
                   MAV_CMD_COMPONENT_ARM_DISARM,
                   true,        // show error if fails
                   0.0f,
                   21196.0f);  // Magic number for emergency stop
Don Gagne's avatar
Don Gagne committed
2326 2327
}

2328 2329 2330 2331 2332 2333
void Vehicle::setCurrentMissionSequence(int seq)
{
    if (!_firmwarePlugin->sendHomePositionToVehicle()) {
        seq--;
    }
    mavlink_message_t msg;
2334 2335 2336 2337 2338 2339 2340 2341
    mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(),
                                              _mavlink->getComponentId(),
                                              priorityLink()->mavlinkChannel(),
                                              &msg,
                                              id(),
                                              _compID,
                                              seq);
    sendMessageOnLink(priorityLink(), msg);
2342 2343
}

2344
void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
Don Gagne's avatar
Don Gagne committed
2345
{
2346 2347 2348 2349 2350 2351 2352 2353 2354 2355 2356 2357 2358 2359 2360 2361 2362 2363 2364 2365 2366 2367 2368
    MavCommandQueueEntry_t entry;

    entry.component = component;
    entry.command = command;
    entry.showError = showError;
    entry.rgParam[0] = param1;
    entry.rgParam[1] = param2;
    entry.rgParam[2] = param3;
    entry.rgParam[3] = param4;
    entry.rgParam[4] = param5;
    entry.rgParam[5] = param6;
    entry.rgParam[6] = param7;

    _mavCommandQueue.append(entry);

    if (_mavCommandQueue.count() == 1) {
        _mavCommandRetryCount = 0;
        _sendMavCommandAgain();
    }
}

void Vehicle::_sendMavCommandAgain(void)
{
2369 2370 2371 2372 2373 2374
    if(!_mavCommandQueue.size()) {
        qWarning() << "Command resend with no commands in queue";
        _mavCommandAckTimer.stop();
        return;
    }

2375 2376 2377
    MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];

    if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
2378 2379
        if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
            // We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
DonLakeFlyer's avatar
DonLakeFlyer committed
2380
            _setCapabilities(0);
DonLakeFlyer's avatar
DonLakeFlyer committed
2381
            _startPlanRequest();
2382 2383
        }

2384
        if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
2385 2386 2387 2388 2389
            // We aren't going to get a response back for the protocol version, so assume v1 is all we can do.
            // If the max protocol version is uninitialized, fall back to v1.
            if (_maxProtoVersion == 0) {
                _setMaxProtoVersion(100);
            }
2390 2391
        }

2392 2393
        emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
        if (queuedCommand.showError) {
2394
            qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(_toolbox->missionCommandTree()->friendlyName(queuedCommand.command)));
2395 2396 2397 2398 2399 2400
        }
        _mavCommandQueue.removeFirst();
        _sendNextQueuedMavCommand();
        return;
    }

2401
    if (_mavCommandRetryCount > 1) {
2402 2403 2404 2405 2406 2407 2408 2409 2410 2411 2412 2413 2414 2415 2416 2417 2418 2419 2420
        // We always let AUTOPILOT_CAPABILITIES go through multiple times even if we don't get acks. This is because
        // we really need to get capabilities and version info back over a lossy link.
        if (queuedCommand.command != MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
            if (px4Firmware()) {
                // Older PX4 firmwares are inconsistent with repect to sending back an Ack from a COMMAND_LONG, hence we can't support retry logic for it.
                if (_firmwareMajorVersion != versionNotSetValue) {
                    // If no version set assume lastest master dev build, so acks are suppored
                    if (_firmwareMajorVersion <= 1 && _firmwareMinorVersion <= 5 && _firmwarePatchVersion <= 3) {
                        // Acks not supported in this version
                        return;
                    }
                }
            } else {
                if (queuedCommand.command == MAV_CMD_START_RX_PAIR) {
                    // The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares
                    // we aren't really sure whether they are correct or not.
                    return;
                }
            }
2421
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
2422
        qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
2423 2424
    }

2425 2426
    _mavCommandAckTimer.start();

Don Gagne's avatar
Don Gagne committed
2427 2428 2429
    mavlink_message_t       msg;
    mavlink_command_long_t  cmd;

Don Gagne's avatar
Don Gagne committed
2430
    memset(&cmd, 0, sizeof(cmd));
2431
    cmd.command = queuedCommand.command;
Don Gagne's avatar
Don Gagne committed
2432
    cmd.confirmation = 0;
2433 2434 2435 2436 2437 2438 2439 2440 2441
    cmd.param1 = queuedCommand.rgParam[0];
    cmd.param2 = queuedCommand.rgParam[1];
    cmd.param3 = queuedCommand.rgParam[2];
    cmd.param4 = queuedCommand.rgParam[3];
    cmd.param5 = queuedCommand.rgParam[4];
    cmd.param6 = queuedCommand.rgParam[5];
    cmd.param7 = queuedCommand.rgParam[6];
    cmd.target_system = _id;
    cmd.target_component = queuedCommand.component;
2442 2443 2444 2445 2446
    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                         _mavlink->getComponentId(),
                                         priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
Don Gagne's avatar
Don Gagne committed
2447

2448
    sendMessageOnLink(priorityLink(), msg);
Don Gagne's avatar
Don Gagne committed
2449
}
2450

2451 2452 2453 2454 2455 2456 2457 2458 2459
void Vehicle::_sendNextQueuedMavCommand(void)
{
    if (_mavCommandQueue.count()) {
        _mavCommandRetryCount = 0;
        _sendMavCommandAgain();
    }
}


Don Gagne's avatar
Don Gagne committed
2460 2461 2462 2463 2464 2465 2466 2467 2468 2469 2470 2471 2472
void Vehicle::setPrearmError(const QString& prearmError)
{
    _prearmError = prearmError;
    emit prearmErrorChanged(_prearmError);
    if (!_prearmError.isEmpty()) {
        _prearmErrorTimer.start();
    }
}

void Vehicle::_prearmErrorTimeout(void)
{
    setPrearmError(QString());
}
2473

2474
void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType)
Don Gagne's avatar
Don Gagne committed
2475 2476 2477 2478
{
    _firmwareMajorVersion = majorVersion;
    _firmwareMinorVersion = minorVersion;
    _firmwarePatchVersion = patchVersion;
2479
    _firmwareVersionType = versionType;
2480 2481 2482 2483 2484 2485 2486 2487 2488
    emit firmwareVersionChanged();
}

void Vehicle::setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion)
{
    _firmwareCustomMajorVersion = majorVersion;
    _firmwareCustomMinorVersion = minorVersion;
    _firmwareCustomPatchVersion = patchVersion;
    emit firmwareCustomVersionChanged();
2489 2490 2491 2492 2493 2494 2495 2496 2497 2498 2499 2500 2501 2502 2503 2504 2505
}

QString Vehicle::firmwareVersionTypeString(void) const
{
    switch (_firmwareVersionType) {
    case FIRMWARE_VERSION_TYPE_DEV:
        return QStringLiteral("dev");
    case FIRMWARE_VERSION_TYPE_ALPHA:
        return QStringLiteral("alpha");
    case FIRMWARE_VERSION_TYPE_BETA:
        return QStringLiteral("beta");
    case FIRMWARE_VERSION_TYPE_RC:
        return QStringLiteral("rc");
    case FIRMWARE_VERSION_TYPE_OFFICIAL:
    default:
        return QStringLiteral("");
    }
Don Gagne's avatar
Don Gagne committed
2506 2507
}

2508 2509
void Vehicle::rebootVehicle()
{
2510
    sendMavCommand(_defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f);
Don Gagne's avatar
Don Gagne committed
2511 2512
}

2513 2514 2515 2516 2517 2518 2519 2520
void Vehicle::setSoloFirmware(bool soloFirmware)
{
    if (soloFirmware != _soloFirmware) {
        _soloFirmware = soloFirmware;
        emit soloFirmwareChanged(soloFirmware);
    }
}

Don Gagne's avatar
Don Gagne committed
2521 2522
#if 0
    // Temporarily removed, waiting for new command implementation
Don Gagne's avatar
Don Gagne committed
2523 2524
void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
{
2525
    doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
Don Gagne's avatar
Don Gagne committed
2526
}
Don Gagne's avatar
Don Gagne committed
2527
#endif
Don Gagne's avatar
Don Gagne committed
2528

2529
QString Vehicle::brandImageIndoor(void) const
2530
{
2531 2532 2533 2534 2535 2536
    return _firmwarePlugin->brandImageIndoor(this);
}

QString Vehicle::brandImageOutdoor(void) const
{
    return _firmwarePlugin->brandImageOutdoor(this);
2537 2538
}

2539 2540 2541 2542 2543 2544 2545 2546 2547 2548 2549 2550 2551 2552 2553 2554 2555 2556 2557 2558 2559 2560 2561 2562 2563 2564 2565 2566 2567 2568 2569 2570 2571 2572 2573 2574 2575 2576 2577 2578 2579 2580 2581 2582 2583 2584 2585
QStringList Vehicle::unhealthySensors(void) const
{
    QStringList sensorList;

    struct sensorInfo_s {
        uint32_t    bit;
        const char* sensorName;
    };

    static const sensorInfo_s rgSensorInfo[] = {
        { MAV_SYS_STATUS_SENSOR_3D_GYRO,                "Gyro" },
        { MAV_SYS_STATUS_SENSOR_3D_ACCEL,               "Accelerometer" },
        { MAV_SYS_STATUS_SENSOR_3D_MAG,                 "Magnetometer" },
        { MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE,      "Absolute pressure" },
        { MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE,  "Differential pressure" },
        { MAV_SYS_STATUS_SENSOR_GPS,                    "GPS" },
        { MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW,           "Optical flow" },
        { MAV_SYS_STATUS_SENSOR_VISION_POSITION,        "Computer vision position" },
        { MAV_SYS_STATUS_SENSOR_LASER_POSITION,         "Laser based position" },
        { MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH,  "External ground truth" },
        { MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL,   "Angular rate control" },
        { MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, "Attitude stabilization" },
        { MAV_SYS_STATUS_SENSOR_YAW_POSITION,           "Yaw position" },
        { MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL,     "Z/altitude control" },
        { MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL,    "X/Y position control" },
        { MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS,          "Motor outputs / control" },
        { MAV_SYS_STATUS_SENSOR_RC_RECEIVER,            "RC receiver" },
        { MAV_SYS_STATUS_SENSOR_3D_GYRO2,               "Gyro 2" },
        { MAV_SYS_STATUS_SENSOR_3D_ACCEL2,              "Accelerometer 2" },
        { MAV_SYS_STATUS_SENSOR_3D_MAG2,                "Magnetometer 2" },
        { MAV_SYS_STATUS_GEOFENCE,                      "GeoFence" },
        { MAV_SYS_STATUS_AHRS,                          "AHRS" },
        { MAV_SYS_STATUS_TERRAIN,                       "Terrain" },
        { MAV_SYS_STATUS_REVERSE_MOTOR,                 "Motors reversed" },
        { MAV_SYS_STATUS_LOGGING,                       "Logging" },
    };

    for (size_t i=0; i<sizeof(rgSensorInfo)/sizeof(sensorInfo_s); i++) {
        const sensorInfo_s* pSensorInfo = &rgSensorInfo[i];
        if ((_onboardControlSensorsEnabled & pSensorInfo->bit) && !(_onboardControlSensorsHealth & pSensorInfo->bit)) {
            sensorList << pSensorInfo->sensorName;
        }
    }

    return sensorList;
}

2586 2587 2588 2589 2590 2591 2592 2593
void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId)
{
    if (_offlineEditingVehicle) {
        _defaultComponentId = defaultComponentId;
    } else {
        qWarning() << "Call to Vehicle::setOfflineEditingDefaultComponentId on vehicle which is not offline";
    }
}
2594

Don Gagne's avatar
Don Gagne committed
2595 2596
void Vehicle::triggerCamera(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
2597
    sendMavCommand(_defaultComponentId,
Don Gagne's avatar
Don Gagne committed
2598 2599 2600
                   MAV_CMD_DO_DIGICAM_CONTROL,
                   true,                            // show errors
                   0.0, 0.0, 0.0, 0.0,              // param 1-4 unused
2601 2602 2603
                   1.0,                             // trigger camera
                   0.0,                             // param 6 unused
                   1.0);                            // test shot flag
Don Gagne's avatar
Don Gagne committed
2604 2605
}

2606 2607
const char* VehicleGPSFactGroup::_latFactName =                 "lat";
const char* VehicleGPSFactGroup::_lonFactName =                 "lon";
2608 2609 2610 2611 2612
const char* VehicleGPSFactGroup::_hdopFactName =                "hdop";
const char* VehicleGPSFactGroup::_vdopFactName =                "vdop";
const char* VehicleGPSFactGroup::_courseOverGroundFactName =    "courseOverGround";
const char* VehicleGPSFactGroup::_countFactName =               "count";
const char* VehicleGPSFactGroup::_lockFactName =                "lock";
Don Gagne's avatar
Don Gagne committed
2613 2614 2615

VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent)
2616 2617
    , _latFact              (0, _latFactName,               FactMetaData::valueTypeDouble)
    , _lonFact              (0, _lonFactName,               FactMetaData::valueTypeDouble)
Don Gagne's avatar
Don Gagne committed
2618 2619 2620 2621 2622 2623
    , _hdopFact             (0, _hdopFactName,              FactMetaData::valueTypeDouble)
    , _vdopFact             (0, _vdopFactName,              FactMetaData::valueTypeDouble)
    , _courseOverGroundFact (0, _courseOverGroundFactName,  FactMetaData::valueTypeDouble)
    , _countFact            (0, _countFactName,             FactMetaData::valueTypeInt32)
    , _lockFact             (0, _lockFactName,              FactMetaData::valueTypeInt32)
{
2624 2625
    _addFact(&_latFact,                 _latFactName);
    _addFact(&_lonFact,                 _lonFactName);
Don Gagne's avatar
Don Gagne committed
2626 2627 2628 2629 2630
    _addFact(&_hdopFact,                _hdopFactName);
    _addFact(&_vdopFact,                _vdopFactName);
    _addFact(&_courseOverGroundFact,    _courseOverGroundFactName);
    _addFact(&_lockFact,                _lockFactName);
    _addFact(&_countFact,               _countFactName);
2631

2632 2633
    _latFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _lonFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
2634 2635 2636
    _hdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _vdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _courseOverGroundFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
Don Gagne's avatar
Don Gagne committed
2637 2638
}

2639
void Vehicle::startMavlinkLog()
2640
{
2641
    sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_START, false /* showError */);
2642 2643
}

2644
void Vehicle::stopMavlinkLog()
2645
{
2646
    sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_STOP, false /* showError */);
2647 2648
}

2649
void Vehicle::_ackMavlinkLogData(uint16_t sequence)
2650 2651 2652
{
    mavlink_message_t msg;
    mavlink_logging_ack_t ack;
Don Gagne's avatar
Don Gagne committed
2653
    memset(&ack, 0, sizeof(ack));
2654
    ack.sequence = sequence;
2655
    ack.target_component = _defaultComponentId;
2656 2657 2658 2659 2660 2661 2662 2663 2664 2665
    ack.target_system = id();
    mavlink_msg_logging_ack_encode_chan(
        _mavlink->getSystemId(),
        _mavlink->getComponentId(),
        priorityLink()->mavlinkChannel(),
        &msg,
        &ack);
    sendMessageOnLink(priorityLink(), msg);
}

2666
void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
2667 2668 2669
{
    mavlink_logging_data_t log;
    mavlink_msg_logging_data_decode(&message, &log);
Gus Grubba's avatar
Gus Grubba committed
2670 2671
    emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
        log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
2672 2673
}

2674
void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
2675
{
2676 2677
    mavlink_logging_data_acked_t log;
    mavlink_msg_logging_data_acked_decode(&message, &log);
2678
    _ackMavlinkLogData(log.sequence);
Gus Grubba's avatar
Gus Grubba committed
2679 2680
    emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
        log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
2681 2682
}

2683 2684 2685 2686 2687 2688
void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
{
    firmwarePluginInstanceData->setParent(this);
    _firmwarePluginInstanceData = firmwarePluginInstanceData;
}

2689 2690 2691 2692 2693
QString Vehicle::missionFlightMode(void) const
{
    return _firmwarePlugin->missionFlightMode();
}

2694 2695 2696 2697 2698
QString Vehicle::pauseFlightMode(void) const
{
    return _firmwarePlugin->pauseFlightMode();
}

2699 2700 2701 2702 2703
QString Vehicle::rtlFlightMode(void) const
{
    return _firmwarePlugin->rtlFlightMode();
}

2704 2705 2706 2707 2708
QString Vehicle::landFlightMode(void) const
{
    return _firmwarePlugin->landFlightMode();
}

2709 2710 2711 2712 2713
QString Vehicle::takeControlFlightMode(void) const
{
    return _firmwarePlugin->takeControlFlightMode();
}

2714 2715 2716 2717 2718 2719 2720 2721 2722 2723 2724 2725 2726 2727 2728 2729 2730 2731 2732 2733 2734 2735 2736 2737
QString Vehicle::vehicleImageOpaque() const
{
    if(_firmwarePlugin)
        return _firmwarePlugin->vehicleImageOpaque(this);
    else
        return QString();
}

QString Vehicle::vehicleImageOutline() const
{
    if(_firmwarePlugin)
        return _firmwarePlugin->vehicleImageOutline(this);
    else
        return QString();
}

QString Vehicle::vehicleImageCompass() const
{
    if(_firmwarePlugin)
        return _firmwarePlugin->vehicleImageCompass(this);
    else
        return QString();
}

2738
const QVariantList& Vehicle::toolBarIndicators()
2739 2740 2741 2742 2743 2744 2745 2746
{
    if(_firmwarePlugin) {
        return _firmwarePlugin->toolBarIndicators(this);
    }
    static QVariantList emptyList;
    return emptyList;
}

2747 2748 2749 2750 2751 2752 2753 2754 2755
const QVariantList& Vehicle::cameraList(void) const
{
    if (_firmwarePlugin) {
        return _firmwarePlugin->cameraList(this);
    }
    static QVariantList emptyList;
    return emptyList;
}

2756 2757 2758 2759
bool Vehicle::vehicleYawsToNextWaypointInMission(void) const
{
    return _firmwarePlugin->vehicleYawsToNextWaypointInMission(this);
}
2760

2761 2762 2763 2764 2765 2766 2767 2768 2769 2770 2771 2772 2773 2774 2775 2776 2777 2778 2779 2780 2781 2782 2783
void Vehicle::_setupAutoDisarmSignalling(void)
{
    QString param = _firmwarePlugin->autoDisarmParameter(this);

    if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
        Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
        connect(fact, &Fact::rawValueChanged, this, &Vehicle::autoDisarmChanged);
        emit autoDisarmChanged();
    }
}

bool Vehicle::autoDisarm(void)
{
    QString param = _firmwarePlugin->autoDisarmParameter(this);

    if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
        Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
        return fact->rawValue().toDouble() > 0;
    }

    return false;
}

2784 2785 2786
void Vehicle::_handleADSBVehicle(const mavlink_message_t& message)
{
    mavlink_adsb_vehicle_t adsbVehicle;
Don Gagne's avatar
Don Gagne committed
2787
    static const int maxTimeSinceLastSeen = 15;
2788 2789 2790 2791

    mavlink_msg_adsb_vehicle_decode(&message, &adsbVehicle);
    if (adsbVehicle.flags | ADSB_FLAGS_VALID_COORDS) {
        if (_adsbICAOMap.contains(adsbVehicle.ICAO_address)) {
DonLakeFlyer's avatar
DonLakeFlyer committed
2792 2793 2794 2795 2796 2797
            if (adsbVehicle.tslc > maxTimeSinceLastSeen) {
                ADSBVehicle* vehicle = _adsbICAOMap[adsbVehicle.ICAO_address];
                _adsbVehicles.removeOne(vehicle);
                _adsbICAOMap.remove(adsbVehicle.ICAO_address);
                vehicle->deleteLater();
            } else {
Don Gagne's avatar
Don Gagne committed
2798
                _adsbICAOMap[adsbVehicle.ICAO_address]->update(adsbVehicle);
DonLakeFlyer's avatar
DonLakeFlyer committed
2799
            }
Don Gagne's avatar
Don Gagne committed
2800
        } else if (adsbVehicle.tslc <= maxTimeSinceLastSeen) {
2801 2802 2803 2804 2805 2806 2807
            ADSBVehicle* vehicle = new ADSBVehicle(adsbVehicle, this);
            _adsbICAOMap[adsbVehicle.ICAO_address] = vehicle;
            _adsbVehicles.append(vehicle);
        }
    }
}

2808 2809 2810 2811 2812 2813 2814 2815 2816
void Vehicle::_updateDistanceToHome(void)
{
    if (coordinate().isValid() && homePosition().isValid()) {
        _distanceToHomeFact.setRawValue(coordinate().distanceTo(homePosition()));
    } else {
        _distanceToHomeFact.setRawValue(qQNaN());
    }
}

2817 2818 2819
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

2820 2821 2822 2823 2824 2825 2826 2827 2828 2829 2830 2831 2832 2833 2834 2835
const char* VehicleBatteryFactGroup::_voltageFactName =                     "voltage";
const char* VehicleBatteryFactGroup::_percentRemainingFactName =            "percentRemaining";
const char* VehicleBatteryFactGroup::_mahConsumedFactName =                 "mahConsumed";
const char* VehicleBatteryFactGroup::_currentFactName =                     "current";
const char* VehicleBatteryFactGroup::_temperatureFactName =                 "temperature";
const char* VehicleBatteryFactGroup::_cellCountFactName =                   "cellCount";

const char* VehicleBatteryFactGroup::_settingsGroup =                       "Vehicle.battery";

const double VehicleBatteryFactGroup::_voltageUnavailable =           -1.0;
const int    VehicleBatteryFactGroup::_percentRemainingUnavailable =  -1;
const int    VehicleBatteryFactGroup::_mahConsumedUnavailable =       -1;
const int    VehicleBatteryFactGroup::_currentUnavailable =           -1;
const double VehicleBatteryFactGroup::_temperatureUnavailable =       -1.0;
const int    VehicleBatteryFactGroup::_cellCountUnavailable =         -1.0;

Don Gagne's avatar
Don Gagne committed
2836 2837
VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent)
2838 2839 2840
    , _voltageFact                  (0, _voltageFactName,                   FactMetaData::valueTypeDouble)
    , _percentRemainingFact         (0, _percentRemainingFactName,          FactMetaData::valueTypeInt32)
    , _mahConsumedFact              (0, _mahConsumedFactName,               FactMetaData::valueTypeInt32)
Don Gagne's avatar
Don Gagne committed
2841
    , _currentFact                  (0, _currentFactName,                   FactMetaData::valueTypeFloat)
2842 2843 2844
    , _temperatureFact              (0, _temperatureFactName,               FactMetaData::valueTypeDouble)
    , _cellCountFact                (0, _cellCountFactName,                 FactMetaData::valueTypeInt32)
{
2845 2846 2847 2848 2849 2850
    _addFact(&_voltageFact,                 _voltageFactName);
    _addFact(&_percentRemainingFact,        _percentRemainingFactName);
    _addFact(&_mahConsumedFact,             _mahConsumedFactName);
    _addFact(&_currentFact,                 _currentFactName);
    _addFact(&_temperatureFact,             _temperatureFactName);
    _addFact(&_cellCountFact,               _cellCountFactName);
Don Gagne's avatar
Don Gagne committed
2851 2852 2853 2854 2855 2856 2857 2858 2859 2860

    // Start out as not available
    _voltageFact.setRawValue            (_voltageUnavailable);
    _percentRemainingFact.setRawValue   (_percentRemainingUnavailable);
    _mahConsumedFact.setRawValue        (_mahConsumedUnavailable);
    _currentFact.setRawValue            (_currentUnavailable);
    _temperatureFact.setRawValue        (_temperatureUnavailable);
    _cellCountFact.setRawValue          (_cellCountUnavailable);
}

2861 2862 2863 2864
const char* VehicleWindFactGroup::_directionFactName =      "direction";
const char* VehicleWindFactGroup::_speedFactName =          "speed";
const char* VehicleWindFactGroup::_verticalSpeedFactName =  "verticalSpeed";

Don Gagne's avatar
Don Gagne committed
2865 2866 2867 2868 2869 2870 2871 2872 2873 2874 2875 2876 2877 2878 2879 2880
VehicleWindFactGroup::VehicleWindFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/WindFact.json", parent)
    , _directionFact    (0, _directionFactName,     FactMetaData::valueTypeDouble)
    , _speedFact        (0, _speedFactName,         FactMetaData::valueTypeDouble)
    , _verticalSpeedFact(0, _verticalSpeedFactName, FactMetaData::valueTypeDouble)
{
    _addFact(&_directionFact,       _directionFactName);
    _addFact(&_speedFact,           _speedFactName);
    _addFact(&_verticalSpeedFact,   _verticalSpeedFactName);

    // Start out as not available "--.--"
    _directionFact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
    _speedFact.setRawValue          (std::numeric_limits<float>::quiet_NaN());
    _verticalSpeedFact.setRawValue  (std::numeric_limits<float>::quiet_NaN());
}

2881 2882 2883 2884 2885 2886 2887
const char* VehicleVibrationFactGroup::_xAxisFactName =      "xAxis";
const char* VehicleVibrationFactGroup::_yAxisFactName =      "yAxis";
const char* VehicleVibrationFactGroup::_zAxisFactName =      "zAxis";
const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1";
const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2";
const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3";

2888 2889 2890 2891 2892 2893 2894 2895 2896 2897 2898 2899 2900 2901 2902 2903 2904 2905 2906 2907 2908
VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/VibrationFact.json", parent)
    , _xAxisFact        (0, _xAxisFactName,         FactMetaData::valueTypeDouble)
    , _yAxisFact        (0, _yAxisFactName,         FactMetaData::valueTypeDouble)
    , _zAxisFact        (0, _zAxisFactName,         FactMetaData::valueTypeDouble)
    , _clipCount1Fact   (0, _clipCount1FactName,    FactMetaData::valueTypeUint32)
    , _clipCount2Fact   (0, _clipCount2FactName,    FactMetaData::valueTypeUint32)
    , _clipCount3Fact   (0, _clipCount3FactName,    FactMetaData::valueTypeUint32)
{
    _addFact(&_xAxisFact,       _xAxisFactName);
    _addFact(&_yAxisFact,       _yAxisFactName);
    _addFact(&_zAxisFact,       _zAxisFactName);
    _addFact(&_clipCount1Fact,  _clipCount1FactName);
    _addFact(&_clipCount2Fact,  _clipCount2FactName);
    _addFact(&_clipCount3Fact,  _clipCount3FactName);

    // Start out as not available "--.--"
    _xAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _yAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _zAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}
2909 2910 2911 2912 2913 2914 2915 2916 2917 2918 2919 2920 2921 2922 2923 2924 2925 2926 2927 2928 2929


const char* VehicleTemperatureFactGroup::_temperature1FactName =      "temperature1";
const char* VehicleTemperatureFactGroup::_temperature2FactName =      "temperature2";
const char* VehicleTemperatureFactGroup::_temperature3FactName =      "temperature3";

VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/TemperatureFact.json", parent)
    , _temperature1Fact    (0, _temperature1FactName,     FactMetaData::valueTypeDouble)
    , _temperature2Fact    (0, _temperature2FactName,     FactMetaData::valueTypeDouble)
    , _temperature3Fact    (0, _temperature3FactName,     FactMetaData::valueTypeDouble)
{
    _addFact(&_temperature1Fact,       _temperature1FactName);
    _addFact(&_temperature2Fact,       _temperature2FactName);
    _addFact(&_temperature3Fact,       _temperature3FactName);

    // Start out as not available "--.--"
    _temperature1Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
    _temperature2Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
    _temperature3Fact.setRawValue      (std::numeric_limits<float>::quiet_NaN());
}