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

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

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

QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")

44 45 46 47
#define UPDATE_TIMER 50
#define DEFAULT_LAT  38.965767f
#define DEFAULT_LON -120.083923f

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

50 51 52
const char* Vehicle::_settingsGroup =               "Vehicle%1";        // %1 replaced with mavlink system id
const char* Vehicle::_joystickModeSettingsKey =     "JoystickMode";
const char* Vehicle::_joystickEnabledSettingsKey =  "JoystickEnabled";
53

Don Gagne's avatar
Don Gagne committed
54 55 56 57 58 59 60 61
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";
62
const char* Vehicle::_flightDistanceFactName =      "flightDistance";
63
const char* Vehicle::_flightTimeFactName =          "flightTime";
64
const char* Vehicle::_distanceToHomeFactName =      "distanceToHome";
65
const char* Vehicle::_hobbsFactName =               "hobbs";
Don Gagne's avatar
Don Gagne committed
66 67 68

const char* Vehicle::_gpsFactGroupName =        "gps";
const char* Vehicle::_batteryFactGroupName =    "battery";
Don Gagne's avatar
Don Gagne committed
69
const char* Vehicle::_windFactGroupName =       "wind";
70
const char* Vehicle::_vibrationFactGroupName =  "vibration";
71
const char* Vehicle::_temperatureFactGroupName = "temperature";
dheideman's avatar
dheideman committed
72
const char* Vehicle::_clockFactGroupName =      "clock";
Don Gagne's avatar
Don Gagne committed
73

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

187 188
    connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings);
    connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings);
Jacob Walser's avatar
Jacob Walser committed
189

190
    _mavlink = _toolbox->mavlinkProtocol();
dogmaphobic's avatar
dogmaphobic committed
191

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

194
    connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
Don Gagne's avatar
Don Gagne committed
195
    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
196
    connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
197

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

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

Don Gagne's avatar
Don Gagne committed
202 203
    connect(_uas, &UAS::imageReady,                     this, &Vehicle::_imageReady);
    connect(this, &Vehicle::remoteControlRSSIChanged,   this, &Vehicle::_remoteControlRSSIChanged);
dogmaphobic's avatar
dogmaphobic committed
204

205
    _commonInit();
Don Gagne's avatar
Don Gagne committed
206
    _autopilotPlugin = _firmwarePlugin->autopilotPlugin(this);
dogmaphobic's avatar
dogmaphobic committed
207

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

Don Gagne's avatar
Don Gagne committed
211 212 213 214 215
    // PreArm Error self-destruct timer
    connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
    _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
    _prearmErrorTimer.setSingleShot(true);

216 217
    // Connection Lost timer
    _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs);
218 219 220
    _connectionLostTimer.setSingleShot(false);
    _connectionLostTimer.start();
    connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout);
dogmaphobic's avatar
dogmaphobic committed
221

222 223
    // Send MAV_CMD ack timer
    _mavCommandAckTimer.setSingleShot(true);
DonLakeFlyer's avatar
DonLakeFlyer committed
224
    _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
225 226
    connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);

227
    _mav = uas();
228

229
    // Listen for system messages
230 231
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
    connect(_toolbox->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
232 233 234
    // 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)));
235

236 237 238 239 240 241 242 243 244 245 246 247 248 249
    if (_highLatencyLink) {
        // High latency links don't request information
        _setMaxProtoVersion(100);
        _setCapabilities(0);
        _initialPlanRequestComplete = true;
        _missionManagerInitialRequestSent = true;
        _geoFenceManagerInitialRequestSent = true;
        _rallyPointManagerInitialRequestSent = true;
    } else {
        // 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
dogmaphobic's avatar
dogmaphobic committed
250

251 252 253 254 255 256
        // 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
    }
257

258
    _firmwarePlugin->initializeVehicle(this);
dogmaphobic's avatar
dogmaphobic committed
259

260 261
    _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay);
    connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext);
dogmaphobic's avatar
dogmaphobic committed
262

263 264
    _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints);
    connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint);
265 266

    // Create camera manager instance
267
    _cameras = _firmwarePlugin->createCameraManager(this);
268
    emit dynamicCamerasChanged();
269 270
}

271 272 273 274 275
// Disconnected Vehicle for offline editing
Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 QObject*                   parent)
276 277
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json", parent)
    , _id(0)
278
    , _defaultComponentId(MAV_COMP_ID_ALL)
279
    , _active(false)
280
    , _offlineEditingVehicle(true)
281 282
    , _firmwareType(firmwareType)
    , _vehicleType(vehicleType)
283
    , _firmwarePlugin(NULL)
284
    , _firmwarePluginInstanceData(NULL)
285
    , _autopilotPlugin(NULL)
286 287
    , _mavlink(NULL)
    , _soloFirmware(false)
288 289
    , _toolbox(qgcApp()->toolbox())
    , _settingsManager(_toolbox->settingsManager())
290 291 292 293 294 295 296 297 298 299 300
    , _joystickMode(JoystickModeRC)
    , _joystickEnabled(false)
    , _uas(NULL)
    , _mav(NULL)
    , _currentMessageCount(0)
    , _messageCount(0)
    , _currentErrorCount(0)
    , _currentWarningCount(0)
    , _currentNormalCount(0)
    , _currentMessageType(MessageNone)
    , _updateCount(0)
301 302
    , _rcRSSI(255)
    , _rcRSSIstore(255)
303
    , _autoDisconnect(false)
304
    , _flying(false)
305
    , _landing(false)
306
    , _vtolInFwdFlight(false)
307 308 309 310
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
311 312
    , _gpsRawIntMessageAvailable(false)
    , _globalPositionIntMessageAvailable(false)
DonLakeFlyer's avatar
DonLakeFlyer committed
313 314
    , _defaultCruiseSpeed(_settingsManager->appSettings()->offlineEditingCruiseSpeed()->rawValue().toDouble())
    , _defaultHoverSpeed(_settingsManager->appSettings()->offlineEditingHoverSpeed()->rawValue().toDouble())
315
    , _vehicleCapabilitiesKnown(true)
316
    , _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
317
    , _highLatencyLink(false)
318
    , _cameras(NULL)
319 320
    , _connectionLost(false)
    , _connectionLostEnabled(true)
DonLakeFlyer's avatar
DonLakeFlyer committed
321
    , _initialPlanRequestComplete(false)
322
    , _missionManager(NULL)
323
    , _missionManagerInitialRequestSent(false)
324
    , _geoFenceManager(NULL)
325 326 327
    , _geoFenceManagerInitialRequestSent(false)
    , _rallyPointManager(NULL)
    , _rallyPointManagerInitialRequestSent(false)
328
    , _parameterManager(NULL)
329 330 331 332
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
    , _nextSendMessageMultipleIndex(0)
333
    , _firmwarePluginManager(firmwarePluginManager)
334 335 336 337 338 339 340 341 342
    , _joystickManager(NULL)
    , _flowImageIndex(0)
    , _allLinksInactiveSent(false)
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
343 344 345
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
346 347 348 349
    , _firmwareCustomMajorVersion(versionNotSetValue)
    , _firmwareCustomMinorVersion(versionNotSetValue)
    , _firmwareCustomPatchVersion(versionNotSetValue)
    , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL)
350
    , _gitHash(versionNotSetValue)
Gus Grubba's avatar
Gus Grubba committed
351
    , _uid(0)
352
    , _lastAnnouncedLowBatteryPercent(100)
353 354 355 356 357 358 359 360
    , _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)
361 362
    , _flightDistanceFact   (0, _flightDistanceFactName,    FactMetaData::valueTypeDouble)
    , _flightTimeFact       (0, _flightTimeFactName,        FactMetaData::valueTypeElapsedTimeInSeconds)
363
    , _distanceToHomeFact   (0, _distanceToHomeFactName,    FactMetaData::valueTypeDouble)
364
    , _hobbsFact            (0, _hobbsFactName,             FactMetaData::valueTypeString)
365 366 367 368
    , _gpsFactGroup(this)
    , _batteryFactGroup(this)
    , _windFactGroup(this)
    , _vibrationFactGroup(this)
dheideman's avatar
dheideman committed
369
    , _clockFactGroup(this)
370
{
371
    _commonInit();
372
    _firmwarePlugin->initializeVehicle(this);
373 374 375 376 377
}

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

379 380
    connect(_firmwarePlugin, &FirmwarePlugin::toolbarIndicatorsChanged, this, &Vehicle::toolBarIndicatorsChanged);

381 382
    connect(this, &Vehicle::coordinateChanged,      this, &Vehicle::_updateDistanceToHome);
    connect(this, &Vehicle::homePositionChanged,    this, &Vehicle::_updateDistanceToHome);
383
    connect(this, &Vehicle::hobbsMeterChanged,      this, &Vehicle::_updateHobbsMeter);
384

385
    _missionManager = new MissionManager(this);
386
    connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
387
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_missionLoadComplete);
388 389 390 391
    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);
392

393
    _parameterManager = new ParameterManager(this);
Don Gagne's avatar
Don Gagne committed
394
    connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
395

396
    // GeoFenceManager needs to access ParameterManager so make sure to create after
397
    _geoFenceManager = new GeoFenceManager(this);
398
    connect(_geoFenceManager, &GeoFenceManager::error,          this, &Vehicle::_geoFenceManagerError);
DonLakeFlyer's avatar
DonLakeFlyer committed
399
    connect(_geoFenceManager, &GeoFenceManager::loadComplete,   this, &Vehicle::_geoFenceLoadComplete);
400

401
    _rallyPointManager = new RallyPointManager(this);
DonLakeFlyer's avatar
DonLakeFlyer committed
402 403
    connect(_rallyPointManager, &RallyPointManager::error,          this, &Vehicle::_rallyPointManagerError);
    connect(_rallyPointManager, &RallyPointManager::loadComplete,   this, &Vehicle::_rallyPointLoadComplete);
404

405
    // Offline editing vehicle tracks settings changes for offline editing settings
406 407 408 409
    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);
410

411 412 413 414 415 416 417 418 419 420
    // 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);
421
    _addFact(&_flightDistanceFact,      _flightDistanceFactName);
422
    _addFact(&_flightTimeFact,          _flightTimeFactName);
423
    _addFact(&_distanceToHomeFact,      _distanceToHomeFactName);
424 425

    _hobbsFact.setRawValue(QVariant(QString("0000:00:00")));
426
    _addFact(&_hobbsFact,               _hobbsFactName);
427 428 429 430 431

    _addFactGroup(&_gpsFactGroup,       _gpsFactGroupName);
    _addFactGroup(&_batteryFactGroup,   _batteryFactGroupName);
    _addFactGroup(&_windFactGroup,      _windFactGroupName);
    _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
432
    _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
dheideman's avatar
dheideman committed
433
    _addFactGroup(&_clockFactGroup,     _clockFactGroupName);
434

Jacob Walser's avatar
Jacob Walser committed
435
    // Add firmware-specific fact groups, if provided
436 437 438 439 440 441 442
    QMap<QString, FactGroup*>* fwFactGroups = _firmwarePlugin->factGroups();
    if (fwFactGroups) {
        QMapIterator<QString, FactGroup*> i(*fwFactGroups);
        while(i.hasNext()) {
            i.next();
            _addFactGroup(i.value(), i.key());
        }
443 444
    }

445
    _flightDistanceFact.setRawValue(0);
446
    _flightTimeFact.setRawValue(0);
447 448
}

449 450
Vehicle::~Vehicle()
{
Don Gagne's avatar
Don Gagne committed
451 452
    qCDebug(VehicleLog) << "~Vehicle" << this;

453 454
    delete _missionManager;
    _missionManager = NULL;
455

456 457 458
    delete _autopilotPlugin;
    _autopilotPlugin = NULL;

459 460
    delete _mav;
    _mav = NULL;
dogmaphobic's avatar
dogmaphobic committed
461

462 463
}

464 465 466 467 468 469 470 471 472 473
void Vehicle::prepareDelete()
{
    if(_cameras) {
        delete _cameras;
        _cameras = NULL;
        emit dynamicCamerasChanged();
        qApp->processEvents();
    }
}

474 475 476 477
void Vehicle::_offlineFirmwareTypeSettingChanged(QVariant value)
{
    _firmwareType = static_cast<MAV_AUTOPILOT>(value.toInt());
    emit firmwareTypeChanged();
478 479 480 481 482 483
    if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
        _capabilityBits = 0;
    } else {
        _capabilityBits = MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
    }
    emit capabilityBitsChanged(_capabilityBits);
484 485 486 487 488 489 490 491 492 493
}

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

void Vehicle::_offlineCruiseSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
494 495
    _defaultCruiseSpeed = value.toDouble();
    emit defaultCruiseSpeedChanged(_defaultCruiseSpeed);
496 497 498 499
}

void Vehicle::_offlineHoverSpeedSettingChanged(QVariant value)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
500 501
    _defaultHoverSpeed = value.toDouble();
    emit defaultHoverSpeedChanged(_defaultHoverSpeed);
502 503 504 505 506 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
}

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()
533 534 535 536 537 538 539 540
{
    _messagesReceived   = 0;
    _messagesSent       = 0;
    _messagesLost       = 0;
    _messageSeq         = 0;
    _heardFrom          = false;
}

541 542
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
543 544 545 546 547 548 549 550
    // if the minimum supported version of MAVLink is already 2.0
    // set our max proto version to it.
    unsigned mavlinkVersion = _mavlink->getCurrentVersion();
    if (_maxProtoVersion != mavlinkVersion && mavlinkVersion >= 200) {
        _maxProtoVersion = _mavlink->getCurrentVersion();
        qCDebug(VehicleLog) << "Vehicle::_mavlinkMessageReceived setting _maxProtoVersion" << _maxProtoVersion;
    }

Don Gagne's avatar
Don Gagne committed
551
    if (message.sysid != _id && message.sysid != 0) {
552 553 554 555
        // 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;
        }
556
    }
557

558 559 560
    if (!_containsLink(link)) {
        _addLink(link);
    }
dogmaphobic's avatar
dogmaphobic committed
561

562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587
    //-- 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();
        }
    }

588

589 590 591 592 593
    // Mark this vehicle as active - but only if the traffic is coming from
    // the actual vehicle
    if (message.sysid == _id) {
        _connectionActive();
    }
594

Don Gagne's avatar
Don Gagne committed
595
    // Give the plugin a change to adjust the message contents
596 597 598
    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
599

600 601 602 603 604
    // Give the Core Plugin access to all mavlink traffic
    if (!_toolbox->corePlugin()->mavlinkMessage(this, link, message)) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
605
    switch (message.msgid) {
Don Gagne's avatar
Don Gagne committed
606 607 608 609 610 611
    case MAVLINK_MSG_ID_HOME_POSITION:
        _handleHomePosition(message);
        break;
    case MAVLINK_MSG_ID_HEARTBEAT:
        _handleHeartbeat(message);
        break;
612 613 614
    case MAVLINK_MSG_ID_RADIO_STATUS:
        _handleRadioStatus(message);
        break;
Don Gagne's avatar
Don Gagne committed
615 616 617 618 619 620
    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
621 622 623 624 625 626
    case MAVLINK_MSG_ID_BATTERY_STATUS:
        _handleBatteryStatus(message);
        break;
    case MAVLINK_MSG_ID_SYS_STATUS:
        _handleSysStatus(message);
        break;
627 628 629 630 631 632 633 634 635 636 637 638
    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;
639 640 641
    case MAVLINK_MSG_ID_VIBRATION:
        _handleVibration(message);
        break;
Don Gagne's avatar
Don Gagne committed
642 643 644
    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        _handleExtendedSysState(message);
        break;
645 646 647
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
648 649 650
    case MAVLINK_MSG_ID_COMMAND_LONG:
        _handleCommandLong(message);
        break;
651
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
Gus Grubba's avatar
Gus Grubba committed
652
        _handleAutopilotVersion(link, message);
653
        break;
654 655 656
    case MAVLINK_MSG_ID_PROTOCOL_VERSION:
        _handleProtocolVersion(link, message);
        break;
657 658 659
    case MAVLINK_MSG_ID_WIND_COV:
        _handleWindCov(message);
        break;
660 661 662
    case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
        _handleHilActuatorControls(message);
        break;
663 664 665 666 667 668
    case MAVLINK_MSG_ID_LOGGING_DATA:
        _handleMavlinkLoggingData(message);
        break;
    case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
        _handleMavlinkLoggingDataAcked(message);
        break;
669 670 671 672 673 674 675 676 677 678 679 680
    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;
681 682 683 684 685 686 687 688
    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);
689
        break;
690 691
    case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
        _handleCameraImageCaptured(message);
692
        break;
693 694 695
    case MAVLINK_MSG_ID_ADSB_VEHICLE:
        _handleADSBVehicle(message);
        break;
696 697 698
    case MAVLINK_MSG_ID_HIGH_LATENCY2:
        _handleHighLatency2(message);
        break;
Don Gagne's avatar
Don Gagne committed
699

700 701 702 703 704 705 706
    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
707

708
        // Following are ArduPilot dialect messages
709 710 711 712
#if !defined(NO_ARDUPILOT_DIALECT)
    case MAVLINK_MSG_ID_CAMERA_FEEDBACK:
        _handleCameraFeedback(message);
        break;
Don Gagne's avatar
Don Gagne committed
713 714 715
    case MAVLINK_MSG_ID_WIND:
        _handleWind(message);
        break;
716
#endif
717
    }
dogmaphobic's avatar
dogmaphobic committed
718

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

723 724 725
    _uas->receiveMessage(message);
}

726

727
#if !defined(NO_ARDUPILOT_DIALECT)
728 729 730 731 732 733 734 735 736 737
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));
}
738
#endif
739 740 741 742 743 744 745 746 747 748 749 750 751 752

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

753 754 755 756 757 758 759 760 761 762 763 764 765 766 767
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);

768 769
    _gpsRawIntMessageAvailable = true;

770
    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
771
        if (!_globalPositionIntMessageAvailable) {
772 773 774 775 776
            //-- 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);
777
            _altitudeAMSLFact.setRawValue(gpsRawInt.alt / 1000.0);
778 779 780
        }
    }

781 782
    _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7);
    _gpsFactGroup.lon()->setRawValue(gpsRawInt.lon * 1e-7);
783
    _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
784 785 786
    _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);
787 788 789 790 791 792 793 794
    _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);

795 796 797 798 799
    _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.
800
    if (globalPositionInt.lat == 0 && globalPositionInt.lon == 0) {
801 802 803
        return;
    }

804
    _globalPositionIntMessageAvailable = true;
805 806 807 808 809
    //-- 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);
810 811
}

812 813 814 815 816
void Vehicle::_handleHighLatency2(mavlink_message_t& message)
{
    mavlink_high_latency2_t highLatency2;
    mavlink_msg_high_latency2_decode(&message, &highLatency2);

817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834
    QString previousFlightMode;
    if (_base_mode != 0 || _custom_mode != 0){
        // Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about
        // bad modes while unit testing.
        previousFlightMode = flightMode();
    }
    _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
    _custom_mode = _firmwarePlugin->highLatencyCustomModeTo32Bits(highLatency2.custom_mode);
    if (previousFlightMode != flightMode()) {
        emit flightModeChanged(flightMode());
    }

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

835 836 837 838 839 840 841 842 843
    _coordinate.setLatitude(highLatency2.latitude  / (double)1E7);
    _coordinate.setLongitude(highLatency2.longitude / (double)1E7);
    _coordinate.setAltitude(highLatency2.altitude);
    emit coordinateChanged(_coordinate);

    _airSpeedFact.setRawValue((double)highLatency2.airspeed / 5.0);
    _groundSpeedFact.setRawValue((double)highLatency2.groundspeed / 5.0);
    _climbRateFact.setRawValue((double)highLatency2.climb_rate / 10.0);
    _headingFact.setRawValue((double)highLatency2.heading * 2.0);
DonLakeFlyer's avatar
DonLakeFlyer committed
844 845
    _altitudeRelativeFact.setRawValue(std::numeric_limits<double>::quiet_NaN());
    _altitudeAMSLFact.setRawValue(highLatency2.altitude);
846 847 848 849 850 851 852 853 854 855 856 857 858

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

    _batteryFactGroup.percentRemaining()->setRawValue(highLatency2.battery);

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

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

    struct failure2Sensor_s {
861
        HL_FAILURE_FLAG         failureBit;
862 863 864 865
        MAV_SYS_STATUS_SENSOR   sensorBit;
    };

    static const failure2Sensor_s rgFailure2Sensor[] = {
866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883
        { HL_FAILURE_FLAG_GPS,                      MAV_SYS_STATUS_SENSOR_GPS },
        { HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE,    MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE },
        { HL_FAILURE_FLAG_ABSOLUTE_PRESSURE,        MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE },
        { HL_FAILURE_FLAG_3D_ACCEL,                 MAV_SYS_STATUS_SENSOR_3D_ACCEL },
        { HL_FAILURE_FLAG_3D_GYRO,                  MAV_SYS_STATUS_SENSOR_3D_GYRO },
        { HL_FAILURE_FLAG_3D_MAG,                   MAV_SYS_STATUS_SENSOR_3D_MAG },
#if 0
        // FIXME: These don't currently map to existing sensor health bits. Support needs to be added to show them
        // on health page of instrument panel as well.
        { HL_FAILURE_FLAG_TERRAIN=64, /* Terrain subsystem failure. | */
        { HL_FAILURE_FLAG_BATTERY=128, /* Battery failure/critical low battery. | */
        { HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no rc connection. | */
        { HL_FAILURE_FLAG_OFFBOARD_LINK=512, /* Offboard link failure. | */
        { HL_FAILURE_FLAG_ENGINE=1024, /* Engine failure. | */
        { HL_FAILURE_FLAG_GEOFENCE=2048, /* Geofence violation. | */
        { HL_FAILURE_FLAG_ESTIMATOR=4096, /* Estimator failure, for example measurement rejection or large variances. | */
        { HL_FAILURE_FLAG_MISSION=8192, /* Mission failure. | */
#endif
884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900
    };

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

903 904 905 906 907
void Vehicle::_handleAltitude(mavlink_message_t& message)
{
    mavlink_altitude_t altitude;
    mavlink_msg_altitude_decode(&message, &altitude);

908 909 910 911 912 913 914
    // 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);
        }
    }
915 916
}

DonLakeFlyer's avatar
DonLakeFlyer committed
917 918
void Vehicle::_setCapabilities(uint64_t capabilityBits)
{
919
    _capabilityBits = capabilityBits;
DonLakeFlyer's avatar
DonLakeFlyer committed
920
    _vehicleCapabilitiesKnown = true;
921
    emit capabilitiesKnownChanged(true);
922
    emit capabilityBitsChanged(_capabilityBits);
DonLakeFlyer's avatar
DonLakeFlyer committed
923

924 925 926 927 928 929 930
    // 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.");
    }

931 932 933 934 935 936 937
    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
938 939
}

Gus Grubba's avatar
Gus Grubba committed
940
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
941
{
942 943
    Q_UNUSED(link);

944 945 946
    mavlink_autopilot_version_t autopilotVersion;
    mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);

Gus Grubba's avatar
Gus Grubba committed
947 948 949
    _uid = (quint64)autopilotVersion.uid;
    emit vehicleUIDChanged();

950 951 952 953 954 955 956 957
    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
958
        setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
959
    }
960

Don Gagne's avatar
Don Gagne committed
961 962 963 964 965 966 967 968
    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);

969
        // 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
970 971 972 973
        _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')));
974
        }
Don Gagne's avatar
Don Gagne committed
975 976 977
    } 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);
978
    }
Don Gagne's avatar
Don Gagne committed
979
    emit gitHashChanged(_gitHash);
980

DonLakeFlyer's avatar
DonLakeFlyer committed
981 982
    _setCapabilities(autopilotVersion.capabilities);
    _startPlanRequest();
983 984
}

985 986 987 988 989 990 991 992 993 994 995
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) {
996 997 998

    // Set only once or if we need to reduce the max version
    if (_maxProtoVersion == 0 || version < _maxProtoVersion) {
DonLakeFlyer's avatar
DonLakeFlyer committed
999
        qCDebug(VehicleLog) << "_setMaxProtoVersion before:after" << _maxProtoVersion << version;
1000 1001
        _maxProtoVersion = version;
        emit requestProtocolVersion(_maxProtoVersion);
1002 1003 1004 1005 1006

        // 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();
1007
    }
1008 1009
}

Gus Grubba's avatar
Gus Grubba committed
1010 1011 1012 1013 1014
QString Vehicle::vehicleUIDStr()
{
    QString uid;
    uint8_t* pUid = (uint8_t*)(void*)&_uid;
    uid.sprintf("%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
1015 1016 1017 1018 1019 1020 1021 1022
                pUid[0] & 0xff,
            pUid[1] & 0xff,
            pUid[2] & 0xff,
            pUid[3] & 0xff,
            pUid[4] & 0xff,
            pUid[5] & 0xff,
            pUid[6] & 0xff,
            pUid[7] & 0xff);
Gus Grubba's avatar
Gus Grubba committed
1023 1024 1025
    return uid;
}

1026 1027 1028 1029 1030 1031
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],
1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047
            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);
1048 1049
}

1050 1051
void Vehicle::_handleCommandLong(mavlink_message_t& message)
{
1052 1053
#ifdef NO_SERIAL_LINK
    // If not using serial link, bail out.
DonLakeFlyer's avatar
DonLakeFlyer committed
1054 1055
    Q_UNUSED(message)
#else
1056 1057 1058 1059
    mavlink_command_long_t cmd;
    mavlink_msg_command_long_decode(&message, &cmd);

    switch (cmd.command) {
1060 1061 1062 1063 1064 1065 1066 1067 1068 1069
    // 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));
1070 1071
                }
            }
1072
        }
1073 1074
        break;
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
1075
#endif
1076 1077
}

Don Gagne's avatar
Don Gagne committed
1078 1079 1080 1081 1082 1083 1084
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:
1085 1086
        _setFlying(false);
        _setLanding(false);
Don Gagne's avatar
Don Gagne committed
1087
        break;
1088
    case MAV_LANDED_STATE_TAKEOFF:
Don Gagne's avatar
Don Gagne committed
1089
    case MAV_LANDED_STATE_IN_AIR:
1090 1091 1092 1093 1094 1095 1096 1097 1098
        _setFlying(true);
        _setLanding(false);
        break;
    case MAV_LANDED_STATE_LANDING:
        _setFlying(true);
        _setLanding(true);
        break;
    default:
        break;
Don Gagne's avatar
Don Gagne committed
1099
    }
1100 1101

    if (vtol()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
1102 1103 1104 1105 1106
        bool vtolInFwdFlight = extendedState.vtol_state == MAV_VTOL_STATE_FW;
        if (vtolInFwdFlight != _vtolInFwdFlight) {
            _vtolInFwdFlight = vtolInFwdFlight;
            emit vtolInFwdFlightChanged(vtolInFwdFlight);
        }
1107
    }
Don Gagne's avatar
Don Gagne committed
1108 1109
}

1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122
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);
}

1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135
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);
}

1136
#if !defined(NO_ARDUPILOT_DIALECT)
Don Gagne's avatar
Don Gagne committed
1137 1138 1139 1140 1141 1142 1143 1144 1145
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);
}
1146
#endif
Don Gagne's avatar
Don Gagne committed
1147

Don Gagne's avatar
Don Gagne committed
1148 1149 1150 1151 1152 1153 1154 1155
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 {
1156
        // Current is in Amps, current_battery is 10 * milliamperes (1 = 10 milliampere)
1157
        _batteryFactGroup.current()->setRawValue((float)sysStatus.current_battery / 100.0f);
Don Gagne's avatar
Don Gagne committed
1158 1159 1160 1161 1162
    }
    if (sysStatus.voltage_battery == UINT16_MAX) {
        _batteryFactGroup.voltage()->setRawValue(VehicleBatteryFactGroup::_voltageUnavailable);
    } else {
        _batteryFactGroup.voltage()->setRawValue((double)sysStatus.voltage_battery / 1000.0);
1163 1164
        // current_battery is 10 mA and voltage_battery is 1mV. (10/1e3 times 1/1e3 = 1/1e5)
        _batteryFactGroup.instantPower()->setRawValue((float)(sysStatus.current_battery*sysStatus.voltage_battery)/(100000.0));
Don Gagne's avatar
Don Gagne committed
1165 1166
    }
    _batteryFactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining);
1167

1168 1169 1170
    if (sysStatus.battery_remaining > 0) {
        if (sysStatus.battery_remaining < _settingsManager->appSettings()->batteryPercentRemainingAnnounce()->rawValue().toInt() &&
                sysStatus.battery_remaining < _lastAnnouncedLowBatteryPercent) {
1171
            _say(QString(tr("%1 low battery: %2 percent remaining")).arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining));
1172
        }
1173
        _lastAnnouncedLowBatteryPercent = sysStatus.battery_remaining;
1174
    }
1175 1176 1177 1178 1179 1180 1181 1182 1183 1184

    _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
1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215
}

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
1216 1217 1218 1219 1220 1221 1222 1223
void Vehicle::_setHomePosition(QGeoCoordinate& homeCoord)
{
    if (homeCoord != _homePosition) {
        _homePosition = homeCoord;
        emit homePositionChanged(_homePosition);
    }
}

Don Gagne's avatar
Don Gagne committed
1224 1225 1226
void Vehicle::_handleHomePosition(mavlink_message_t& message)
{
    mavlink_home_position_t homePos;
dogmaphobic's avatar
dogmaphobic committed
1227

Don Gagne's avatar
Don Gagne committed
1228
    mavlink_msg_home_position_decode(&message, &homePos);
Don Gagne's avatar
Don Gagne committed
1229 1230 1231 1232

    QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
                                    homePos.longitude / 10000000.0,
                                    homePos.altitude / 1000.0);
DonLakeFlyer's avatar
DonLakeFlyer committed
1233
    _setHomePosition(newHomePosition);
Don Gagne's avatar
Don Gagne committed
1234 1235 1236 1237
}

void Vehicle::_handleHeartbeat(mavlink_message_t& message)
{
1238 1239 1240 1241
    if (message.compid != _defaultComponentId) {
        return;
    }

Don Gagne's avatar
Don Gagne committed
1242
    mavlink_heartbeat_t heartbeat;
dogmaphobic's avatar
dogmaphobic committed
1243

Don Gagne's avatar
Don Gagne committed
1244
    mavlink_msg_heartbeat_decode(&message, &heartbeat);
dogmaphobic's avatar
dogmaphobic committed
1245

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

Don Gagne's avatar
Don Gagne committed
1248 1249 1250
    if (_armed != newArmed) {
        _armed = newArmed;
        emit armedChanged(_armed);
dogmaphobic's avatar
dogmaphobic committed
1251

1252 1253 1254
        // We are transitioning to the armed state, begin tracking trajectory points for the map
        if (_armed) {
            _mapTrajectoryStart();
1255
            _clearCameraTriggerPoints();
1256 1257
        } else {
            _mapTrajectoryStop();
1258 1259 1260 1261 1262
            // Also handle Video Streaming
            if(_settingsManager->videoSettings()->disableWhenDisarmed()->rawValue().toBool()) {
                _settingsManager->videoSettings()->streamEnabled()->setRawValue(false);
                qgcApp()->toolbox()->videoManager()->videoReceiver()->stop();
            }
1263
        }
Don Gagne's avatar
Don Gagne committed
1264 1265 1266
    }

    if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) {
1267 1268 1269 1270 1271 1272
        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
1273 1274
        _base_mode = heartbeat.base_mode;
        _custom_mode = heartbeat.custom_mode;
1275 1276 1277
        if (previousFlightMode != flightMode()) {
            emit flightModeChanged(flightMode());
        }
Don Gagne's avatar
Don Gagne committed
1278 1279 1280
    }
}

1281 1282
void Vehicle::_handleRadioStatus(mavlink_message_t& message)
{
1283

1284 1285 1286
    //-- Process telemetry status message
    mavlink_radio_status_t rstatus;
    mavlink_msg_radio_status_decode(&message, &rstatus);
1287

1288 1289
    int rssi    = rstatus.rssi;
    int remrssi = rstatus.remrssi;
1290 1291
    int lnoise = (int)(int8_t)rstatus.noise;
    int rnoise = (int)(int8_t)rstatus.remnoise;
1292 1293 1294 1295 1296 1297 1298 1299 1300 1301 1302 1303 1304 1305
    //-- 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);
1306 1307 1308
    } else {
        rssi    = (int)(int8_t)rstatus.rssi;
        remrssi = (int)(int8_t)rstatus.remrssi;
1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330
    }
    //-- 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);
    }
1331 1332
    if(_telemetryLNoise != lnoise) {
        _telemetryLNoise = lnoise;
1333 1334
        emit telemetryLNoiseChanged(_telemetryLNoise);
    }
1335 1336
    if(_telemetryRNoise != rnoise) {
        _telemetryRNoise = rnoise;
1337 1338 1339 1340
        emit telemetryRNoiseChanged(_telemetryRNoise);
    }
}

Don Gagne's avatar
Don Gagne committed
1341 1342 1343 1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405
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
1406 1407 1408 1409
    for (int i=0; i<cMaxRcChannels; i++) {
        pwmValues[i] = -1;
    }

Don Gagne's avatar
Don Gagne committed
1410 1411 1412 1413 1414 1415
    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
1416
            channelCount = i + 1;
Don Gagne's avatar
Don Gagne committed
1417 1418 1419 1420 1421 1422 1423 1424 1425 1426 1427
            pwmValues[i] = channelValue;
        }
    }
    for (int i=9; i<18; i++) {
        pwmValues[i] = -1;
    }

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

1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440 1441 1442 1443 1444 1445
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);
}

1446 1447
bool Vehicle::_containsLink(LinkInterface* link)
{
Don Gagne's avatar
Don Gagne committed
1448
    return _links.contains(link);
1449 1450 1451 1452 1453 1454
}

void Vehicle::_addLink(LinkInterface* link)
{
    if (!_containsLink(link)) {
        qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
1455 1456
        _links += link;
        _updatePriorityLink();
1457
        _updateHighLatencyLink();
1458 1459
        connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
        connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
1460
        connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink);
1461 1462 1463
    }
}

Don Gagne's avatar
Don Gagne committed
1464
void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
1465
{
Don Gagne's avatar
Don Gagne committed
1466
    qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
dogmaphobic's avatar
dogmaphobic committed
1467

Don Gagne's avatar
Don Gagne committed
1468
    _links.removeOne(link);
1469
    _updatePriorityLink();
dogmaphobic's avatar
dogmaphobic committed
1470

Don Gagne's avatar
Don Gagne committed
1471
    if (_links.count() == 0 && !_allLinksInactiveSent) {
Don Gagne's avatar
Don Gagne committed
1472
        qCDebug(VehicleLog) << "All links inactive";
Don Gagne's avatar
Don Gagne committed
1473 1474 1475
        // Make sure to not send this more than one time
        _allLinksInactiveSent = true;
        emit allLinksInactive(this);
1476 1477 1478
    }
}

1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491
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)
{
1492 1493 1494 1495 1496
    // Make sure this is still a good link
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return;
    }

1497 1498 1499 1500 1501 1502
#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

1503
    // Give the plugin a chance to adjust
1504
    _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message);
Don Gagne's avatar
Don Gagne committed
1505 1506 1507 1508 1509 1510

    // 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);
1511 1512
    _messagesSent++;
    emit messagesSentChanged();
1513 1514
}

1515
void Vehicle::_updatePriorityLink(void)
1516
{
1517 1518
    LinkInterface* newPriorityLink = NULL;

1519
    // This routine specifically does not clear _priorityLink when there are no links remaining.
1520 1521
    // 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.
1522 1523 1524 1525 1526 1527 1528
    if (_links.count() == 0) {
        return;
    }

    // Check for the existing priority link to still be valid
    for (int i=0; i<_links.count(); i++) {
        if (_priorityLink.data() == _links[i]) {
1529 1530 1531 1532 1533
            if (!_priorityLink.data()->highLatency()) {
                // Link is still valid. Continue to use it unless it is high latency. In that case we still look for a better
                // link to use as priority link.
                return;
            }
1534 1535 1536 1537 1538 1539 1540 1541 1542 1543
        }
    }

    // The previous priority link is no longer valid. We must no find the best link available in this priority order:
    //      Direct USB connection
    //      Not a high latency link
    //      Any link

#ifndef NO_SERIAL_LINK
    // Search for direct usb connection
1544 1545
    for (int i=0; i<_links.count(); i++) {
        LinkInterface* link = _links[i];
1546 1547 1548 1549 1550 1551 1552 1553 1554
        SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
        if (pSerialLink) {
            LinkConfiguration* config = pSerialLink->getLinkConfiguration();
            if (config) {
                SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(config);
                if (pSerialConfig && pSerialConfig->usbDirect()) {
                    if (_priorityLink.data() != link) {
                        newPriorityLink = link;
                        break;
1555
                    }
1556
                    return;
1557 1558
                }
            }
1559 1560
        }
    }
1561
#endif
1562

1563 1564 1565 1566 1567 1568 1569 1570 1571
    if (!newPriorityLink) {
        // Search for non-high latency link
        for (int i=0; i<_links.count(); i++) {
            LinkInterface* link = _links[i];
            if (!link->highLatency()) {
                newPriorityLink = link;
                break;
            }
        }
1572 1573
    }

1574 1575 1576
    if (!newPriorityLink) {
        // Use any link
        newPriorityLink = _links[0];
1577
    }
1578 1579 1580

    _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
    _updateHighLatencyLink();
1581 1582
}

1583 1584
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
1585
    if (qIsInf(roll)) {
Don Gagne's avatar
Don Gagne committed
1586
        _rollFact.setRawValue(0);
1587
    } else {
Don Gagne's avatar
Don Gagne committed
1588
        _rollFact.setRawValue(roll * (180.0 / M_PI));
1589
    }
1590
    if (qIsInf(pitch)) {
Don Gagne's avatar
Don Gagne committed
1591
        _pitchFact.setRawValue(0);
1592
    } else {
Don Gagne's avatar
Don Gagne committed
1593
        _pitchFact.setRawValue(pitch * (180.0 / M_PI));
1594
    }
1595
    if (qIsInf(yaw)) {
Don Gagne's avatar
Don Gagne committed
1596
        _headingFact.setRawValue(0);
1597
    } else {
Don Gagne's avatar
Don Gagne committed
1598
        yaw = yaw * (180.0 / M_PI);
Mark Whitehorn's avatar
Mark Whitehorn committed
1599 1600 1601
        if (yaw < 0.0) yaw += 360.0;
        // truncate to integer so widget never displays 360
        _headingFact.setRawValue(trunc(yaw));
1602 1603 1604 1605 1606 1607 1608 1609
    }
}

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
1610 1611 1612 1613 1614 1615 1616 1617 1618 1619 1620 1621 1622 1623 1624 1625 1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637 1638 1639 1640
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);
}

1641 1642 1643 1644 1645 1646 1647 1648 1649 1650 1651 1652 1653
/*
 * 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
1654 1655 1656
QString Vehicle::formatedMessages()
{
    QString messages;
1657
    foreach(UASMessage* message, _toolbox->uasMessageHandler()->messages()) {
dogmaphobic's avatar
dogmaphobic committed
1658 1659 1660 1661 1662
        messages += message->getFormatedText();
    }
    return messages;
}

dogmaphobic's avatar
dogmaphobic committed
1663 1664
void Vehicle::clearMessages()
{
1665
    _toolbox->uasMessageHandler()->clearMessages();
dogmaphobic's avatar
dogmaphobic committed
1666 1667
}

dogmaphobic's avatar
dogmaphobic committed
1668 1669 1670 1671 1672 1673 1674 1675 1676
void Vehicle::_handletextMessageReceived(UASMessage* message)
{
    if(message)
    {
        _formatedMessage = message->getFormatedText();
        emit formatedMessageChanged();
    }
}

1677 1678 1679 1680 1681 1682 1683 1684 1685 1686 1687 1688 1689 1690 1691
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
1692

1693
    UASMessageHandler* pMh = _toolbox->uasMessageHandler();
1694 1695 1696 1697 1698 1699 1700 1701 1702 1703 1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721 1722 1723 1724 1725 1726 1727 1728 1729 1730 1731 1732 1733 1734 1735 1736 1737 1738 1739 1740 1741 1742 1743 1744 1745 1746 1747 1748 1749 1750 1751 1752 1753 1754 1755 1756 1757 1758
    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();
    }
}
1759 1760 1761 1762 1763 1764 1765 1766

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

void Vehicle::_loadSettings(void)
{
1767 1768 1769 1770
    if (!_active) {
        return;
    }

1771
    QSettings settings;
dogmaphobic's avatar
dogmaphobic committed
1772

1773
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1774

1775
    bool convertOk;
dogmaphobic's avatar
dogmaphobic committed
1776

1777 1778 1779 1780
    _joystickMode = (JoystickMode_t)settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk);
    if (!convertOk) {
        _joystickMode = JoystickModeRC;
    }
dogmaphobic's avatar
dogmaphobic committed
1781

1782
    // Joystick enabled is a global setting so first make sure there are any joysticks connected
1783
    if (_toolbox->joystickManager()->joysticks().count()) {
1784
        setJoystickEnabled(settings.value(_joystickEnabledSettingsKey, false).toBool());
1785
    }
1786 1787 1788 1789 1790
}

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

1792
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1793

1794
    settings.setValue(_joystickModeSettingsKey, _joystickMode);
1795 1796 1797

    // The joystick enabled setting should only be changed if a joystick is present
    // since the checkbox can only be clicked if one is present
1798
    if (_toolbox->joystickManager()->joysticks().count()) {
1799 1800
        settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled);
    }
1801 1802 1803 1804 1805 1806 1807 1808 1809 1810 1811 1812 1813
}

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
1814

1815 1816 1817 1818 1819 1820 1821 1822
    _joystickMode = (JoystickMode_t)mode;
    _saveSettings();
    emit joystickModeChanged(mode);
}

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

1824
    list << "Normal" << "Attitude" << "Position" << "Force" << "Velocity";
dogmaphobic's avatar
dogmaphobic committed
1825

1826 1827
    return list;
}
1828 1829 1830 1831 1832 1833 1834 1835

bool Vehicle::joystickEnabled(void)
{
    return _joystickEnabled;
}

void Vehicle::setJoystickEnabled(bool enabled)
{
1836
    _joystickEnabled = enabled;
1837 1838
    _startJoystick(_joystickEnabled);
    _saveSettings();
1839
    emit joystickEnabledChanged(_joystickEnabled);
1840 1841 1842 1843
}

void Vehicle::_startJoystick(bool start)
{
1844
    Joystick* joystick = _joystickManager->activeJoystick();
1845 1846 1847 1848 1849 1850 1851 1852 1853 1854 1855 1856 1857 1858 1859 1860 1861 1862
    if (joystick) {
        if (start) {
            if (_joystickEnabled) {
                joystick->startPolling(this);
            }
        } else {
            joystick->stopPolling();
        }
    }
}

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

void Vehicle::setActive(bool active)
{
1863 1864 1865 1866
    if (_active != active) {
        _active = active;
        emit activeChanged(_active);
    }
1867
}
1868

1869 1870 1871 1872
QGeoCoordinate Vehicle::homePosition(void)
{
    return _homePosition;
}
Don Gagne's avatar
Don Gagne committed
1873 1874 1875 1876

void Vehicle::setArmed(bool armed)
{
    // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks.
1877
    sendMavCommand(_defaultComponentId,
1878 1879 1880
                   MAV_CMD_COMPONENT_ARM_DISARM,
                   true,    // show error if fails
                   armed ? 1.0f : 0.0f);
Don Gagne's avatar
Don Gagne committed
1881 1882 1883 1884
}

bool Vehicle::flightModeSetAvailable(void)
{
1885
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
Don Gagne's avatar
Don Gagne committed
1886 1887 1888 1889
}

QStringList Vehicle::flightModes(void)
{
Daniel Agar's avatar
Daniel Agar committed
1890
    return _firmwarePlugin->flightModes(this);
Don Gagne's avatar
Don Gagne committed
1891 1892
}

Don Gagne's avatar
Don Gagne committed
1893
QString Vehicle::flightMode(void) const
Don Gagne's avatar
Don Gagne committed
1894 1895 1896 1897 1898 1899 1900 1901 1902 1903 1904 1905 1906 1907 1908 1909
{
    return _firmwarePlugin->flightMode(_base_mode, _custom_mode);
}

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

    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;
1910 1911 1912 1913 1914 1915 1916 1917
        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
1918
    } else {
Don Gagne's avatar
Don Gagne committed
1919
        qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;
Don Gagne's avatar
Don Gagne committed
1920 1921 1922 1923 1924 1925 1926 1927 1928 1929 1930 1931 1932 1933 1934 1935 1936
    }
}

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

1937 1938 1939 1940 1941 1942 1943 1944
    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
1945
}
1946

1947
void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
1948 1949 1950
{
    mavlink_message_t               msg;
    mavlink_request_data_stream_t   dataStream;
dogmaphobic's avatar
dogmaphobic committed
1951

Don Gagne's avatar
Don Gagne committed
1952 1953
    memset(&dataStream, 0, sizeof(dataStream));

1954 1955 1956 1957
    dataStream.req_stream_id = stream;
    dataStream.req_message_rate = rate;
    dataStream.start_stop = 1;  // start
    dataStream.target_system = id();
1958
    dataStream.target_component = _defaultComponentId;
dogmaphobic's avatar
dogmaphobic committed
1959

1960 1961 1962 1963 1964
    mavlink_msg_request_data_stream_encode_chan(_mavlink->getSystemId(),
                                                _mavlink->getComponentId(),
                                                priorityLink()->mavlinkChannel(),
                                                &msg,
                                                &dataStream);
1965

1966 1967 1968 1969
    if (sendMultiple) {
        // We use sendMessageMultiple since we really want these to make it to the vehicle
        sendMessageMultiple(msg);
    } else {
1970
        sendMessageOnLink(priorityLink(), msg);
1971
    }
1972 1973 1974 1975 1976 1977
}

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

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

1981 1982 1983 1984 1985 1986
        if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) {
            _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex);
        } else {
            _nextSendMessageMultipleIndex++;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
1987

1988 1989 1990 1991 1992 1993 1994 1995
    if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) {
        _nextSendMessageMultipleIndex = 0;
    }
}

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

1997 1998
    info.message =      message;
    info.retryCount =   _sendMessageMultipleRetries;
dogmaphobic's avatar
dogmaphobic committed
1999

2000 2001
    _sendMessageMultipleList.append(info);
}
2002 2003 2004 2005

void Vehicle::_missionManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
2006
    qgcApp()->showMessage(tr("Mission transfer failed. Retry transfer. Error: %1").arg(errorMsg));
2007
}
2008

2009 2010 2011
void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
2012
    qgcApp()->showMessage(tr("GeoFence transfer failed. Retry transfer. Error: %1").arg(errorMsg));
2013 2014 2015 2016 2017
}

void Vehicle::_rallyPointManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
2018
    qgcApp()->showMessage(tr("Rally Point transfer failed. Retry transfer. Error: %1").arg(errorMsg));
2019 2020
}

2021 2022 2023
void Vehicle::_addNewMapTrajectoryPoint(void)
{
    if (_mapTrajectoryHaveFirstCoordinate) {
2024 2025
        // Keep three minutes of trajectory on mobile due to perf impact of lines
#ifdef __mobile__
2026 2027 2028
        if (_mapTrajectoryList.count() * _mapTrajectoryMsecsBetweenPoints > 3 * 1000 * 60) {
            _mapTrajectoryList.removeAt(0)->deleteLater();
        }
2029
#endif
2030
        _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this));
2031
        _flightDistanceFact.setRawValue(_flightDistanceFact.rawValue().toDouble() + _mapTrajectoryLastCoordinate.distanceTo(_coordinate));
2032 2033
    }
    _mapTrajectoryHaveFirstCoordinate = true;
2034
    _mapTrajectoryLastCoordinate = _coordinate;
2035
    _flightTimeFact.setRawValue((double)_flightTimer.elapsed() / 1000.0);
2036 2037
}

2038 2039 2040 2041 2042 2043 2044 2045
void Vehicle::_clearTrajectoryPoints(void)
{
    _mapTrajectoryList.clearAndDeleteContents();
}

void Vehicle::_clearCameraTriggerPoints(void)
{
    _cameraTriggerPoints.clearAndDeleteContents();
2046 2047 2048 2049 2050
}

void Vehicle::_mapTrajectoryStart(void)
{
    _mapTrajectoryHaveFirstCoordinate = false;
2051
    _clearTrajectoryPoints();
2052
    _mapTrajectoryTimer.start();
2053
    _flightTimer.start();
2054
    _flightDistanceFact.setRawValue(0);
2055
    _flightTimeFact.setRawValue(0);
2056 2057 2058 2059 2060 2061
}

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

DonLakeFlyer's avatar
DonLakeFlyer committed
2063
void Vehicle::_startPlanRequest(void)
2064
{
DonLakeFlyer's avatar
DonLakeFlyer committed
2065 2066 2067 2068 2069
    if (_missionManagerInitialRequestSent) {
        return;
    }

    if (_parameterManager->parametersReady() && _vehicleCapabilitiesKnown) {
DonLakeFlyer's avatar
DonLakeFlyer committed
2070
        qCDebug(VehicleLog) << "_startPlanRequest";
2071
        _missionManagerInitialRequestSent = true;
2072 2073 2074 2075
        if (_settingsManager->appSettings()->autoLoadMissions()->rawValue().toBool()) {
            QString missionAutoLoadDirPath = _settingsManager->appSettings()->missionSavePath();
            if (!missionAutoLoadDirPath.isEmpty()) {
                QDir missionAutoLoadDir(missionAutoLoadDirPath);
2076 2077
                QString autoloadFilename = missionAutoLoadDir.absoluteFilePath(tr("AutoLoad%1.%2").arg(_id).arg(AppSettings::planFileExtension));
                if (QFile(autoloadFilename).exists()) {
2078
                    _initialPlanRequestComplete = true; // We aren't going to load from the vehicle, so we are done
2079
                    PlanMasterController::sendPlanToVehicle(this, autoloadFilename);
2080 2081
                    return;
                }
Don Gagne's avatar
Don Gagne committed
2082 2083
            }
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
2084
        _missionManager->loadFromVehicle();
2085 2086
    } else {
        if (!_parameterManager->parametersReady()) {
DonLakeFlyer's avatar
DonLakeFlyer committed
2087
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to parameters not ready";
2088
        } else if (!_vehicleCapabilitiesKnown) {
DonLakeFlyer's avatar
DonLakeFlyer committed
2089
            qCDebug(VehicleLog) << "Delaying _startPlanRequest due to vehicle capabilities not known";
2090 2091 2092 2093
        }
    }
}

DonLakeFlyer's avatar
DonLakeFlyer committed
2094 2095 2096 2097 2098
void Vehicle::_missionLoadComplete(void)
{
    // After the initial mission request completes we ask for the geofence
    if (!_geoFenceManagerInitialRequestSent) {
        _geoFenceManagerInitialRequestSent = true;
2099 2100 2101 2102 2103 2104 2105
        if (_geoFenceManager->supported()) {
            qCDebug(VehicleLog) << "_missionLoadComplete requesting GeoFence";
            _geoFenceManager->loadFromVehicle();
        } else {
            qCDebug(VehicleLog) << "_missionLoadComplete GeoFence not supported skipping";
            _geoFenceLoadComplete();
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
2106 2107 2108 2109 2110 2111 2112 2113
    }
}

void Vehicle::_geoFenceLoadComplete(void)
{
    // After geofence request completes we ask for the rally points
    if (!_rallyPointManagerInitialRequestSent) {
        _rallyPointManagerInitialRequestSent = true;
2114 2115 2116 2117 2118 2119 2120
        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
2121 2122 2123 2124 2125
    }
}

void Vehicle::_rallyPointLoadComplete(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
2126
    qCDebug(VehicleLog) << "_missionLoadComplete _initialPlanRequestComplete = true";
2127 2128
    if (!_initialPlanRequestComplete) {
        _initialPlanRequestComplete = true;
Don Gagne's avatar
Don Gagne committed
2129
        emit initialPlanRequestCompleteChanged(true);
2130
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
2131 2132
}

2133 2134 2135
void Vehicle::_parametersReady(bool parametersReady)
{
    if (parametersReady) {
2136
        _setupAutoDisarmSignalling();
DonLakeFlyer's avatar
DonLakeFlyer committed
2137
        _startPlanRequest();
Lorenz Meier's avatar
Lorenz Meier committed
2138
    }
2139
}
2140

Don Gagne's avatar
Don Gagne committed
2141
void Vehicle::disconnectInactiveVehicle(void)
2142
{
Don Gagne's avatar
Don Gagne committed
2143
    // Vehicle is no longer communicating with us, disconnect all links
2144

2145

2146
    LinkManager* linkMgr = _toolbox->linkManager();
2147
    for (int i=0; i<_links.count(); i++) {
2148 2149
        // 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.
2150
        if (!_toolbox->multiVehicleManager()->linkInUse(_links[i], this)) {
2151 2152
            linkMgr->disconnectLink(_links[i]);
        }
2153 2154
    }
}
2155

dogmaphobic's avatar
dogmaphobic committed
2156 2157 2158 2159 2160
void Vehicle::_imageReady(UASInterface*)
{
    if(_uas)
    {
        QImage img = _uas->getImage();
2161
        _toolbox->imageProvider()->setImage(&img, _id);
dogmaphobic's avatar
dogmaphobic committed
2162 2163 2164 2165
        _flowImageIndex++;
        emit flowImageIndexChanged();
    }
}
Don Gagne's avatar
Don Gagne committed
2166 2167 2168

void Vehicle::_remoteControlRSSIChanged(uint8_t rssi)
{
2169 2170 2171 2172
    if (_rcRSSIstore < 0 || _rcRSSIstore > 100) {
        _rcRSSIstore = rssi;
    }

Don Gagne's avatar
Don Gagne committed
2173 2174 2175 2176 2177 2178 2179 2180 2181 2182 2183
    // 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
2184 2185 2186

void Vehicle::virtualTabletJoystickValue(double roll, double pitch, double yaw, double thrust)
{
2187 2188 2189 2190
    // 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
2191
}
2192 2193 2194 2195 2196 2197 2198 2199 2200 2201 2202

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

void Vehicle::_connectionLostTimeout(void)
{
2203 2204 2205 2206 2207
    if (highLatencyLink()) {
        // No connection timeout on high latency links
        return;
    }

2208 2209
    if (_connectionLostEnabled && !_connectionLost) {
        _connectionLost = true;
2210
        _heardFrom = false;
2211
        _maxProtoVersion = 0;
2212
        emit connectionLostChanged(true);
2213
        _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech()));
Don Gagne's avatar
Don Gagne committed
2214
        if (_autoDisconnect) {
2215 2216 2217 2218 2219

            // Reset link state
            for (int i = 0; i < _links.length(); i++) {
                _mavlink->resetMetadataForLink(_links.at(i));
            }
Don Gagne's avatar
Don Gagne committed
2220 2221
            disconnectInactiveVehicle();
        }
2222 2223 2224 2225 2226 2227 2228 2229 2230
    }
}

void Vehicle::_connectionActive(void)
{
    _connectionLostTimer.start();
    if (_connectionLost) {
        _connectionLost = false;
        emit connectionLostChanged(false);
2231
        _say(QString(tr("%1 communication regained")).arg(_vehicleIdSpeech()));
2232 2233 2234

        // Re-negotiate protocol version for the link
        sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
2235
                       MAV_CMD_REQUEST_PROTOCOL_VERSION,
2236
                       false,                                   // No error shown if fails
2237
                       1);                                     // Request protocol version
2238 2239 2240
    }
}

2241
void Vehicle::_say(const QString& text)
2242
{
2243
    _toolbox->audioOutput()->say(text.toLower());
2244
}
2245 2246 2247

bool Vehicle::fixedWing(void) const
{
2248
    return QGCMAVLink::isFixedWing(vehicleType());
2249 2250
}

Don Gagne's avatar
Don Gagne committed
2251 2252
bool Vehicle::rover(void) const
{
2253
    return QGCMAVLink::isRover(vehicleType());
Don Gagne's avatar
Don Gagne committed
2254 2255
}

2256 2257
bool Vehicle::sub(void) const
{
2258
    return QGCMAVLink::isSub(vehicleType());
2259 2260
}

2261 2262
bool Vehicle::multiRotor(void) const
{
2263
    return QGCMAVLink::isMultiRotor(vehicleType());
2264
}
Don Gagne's avatar
Don Gagne committed
2265

Don Gagne's avatar
Don Gagne committed
2266 2267
bool Vehicle::vtol(void) const
{
2268
    return _firmwarePlugin->isVtol(this);
Don Gagne's avatar
Don Gagne committed
2269 2270
}

2271 2272 2273 2274 2275
bool Vehicle::supportsThrottleModeCenterZero(void) const
{
    return _firmwarePlugin->supportsThrottleModeCenterZero();
}

2276 2277 2278 2279 2280
bool Vehicle::supportsNegativeThrust(void) const
{
    return _firmwarePlugin->supportsNegativeThrust();
}

2281 2282 2283 2284 2285
bool Vehicle::supportsRadio(void) const
{
    return _firmwarePlugin->supportsRadio();
}

2286 2287 2288 2289 2290
bool Vehicle::supportsJSButton(void) const
{
    return _firmwarePlugin->supportsJSButton();
}

2291 2292 2293 2294 2295
bool Vehicle::supportsMotorInterference(void) const
{
    return _firmwarePlugin->supportsMotorInterference();
}

2296 2297 2298 2299 2300 2301 2302 2303 2304 2305 2306 2307 2308 2309 2310 2311 2312 2313 2314 2315 2316 2317 2318 2319 2320 2321 2322 2323 2324 2325 2326 2327 2328 2329
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];
}

2330 2331 2332
/// Returns the string to speak to identify the vehicle
QString Vehicle::_vehicleIdSpeech(void)
{
2333
    if (_toolbox->multiVehicleManager()->vehicles()->count() > 1) {
2334
        return QString(tr("vehicle %1")).arg(id());
2335
    } else {
2336
        return QString();
2337 2338 2339
    }
}

Don Gagne's avatar
Don Gagne committed
2340
void Vehicle::_handleFlightModeChanged(const QString& flightMode)
2341
{
2342
    _say(QString(tr("%1 %2 flight mode")).arg(_vehicleIdSpeech()).arg(flightMode));
Don Gagne's avatar
Don Gagne committed
2343
    emit guidedModeChanged(_firmwarePlugin->isGuidedMode(this));
2344 2345 2346 2347
}

void Vehicle::_announceArmedChanged(bool armed)
{
2348
    _say(QString("%1 %2").arg(_vehicleIdSpeech()).arg(armed ? QString(tr("armed")) : QString(tr("disarmed"))));
2349 2350
}

2351
void Vehicle::_setFlying(bool flying)
Don Gagne's avatar
Don Gagne committed
2352
{
2353
    if (_flying != flying) {
Don Gagne's avatar
Don Gagne committed
2354 2355 2356 2357 2358
        _flying = flying;
        emit flyingChanged(flying);
    }
}

2359 2360 2361 2362 2363 2364 2365 2366
void Vehicle::_setLanding(bool landing)
{
    if (armed() && _landing != landing) {
        _landing = landing;
        emit landingChanged(landing);
    }
}

Don Gagne's avatar
Don Gagne committed
2367 2368
bool Vehicle::guidedModeSupported(void) const
{
2369
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability);
Don Gagne's avatar
Don Gagne committed
2370 2371 2372 2373
}

bool Vehicle::pauseVehicleSupported(void) const
{
2374 2375 2376 2377 2378 2379
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::PauseVehicleCapability);
}

bool Vehicle::orbitModeSupported() const
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::OrbitModeCapability);
Don Gagne's avatar
Don Gagne committed
2380 2381
}

2382 2383 2384 2385 2386
bool Vehicle::takeoffVehicleSupported() const
{
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::TakeoffVehicleCapability);
}

Don Gagne's avatar
Don Gagne committed
2387 2388 2389
void Vehicle::guidedModeRTL(void)
{
    if (!guidedModeSupported()) {
2390
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2391 2392 2393 2394 2395 2396 2397 2398
        return;
    }
    _firmwarePlugin->guidedModeRTL(this);
}

void Vehicle::guidedModeLand(void)
{
    if (!guidedModeSupported()) {
2399
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2400 2401 2402 2403 2404
        return;
    }
    _firmwarePlugin->guidedModeLand(this);
}

2405
void Vehicle::guidedModeTakeoff(double altitudeRelative)
Don Gagne's avatar
Don Gagne committed
2406 2407
{
    if (!guidedModeSupported()) {
2408
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2409 2410 2411
        return;
    }
    setGuidedMode(true);
2412
    _firmwarePlugin->guidedModeTakeoff(this, altitudeRelative);
2413 2414
}

2415 2416 2417 2418 2419
double Vehicle::minimumTakeoffAltitude(void)
{
    return _firmwarePlugin->minimumTakeoffAltitude(this);
}

2420 2421 2422
void Vehicle::startMission(void)
{
    _firmwarePlugin->startMission(this);
Don Gagne's avatar
Don Gagne committed
2423 2424 2425 2426 2427
}

void Vehicle::guidedModeGotoLocation(const QGeoCoordinate& gotoCoord)
{
    if (!guidedModeSupported()) {
2428
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2429 2430
        return;
    }
DonLakeFlyer's avatar
DonLakeFlyer committed
2431 2432 2433 2434 2435 2436 2437 2438
    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
2439 2440 2441
    _firmwarePlugin->guidedModeGotoLocation(this, gotoCoord);
}

2442
void Vehicle::guidedModeChangeAltitude(double altitudeChange)
Don Gagne's avatar
Don Gagne committed
2443 2444
{
    if (!guidedModeSupported()) {
2445
        qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
Don Gagne's avatar
Don Gagne committed
2446 2447
        return;
    }
2448
    _firmwarePlugin->guidedModeChangeAltitude(this, altitudeChange);
Don Gagne's avatar
Don Gagne committed
2449 2450
}

2451 2452
void Vehicle::guidedModeOrbit(const QGeoCoordinate& centerCoord, double radius, double velocity, double altitude)
{
2453 2454
    if (!orbitModeSupported()) {
        qgcApp()->showMessage(QStringLiteral("Orbit mode not supported by Vehicle."));
2455 2456 2457 2458 2459
        return;
    }
    _firmwarePlugin->guidedModeOrbit(this, centerCoord, radius, velocity, altitude);
}

Don Gagne's avatar
Don Gagne committed
2460 2461 2462 2463 2464 2465 2466 2467 2468
void Vehicle::pauseVehicle(void)
{
    if (!pauseVehicleSupported()) {
        qgcApp()->showMessage(QStringLiteral("Pause not supported by vehicle."));
        return;
    }
    _firmwarePlugin->pauseVehicle(this);
}

2469
void Vehicle::abortLanding(double climbOutAltitude)
2470 2471 2472 2473
{
    sendMavCommand(defaultComponentId(),
                   MAV_CMD_DO_GO_AROUND,
                   true,        // show error if fails
2474
                   climbOutAltitude);
2475 2476
}

Don Gagne's avatar
Don Gagne committed
2477 2478 2479 2480 2481 2482 2483 2484 2485 2486 2487 2488
bool Vehicle::guidedMode(void) const
{
    return _firmwarePlugin->isGuidedMode(this);
}

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

void Vehicle::emergencyStop(void)
{
2489
    sendMavCommand(_defaultComponentId,
2490 2491 2492 2493
                   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
2494 2495
}

2496 2497 2498 2499 2500 2501
void Vehicle::setCurrentMissionSequence(int seq)
{
    if (!_firmwarePlugin->sendHomePositionToVehicle()) {
        seq--;
    }
    mavlink_message_t msg;
2502 2503 2504 2505 2506 2507 2508 2509
    mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(),
                                              _mavlink->getComponentId(),
                                              priorityLink()->mavlinkChannel(),
                                              &msg,
                                              id(),
                                              _compID,
                                              seq);
    sendMessageOnLink(priorityLink(), msg);
2510 2511
}

2512
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
2513
{
2514 2515 2516 2517 2518 2519 2520 2521 2522 2523 2524 2525 2526 2527 2528 2529 2530 2531 2532 2533 2534 2535 2536
    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)
{
2537 2538 2539 2540 2541 2542
    if(!_mavCommandQueue.size()) {
        qWarning() << "Command resend with no commands in queue";
        _mavCommandAckTimer.stop();
        return;
    }

2543 2544 2545
    MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];

    if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
2546 2547
        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
2548
            qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES. Setting no capabilities. Starting Plan request.";
DonLakeFlyer's avatar
DonLakeFlyer committed
2549
            _setCapabilities(0);
DonLakeFlyer's avatar
DonLakeFlyer committed
2550
            _startPlanRequest();
2551 2552
        }

2553
        if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
2554 2555
            // 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.
DonLakeFlyer's avatar
DonLakeFlyer committed
2556
            qCDebug(VehicleLog) << "Vehicle failed to responded to MAV_CMD_REQUEST_PROTOCOL_VERSION. Starting Plan request.";
2557
            if (_maxProtoVersion == 0) {
DonLakeFlyer's avatar
DonLakeFlyer committed
2558
                qCDebug(VehicleLog) << "Setting _maxProtoVersion to 100 since not yet set.";
2559
                _setMaxProtoVersion(100);
DonLakeFlyer's avatar
DonLakeFlyer committed
2560 2561
            } else {
                qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion;
2562
            }
2563 2564
        }

2565 2566
        emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
        if (queuedCommand.showError) {
2567
            qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(_toolbox->missionCommandTree()->friendlyName(queuedCommand.command)));
2568 2569 2570 2571 2572 2573
        }
        _mavCommandQueue.removeFirst();
        _sendNextQueuedMavCommand();
        return;
    }

2574
    if (_mavCommandRetryCount > 1) {
2575 2576 2577 2578 2579 2580 2581 2582 2583 2584 2585 2586 2587 2588 2589 2590 2591 2592 2593
        // 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;
                }
            }
2594
        }
DonLakeFlyer's avatar
DonLakeFlyer committed
2595
        qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
2596 2597
    }

2598 2599
    _mavCommandAckTimer.start();

Don Gagne's avatar
Don Gagne committed
2600 2601 2602
    mavlink_message_t       msg;
    mavlink_command_long_t  cmd;

Don Gagne's avatar
Don Gagne committed
2603
    memset(&cmd, 0, sizeof(cmd));
2604
    cmd.command = queuedCommand.command;
Don Gagne's avatar
Don Gagne committed
2605
    cmd.confirmation = 0;
2606 2607 2608 2609 2610 2611 2612 2613 2614
    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;
2615 2616 2617 2618 2619
    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                         _mavlink->getComponentId(),
                                         priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
Don Gagne's avatar
Don Gagne committed
2620

2621
    sendMessageOnLink(priorityLink(), msg);
Don Gagne's avatar
Don Gagne committed
2622
}
2623

2624 2625 2626 2627 2628 2629 2630 2631 2632
void Vehicle::_sendNextQueuedMavCommand(void)
{
    if (_mavCommandQueue.count()) {
        _mavCommandRetryCount = 0;
        _sendMavCommandAgain();
    }
}


DonLakeFlyer's avatar
DonLakeFlyer committed
2633 2634 2635 2636 2637 2638 2639 2640 2641 2642 2643 2644 2645 2646 2647 2648 2649 2650 2651 2652 2653 2654 2655 2656 2657 2658 2659 2660 2661 2662 2663 2664 2665 2666 2667 2668 2669 2670 2671 2672 2673 2674 2675 2676 2677 2678 2679 2680 2681 2682 2683 2684 2685 2686 2687 2688 2689 2690 2691 2692
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
    bool showError = false;

    mavlink_command_ack_t ack;
    mavlink_msg_command_ack_decode(&message, &ack);

    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) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES with error(%1). Setting no capabilities. Starting Plan request.").arg(ack.result);
        _setCapabilities(0);
    }

    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
        qCDebug(VehicleLog) << QStringLiteral("Vehicle responded to MAV_CMD_REQUEST_PROTOCOL_VERSION with error(%1).").arg(ack.result);
        if (_maxProtoVersion == 0) {
            qCDebug(VehicleLog) << "Setting _maxProtoVersion to 100 since not yet set.";
            _setMaxProtoVersion(100);
        } else {
            qCDebug(VehicleLog) << "Leaving _maxProtoVersion at current value" << _maxProtoVersion;
        }
        // FIXME: Is this missing here. I believe it is a bug. Debug to verify. May need to go into Stable.
        //_startPlanRequest();
    }

    if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
        _mavCommandAckTimer.stop();
        showError = _mavCommandQueue[0].showError;
        _mavCommandQueue.removeFirst();
    }

    emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);

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

        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;
        default:
            // Do nothing
            break;
        }
    }

    _sendNextQueuedMavCommand();
}

Don Gagne's avatar
Don Gagne committed
2693 2694 2695 2696 2697 2698 2699 2700 2701 2702 2703 2704 2705
void Vehicle::setPrearmError(const QString& prearmError)
{
    _prearmError = prearmError;
    emit prearmErrorChanged(_prearmError);
    if (!_prearmError.isEmpty()) {
        _prearmErrorTimer.start();
    }
}

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

2707
void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType)
Don Gagne's avatar
Don Gagne committed
2708 2709 2710 2711
{
    _firmwareMajorVersion = majorVersion;
    _firmwareMinorVersion = minorVersion;
    _firmwarePatchVersion = patchVersion;
2712
    _firmwareVersionType = versionType;
2713 2714 2715 2716 2717 2718 2719 2720 2721
    emit firmwareVersionChanged();
}

void Vehicle::setFirmwareCustomVersion(int majorVersion, int minorVersion, int patchVersion)
{
    _firmwareCustomMajorVersion = majorVersion;
    _firmwareCustomMinorVersion = minorVersion;
    _firmwareCustomPatchVersion = patchVersion;
    emit firmwareCustomVersionChanged();
2722 2723 2724 2725 2726 2727 2728 2729 2730 2731 2732 2733 2734 2735 2736 2737 2738
}

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
2739 2740
}

2741 2742
void Vehicle::rebootVehicle()
{
2743
    sendMavCommand(_defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f);
Don Gagne's avatar
Don Gagne committed
2744 2745
}

2746 2747 2748 2749 2750 2751 2752 2753
void Vehicle::setSoloFirmware(bool soloFirmware)
{
    if (soloFirmware != _soloFirmware) {
        _soloFirmware = soloFirmware;
        emit soloFirmwareChanged(soloFirmware);
    }
}

Don Gagne's avatar
Don Gagne committed
2754
#if 0
2755
// Temporarily removed, waiting for new command implementation
Don Gagne's avatar
Don Gagne committed
2756 2757
void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
{
2758
    doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
Don Gagne's avatar
Don Gagne committed
2759
}
Don Gagne's avatar
Don Gagne committed
2760
#endif
Don Gagne's avatar
Don Gagne committed
2761

2762
QString Vehicle::brandImageIndoor(void) const
2763
{
2764 2765 2766 2767 2768 2769
    return _firmwarePlugin->brandImageIndoor(this);
}

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

2772 2773 2774 2775 2776 2777 2778 2779 2780 2781 2782 2783 2784 2785 2786 2787 2788 2789 2790 2791 2792 2793 2794 2795 2796 2797 2798 2799 2800 2801 2802 2803 2804 2805 2806
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" },
2807
        { MAV_SYS_STATUS_SENSOR_BATTERY,                "Battery" },
2808 2809 2810 2811 2812 2813 2814 2815 2816 2817 2818 2819
    };

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

2820 2821 2822 2823 2824 2825 2826 2827
void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId)
{
    if (_offlineEditingVehicle) {
        _defaultComponentId = defaultComponentId;
    } else {
        qWarning() << "Call to Vehicle::setOfflineEditingDefaultComponentId on vehicle which is not offline";
    }
}
2828

Don Gagne's avatar
Don Gagne committed
2829 2830
void Vehicle::triggerCamera(void)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
2831
    sendMavCommand(_defaultComponentId,
Don Gagne's avatar
Don Gagne committed
2832 2833 2834
                   MAV_CMD_DO_DIGICAM_CONTROL,
                   true,                            // show errors
                   0.0, 0.0, 0.0, 0.0,              // param 1-4 unused
2835 2836 2837
                   1.0,                             // trigger camera
                   0.0,                             // param 6 unused
                   1.0);                            // test shot flag
Don Gagne's avatar
Don Gagne committed
2838 2839
}

2840 2841 2842
void Vehicle::setVtolInFwdFlight(bool vtolInFwdFlight)
{
    if (_vtolInFwdFlight != vtolInFwdFlight) {
DonLakeFlyer's avatar
DonLakeFlyer committed
2843 2844 2845 2846 2847
        sendMavCommand(_defaultComponentId,
                       MAV_CMD_DO_VTOL_TRANSITION,
                       true,                                                    // show errors
                       vtolInFwdFlight ? MAV_VTOL_STATE_FW : MAV_VTOL_STATE_MC, // transition state
                       0, 0, 0, 0, 0, 0);                                       // param 2-7 unused
2848 2849 2850
    }
}

2851 2852
const char* VehicleGPSFactGroup::_latFactName =                 "lat";
const char* VehicleGPSFactGroup::_lonFactName =                 "lon";
2853 2854 2855 2856 2857
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
2858 2859 2860

VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent)
2861 2862
    , _latFact              (0, _latFactName,               FactMetaData::valueTypeDouble)
    , _lonFact              (0, _lonFactName,               FactMetaData::valueTypeDouble)
Don Gagne's avatar
Don Gagne committed
2863 2864 2865 2866 2867 2868
    , _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)
{
2869 2870
    _addFact(&_latFact,                 _latFactName);
    _addFact(&_lonFact,                 _lonFactName);
Don Gagne's avatar
Don Gagne committed
2871 2872 2873 2874 2875
    _addFact(&_hdopFact,                _hdopFactName);
    _addFact(&_vdopFact,                _vdopFactName);
    _addFact(&_courseOverGroundFact,    _courseOverGroundFactName);
    _addFact(&_lockFact,                _lockFactName);
    _addFact(&_countFact,               _countFactName);
2876

2877 2878
    _latFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
    _lonFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
2879 2880 2881
    _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
2882 2883
}

2884
void Vehicle::startMavlinkLog()
2885
{
2886
    sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_START, false /* showError */);
2887 2888
}

2889
void Vehicle::stopMavlinkLog()
2890
{
2891
    sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_STOP, false /* showError */);
2892 2893
}

2894
void Vehicle::_ackMavlinkLogData(uint16_t sequence)
2895 2896 2897
{
    mavlink_message_t msg;
    mavlink_logging_ack_t ack;
Don Gagne's avatar
Don Gagne committed
2898
    memset(&ack, 0, sizeof(ack));
2899
    ack.sequence = sequence;
2900
    ack.target_component = _defaultComponentId;
2901 2902
    ack.target_system = id();
    mavlink_msg_logging_ack_encode_chan(
2903 2904 2905 2906 2907
                _mavlink->getSystemId(),
                _mavlink->getComponentId(),
                priorityLink()->mavlinkChannel(),
                &msg,
                &ack);
2908 2909 2910
    sendMessageOnLink(priorityLink(), msg);
}

2911
void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
2912 2913 2914
{
    mavlink_logging_data_t log;
    mavlink_msg_logging_data_decode(&message, &log);
Gus Grubba's avatar
Gus Grubba committed
2915
    emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
2916
                        log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
2917 2918
}

2919
void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
2920
{
2921 2922
    mavlink_logging_data_acked_t log;
    mavlink_msg_logging_data_acked_decode(&message, &log);
2923
    _ackMavlinkLogData(log.sequence);
Gus Grubba's avatar
Gus Grubba committed
2924
    emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
2925
                        log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
2926 2927
}

2928 2929 2930 2931 2932 2933
void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
{
    firmwarePluginInstanceData->setParent(this);
    _firmwarePluginInstanceData = firmwarePluginInstanceData;
}

2934 2935 2936 2937 2938
QString Vehicle::missionFlightMode(void) const
{
    return _firmwarePlugin->missionFlightMode();
}

2939 2940 2941 2942 2943
QString Vehicle::pauseFlightMode(void) const
{
    return _firmwarePlugin->pauseFlightMode();
}

2944 2945 2946 2947 2948
QString Vehicle::rtlFlightMode(void) const
{
    return _firmwarePlugin->rtlFlightMode();
}

2949 2950 2951 2952 2953
QString Vehicle::landFlightMode(void) const
{
    return _firmwarePlugin->landFlightMode();
}

2954 2955 2956 2957 2958
QString Vehicle::takeControlFlightMode(void) const
{
    return _firmwarePlugin->takeControlFlightMode();
}

2959 2960 2961 2962 2963 2964 2965 2966 2967 2968 2969 2970 2971 2972 2973 2974 2975 2976 2977 2978 2979 2980 2981 2982
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();
}

2983
const QVariantList& Vehicle::toolBarIndicators()
2984 2985 2986 2987 2988 2989 2990 2991
{
    if(_firmwarePlugin) {
        return _firmwarePlugin->toolBarIndicators(this);
    }
    static QVariantList emptyList;
    return emptyList;
}

Gus Grubba's avatar
Gus Grubba committed
2992
const QVariantList& Vehicle::staticCameraList(void) const
2993 2994 2995 2996 2997 2998 2999 3000
{
    if (_firmwarePlugin) {
        return _firmwarePlugin->cameraList(this);
    }
    static QVariantList emptyList;
    return emptyList;
}

3001 3002 3003 3004
bool Vehicle::vehicleYawsToNextWaypointInMission(void) const
{
    return _firmwarePlugin->vehicleYawsToNextWaypointInMission(this);
}
3005

3006 3007 3008 3009 3010 3011 3012 3013 3014 3015 3016 3017 3018 3019 3020 3021 3022 3023 3024 3025 3026 3027 3028
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;
}

3029 3030 3031
void Vehicle::_handleADSBVehicle(const mavlink_message_t& message)
{
    mavlink_adsb_vehicle_t adsbVehicle;
Don Gagne's avatar
Don Gagne committed
3032
    static const int maxTimeSinceLastSeen = 15;
3033 3034 3035 3036

    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
3037 3038 3039 3040 3041 3042
            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
3043
                _adsbICAOMap[adsbVehicle.ICAO_address]->update(adsbVehicle);
DonLakeFlyer's avatar
DonLakeFlyer committed
3044
            }
Don Gagne's avatar
Don Gagne committed
3045
        } else if (adsbVehicle.tslc <= maxTimeSinceLastSeen) {
3046 3047 3048 3049 3050 3051 3052
            ADSBVehicle* vehicle = new ADSBVehicle(adsbVehicle, this);
            _adsbICAOMap[adsbVehicle.ICAO_address] = vehicle;
            _adsbVehicles.append(vehicle);
        }
    }
}

3053 3054 3055 3056 3057 3058 3059 3060 3061
void Vehicle::_updateDistanceToHome(void)
{
    if (coordinate().isValid() && homePosition().isValid()) {
        _distanceToHomeFact.setRawValue(coordinate().distanceTo(homePosition()));
    } else {
        _distanceToHomeFact.setRawValue(qQNaN());
    }
}

3062 3063 3064 3065 3066
void Vehicle::_updateHobbsMeter(void)
{
    _hobbsFact.setRawValue(hobbsMeter());
}

Don Gagne's avatar
Don Gagne committed
3067 3068 3069 3070 3071 3072 3073 3074 3075 3076 3077
void Vehicle::forceInitialPlanRequestComplete(void)
{
    _initialPlanRequestComplete = true;
    emit initialPlanRequestCompleteChanged(true);
}

void Vehicle::sendPlan(QString planFile)
{
    PlanMasterController::sendPlanToVehicle(this, planFile);
}

3078 3079 3080 3081 3082 3083
QString Vehicle::hobbsMeter()
{
    static const char* HOOBS_HI = "LND_FLIGHT_T_HI";
    static const char* HOOBS_LO = "LND_FLIGHT_T_LO";
    //-- TODO: Does this exist on non PX4?
    if (_parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_HI) &&
3084
            _parameterManager->parameterExists(FactSystem::defaultComponentId, HOOBS_LO)) {
3085 3086
        Fact* factHi = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_HI);
        Fact* factLo = _parameterManager->getParameter(FactSystem::defaultComponentId, HOOBS_LO);
3087
        uint64_t hobbsTimeSeconds = ((uint64_t)factHi->rawValue().toUInt() << 32 | (uint64_t)factLo->rawValue().toUInt()) / 1000000;
3088 3089 3090 3091 3092
        int hours   = hobbsTimeSeconds / 3600;
        int minutes = (hobbsTimeSeconds % 3600) / 60;
        int seconds = hobbsTimeSeconds % 60;
        QString timeStr;
        timeStr.sprintf("%04d:%02d:%02d", hours, minutes, seconds);
3093
        qCDebug(VehicleLog) << "Hobbs Meter:" << timeStr << "(" << factHi->rawValue().toUInt() << factLo->rawValue().toUInt() << ")";
3094 3095 3096 3097
        return timeStr;
    }
    return QString("0000:00:00");
}
Don Gagne's avatar
Don Gagne committed
3098

3099 3100 3101 3102 3103 3104 3105 3106 3107
void Vehicle::_vehicleParamLoaded(bool ready)
{
    //-- TODO: This seems silly but can you think of a better
    //   way to update this?
    if(ready) {
        emit hobbsMeterChanged();
    }
}

3108 3109 3110 3111
void Vehicle::_updateHighLatencyLink(void)
{
    if (_priorityLink->highLatency() != _highLatencyLink) {
        _highLatencyLink = _priorityLink->highLatency();
DonLakeFlyer's avatar
DonLakeFlyer committed
3112
        _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
3113 3114 3115 3116
        emit highLatencyLinkChanged(_highLatencyLink);
    }
}

3117 3118 3119
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

3120 3121 3122 3123 3124 3125
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";
3126
const char* VehicleBatteryFactGroup::_instantPowerFactName =                "instantPower";
3127 3128 3129 3130 3131 3132 3133 3134 3135

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;
3136
const double VehicleBatteryFactGroup::_instantPowerUnavailable =      -1.0;
3137

Don Gagne's avatar
Don Gagne committed
3138 3139
VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent)
3140 3141 3142
    , _voltageFact                  (0, _voltageFactName,                   FactMetaData::valueTypeDouble)
    , _percentRemainingFact         (0, _percentRemainingFactName,          FactMetaData::valueTypeInt32)
    , _mahConsumedFact              (0, _mahConsumedFactName,               FactMetaData::valueTypeInt32)
Don Gagne's avatar
Don Gagne committed
3143
    , _currentFact                  (0, _currentFactName,                   FactMetaData::valueTypeFloat)
3144 3145
    , _temperatureFact              (0, _temperatureFactName,               FactMetaData::valueTypeDouble)
    , _cellCountFact                (0, _cellCountFactName,                 FactMetaData::valueTypeInt32)
3146
    , _instantPowerFact             (0, _instantPowerFactName,              FactMetaData::valueTypeFloat)
3147
{
3148 3149 3150 3151 3152 3153
    _addFact(&_voltageFact,                 _voltageFactName);
    _addFact(&_percentRemainingFact,        _percentRemainingFactName);
    _addFact(&_mahConsumedFact,             _mahConsumedFactName);
    _addFact(&_currentFact,                 _currentFactName);
    _addFact(&_temperatureFact,             _temperatureFactName);
    _addFact(&_cellCountFact,               _cellCountFactName);
3154
    _addFact(&_instantPowerFact,            _instantPowerFactName);
Don Gagne's avatar
Don Gagne committed
3155 3156 3157 3158 3159 3160 3161 3162

    // Start out as not available
    _voltageFact.setRawValue            (_voltageUnavailable);
    _percentRemainingFact.setRawValue   (_percentRemainingUnavailable);
    _mahConsumedFact.setRawValue        (_mahConsumedUnavailable);
    _currentFact.setRawValue            (_currentUnavailable);
    _temperatureFact.setRawValue        (_temperatureUnavailable);
    _cellCountFact.setRawValue          (_cellCountUnavailable);
3163
    _instantPowerFact.setRawValue       (_instantPowerUnavailable);
Don Gagne's avatar
Don Gagne committed
3164 3165
}

3166 3167 3168 3169
const char* VehicleWindFactGroup::_directionFactName =      "direction";
const char* VehicleWindFactGroup::_speedFactName =          "speed";
const char* VehicleWindFactGroup::_verticalSpeedFactName =  "verticalSpeed";

Don Gagne's avatar
Don Gagne committed
3170 3171 3172 3173 3174 3175 3176 3177 3178 3179 3180 3181 3182 3183 3184 3185
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());
}

3186 3187 3188 3189 3190 3191 3192
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";

3193 3194 3195 3196 3197 3198 3199 3200 3201 3202 3203 3204 3205 3206 3207 3208 3209 3210 3211 3212 3213
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());
}
3214 3215 3216 3217 3218 3219 3220 3221 3222 3223 3224 3225 3226 3227 3228 3229 3230 3231 3232 3233 3234


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());
}
dheideman's avatar
dheideman committed
3235 3236 3237 3238 3239 3240 3241 3242 3243 3244 3245 3246 3247 3248 3249 3250 3251 3252 3253 3254 3255 3256 3257 3258

const char* VehicleClockFactGroup::_currentTimeFactName = "currentTime";
const char* VehicleClockFactGroup::_currentDateFactName = "currentDate";

VehicleClockFactGroup::VehicleClockFactGroup(QObject* parent)
    : FactGroup(1000, ":/json/Vehicle/ClockFact.json", parent)
    , _currentTimeFact  (0, _currentTimeFactName,    FactMetaData::valueTypeString)
    , _currentDateFact  (0, _currentDateFactName,    FactMetaData::valueTypeString)
{
    _addFact(&_currentTimeFact, _currentTimeFactName);
    _addFact(&_currentDateFact, _currentDateFactName);

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

void VehicleClockFactGroup::_updateAllValues(void)
{
    _currentTimeFact.setRawValue(QTime::currentTime().toString());
    _currentDateFact.setRawValue(QDateTime::currentDateTime().toString(QLocale::system().dateFormat(QLocale::ShortFormat)));

    FactGroup::_updateAllValues();
}