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

10 11 12 13 14 15

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

QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")

32 33 34 35
#define UPDATE_TIMER 50
#define DEFAULT_LAT  38.965767f
#define DEFAULT_LON -120.083923f

36 37
extern const char* guided_mode_not_supported_by_vehicle;

38 39 40
const char* Vehicle::_settingsGroup =               "Vehicle%1";        // %1 replaced with mavlink system id
const char* Vehicle::_joystickModeSettingsKey =     "JoystickMode";
const char* Vehicle::_joystickEnabledSettingsKey =  "JoystickEnabled";
41

Don Gagne's avatar
Don Gagne committed
42 43 44 45 46 47 48 49
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";
Don Gagne's avatar
Don Gagne committed
50 51 52

const char* Vehicle::_gpsFactGroupName =        "gps";
const char* Vehicle::_batteryFactGroupName =    "battery";
Don Gagne's avatar
Don Gagne committed
53
const char* Vehicle::_windFactGroupName =       "wind";
54
const char* Vehicle::_vibrationFactGroupName =  "vibration";
Don Gagne's avatar
Don Gagne committed
55

56
const int Vehicle::_lowBatteryAnnounceRepeatMSecs = 30 * 1000;
57

58 59 60 61 62 63
Vehicle::Vehicle(LinkInterface*             link,
                 int                        vehicleId,
                 MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 JoystickManager*           joystickManager)
Don Gagne's avatar
Don Gagne committed
64 65
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json")
    , _id(vehicleId)
66
    , _active(false)
67
    , _offlineEditingVehicle(false)
68
    , _firmwareType(firmwareType)
69
    , _vehicleType(vehicleType)
70
    , _firmwarePlugin(NULL)
71
    , _firmwarePluginInstanceData(NULL)
72
    , _autopilotPlugin(NULL)
73 74
    , _mavlink(NULL)
    , _soloFirmware(false)
75
    , _joystickMode(JoystickModeRC)
76
    , _joystickEnabled(false)
77
    , _uas(NULL)
78 79
    , _coordinate(37.803784, -122.462276)
    , _coordinateValid(false)
80
    , _homePositionAvailable(false)
81 82 83 84 85 86 87 88
    , _mav(NULL)
    , _currentMessageCount(0)
    , _messageCount(0)
    , _currentErrorCount(0)
    , _currentWarningCount(0)
    , _currentNormalCount(0)
    , _currentMessageType(MessageNone)
    , _updateCount(0)
89 90
    , _rcRSSI(255)
    , _rcRSSIstore(255)
Don Gagne's avatar
Don Gagne committed
91
    , _autoDisconnect(false)
Don Gagne's avatar
Don Gagne committed
92
    , _flying(false)
93 94 95 96
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
97 98 99
    , _useGpsRawIntForPosition(true)
    , _useGpsRawIntForAltitude(true)
    , _useAltitudeForAltitude(false)
100 101
    , _connectionLost(false)
    , _connectionLostEnabled(true)
102
    , _missionManager(NULL)
103
    , _missionManagerInitialRequestSent(false)
104
    , _geoFenceManager(NULL)
105 106 107
    , _geoFenceManagerInitialRequestSent(false)
    , _rallyPointManager(NULL)
    , _rallyPointManagerInitialRequestSent(false)
108
    , _parameterManager(NULL)
Don Gagne's avatar
Don Gagne committed
109 110 111
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
112
    , _nextSendMessageMultipleIndex(0)
113 114
    , _firmwarePluginManager(firmwarePluginManager)
    , _joystickManager(joystickManager)
dogmaphobic's avatar
dogmaphobic committed
115
    , _flowImageIndex(0)
Don Gagne's avatar
Don Gagne committed
116
    , _allLinksInactiveSent(false)
117 118 119 120 121 122
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
123 124 125
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
126
    , _firmwareVersionType(FIRMWARE_VERSION_TYPE_OFFICIAL)
Don Gagne's avatar
Don Gagne committed
127 128 129 130 131 132 133 134 135
    , _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)
    , _gpsFactGroup(this)
Don Gagne's avatar
Don Gagne committed
136
    , _batteryFactGroup(this)
Don Gagne's avatar
Don Gagne committed
137
    , _windFactGroup(this)
138
    , _vibrationFactGroup(this)
139 140
{
    _addLink(link);
dogmaphobic's avatar
dogmaphobic committed
141

142
    _mavlink = qgcApp()->toolbox()->mavlinkProtocol();
dogmaphobic's avatar
dogmaphobic committed
143

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

146
    connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
Don Gagne's avatar
Don Gagne committed
147
    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
148
    connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
149
    _uas = new UAS(_mavlink, this, _firmwarePluginManager);
dogmaphobic's avatar
dogmaphobic committed
150

Don Gagne's avatar
Don Gagne committed
151 152
    connect(_uas, &UAS::imageReady,                     this, &Vehicle::_imageReady);
    connect(this, &Vehicle::remoteControlRSSIChanged,   this, &Vehicle::_remoteControlRSSIChanged);
dogmaphobic's avatar
dogmaphobic committed
153 154

    _firmwarePlugin     = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
155
    _autopilotPlugin    = _firmwarePlugin->autopilotPlugin(this);
dogmaphobic's avatar
dogmaphobic committed
156

Jimmy Johnson's avatar
Jimmy Johnson committed
157 158 159
    // connect this vehicle to the follow me handle manager
    connect(this, &Vehicle::flightModeChanged,qgcApp()->toolbox()->followMe(), &FollowMe::followMeHandleManager);

Don Gagne's avatar
Don Gagne committed
160 161 162 163 164
    // PreArm Error self-destruct timer
    connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
    _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
    _prearmErrorTimer.setSingleShot(true);

165 166
    // Connection Lost timer
    _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs);
167 168 169
    _connectionLostTimer.setSingleShot(false);
    _connectionLostTimer.start();
    connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout);
dogmaphobic's avatar
dogmaphobic committed
170

171 172 173 174 175
    // Send MAV_CMD ack timer
    _mavCommandAckTimer.setSingleShot(true);
    _mavCommandAckTimer.setInterval(_mavCommandAckTimeoutMSecs);
    connect(&_mavCommandAckTimer, &QTimer::timeout, this, &Vehicle::_sendMavCommandAgain);

176
    _mav = uas();
177

178
    // Listen for system messages
dogmaphobic's avatar
dogmaphobic committed
179 180
    connect(qgcApp()->toolbox()->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
    connect(qgcApp()->toolbox()->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
181 182 183 184
    // 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)));
    connect(_mav, SIGNAL(statusChanged                      (UASInterface*,QString,QString)),                           this, SLOT(_updateState(UASInterface*, QString,QString)));
185

186
    _loadSettings();
dogmaphobic's avatar
dogmaphobic committed
187

188
    _missionManager = new MissionManager(this);
189 190 191
    connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_newMissionItemsAvailable);

192 193
    _parameterManager = new ParameterManager(this);
    connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
194

195
    // GeoFenceManager needs to access ParameterManager so make sure to create after
196 197
    _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
    connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
198 199 200 201
    connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_newGeoFenceAvailable);

    _rallyPointManager = _firmwarePlugin->newRallyPointManager(this);
    connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);
202

203 204 205 206 207
    // 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
208

209
    _firmwarePlugin->initializeVehicle(this);
dogmaphobic's avatar
dogmaphobic committed
210

211 212
    _sendMultipleTimer.start(_sendMessageMultipleIntraMessageDelay);
    connect(&_sendMultipleTimer, &QTimer::timeout, this, &Vehicle::_sendMessageMultipleNext);
dogmaphobic's avatar
dogmaphobic committed
213

214 215
    _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints);
    connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint);
Don Gagne's avatar
Don Gagne committed
216

217 218 219
    // Invalidate the timer to signal first announce
    _lowBatteryAnnounceTimer.invalidate();

Don Gagne's avatar
Don Gagne committed
220 221 222 223 224 225 226 227 228 229 230
    // 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);

Don Gagne's avatar
Don Gagne committed
231 232
    _addFactGroup(&_gpsFactGroup,       _gpsFactGroupName);
    _addFactGroup(&_batteryFactGroup,   _batteryFactGroupName);
Don Gagne's avatar
Don Gagne committed
233
    _addFactGroup(&_windFactGroup,      _windFactGroupName);
234
    _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
Don Gagne's avatar
Don Gagne committed
235

Don Gagne's avatar
Don Gagne committed
236
    _gpsFactGroup.setVehicle(this);
Don Gagne's avatar
Don Gagne committed
237
    _batteryFactGroup.setVehicle(this);
Don Gagne's avatar
Don Gagne committed
238
    _windFactGroup.setVehicle(this);
239
    _vibrationFactGroup.setVehicle(this);
240 241
}

242 243 244 245 246
// Disconnected Vehicle for offline editing
Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 QObject*                   parent)
247 248 249
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json", parent)
    , _id(0)
    , _active(false)
250
    , _offlineEditingVehicle(true)
251 252
    , _firmwareType(firmwareType)
    , _vehicleType(vehicleType)
253
    , _firmwarePlugin(NULL)
254
    , _firmwarePluginInstanceData(NULL)
255 256 257 258 259 260 261 262 263 264 265 266 267 268 269
    , _autopilotPlugin(NULL)
    , _joystickMode(JoystickModeRC)
    , _joystickEnabled(false)
    , _uas(NULL)
    , _coordinate(37.803784, -122.462276)
    , _coordinateValid(false)
    , _homePositionAvailable(false)
    , _mav(NULL)
    , _currentMessageCount(0)
    , _messageCount(0)
    , _currentErrorCount(0)
    , _currentWarningCount(0)
    , _currentNormalCount(0)
    , _currentMessageType(MessageNone)
    , _updateCount(0)
270 271
    , _rcRSSI(255)
    , _rcRSSIstore(255)
272
    , _autoDisconnect(false)
273 274 275 276 277
    , _flying(false)
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
278 279 280
    , _useGpsRawIntForPosition(true)
    , _useGpsRawIntForAltitude(true)
    , _useAltitudeForAltitude(false)
281 282 283
    , _connectionLost(false)
    , _connectionLostEnabled(true)
    , _missionManager(NULL)
284
    , _missionManagerInitialRequestSent(false)
285
    , _geoFenceManager(NULL)
286 287 288
    , _geoFenceManagerInitialRequestSent(false)
    , _rallyPointManager(NULL)
    , _rallyPointManagerInitialRequestSent(false)
289
    , _parameterManager(NULL)
290 291 292 293
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
    , _nextSendMessageMultipleIndex(0)
294
    , _firmwarePluginManager(firmwarePluginManager)
295 296 297 298 299 300 301 302 303
    , _joystickManager(NULL)
    , _flowImageIndex(0)
    , _allLinksInactiveSent(false)
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
304 305 306
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
307 308 309 310 311 312 313 314 315 316 317 318 319
    , _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)
    , _gpsFactGroup(this)
    , _batteryFactGroup(this)
    , _windFactGroup(this)
    , _vibrationFactGroup(this)
{
320
    _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
321
    _firmwarePlugin->initializeVehicle(this);
322 323 324 325

    _missionManager = new MissionManager(this);
    connect(_missionManager, &MissionManager::error, this, &Vehicle::_missionManagerError);

326
    _parameterManager = new ParameterManager(this);
327

328
    // GeoFenceManager needs to access ParameterManager so make sure to create after
329 330
    _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
    connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
331

332 333 334
    _rallyPointManager = _firmwarePlugin->newRallyPointManager(this);
    connect(_rallyPointManager, &RallyPointManager::error, this, &Vehicle::_rallyPointManagerError);

335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356
    // 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);

    _addFactGroup(&_gpsFactGroup,       _gpsFactGroupName);
    _addFactGroup(&_batteryFactGroup,   _batteryFactGroupName);
    _addFactGroup(&_windFactGroup,      _windFactGroupName);
    _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);

    _gpsFactGroup.setVehicle(NULL);
    _batteryFactGroup.setVehicle(NULL);
    _windFactGroup.setVehicle(NULL);
    _vibrationFactGroup.setVehicle(NULL);
}

357 358
Vehicle::~Vehicle()
{
Don Gagne's avatar
Don Gagne committed
359 360
    qCDebug(VehicleLog) << "~Vehicle" << this;

361 362
    delete _missionManager;
    _missionManager = NULL;
363

364 365 366
    delete _autopilotPlugin;
    _autopilotPlugin = NULL;

367 368
    delete _mav;
    _mav = NULL;
dogmaphobic's avatar
dogmaphobic committed
369

370 371
}

372 373 374 375 376 377 378 379 380 381
void
Vehicle::resetCounters()
{
    _messagesReceived   = 0;
    _messagesSent       = 0;
    _messagesLost       = 0;
    _messageSeq         = 0;
    _heardFrom          = false;
}

382 383
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
384

Don Gagne's avatar
Don Gagne committed
385
    if (message.sysid != _id && message.sysid != 0) {
386 387
        return;
    }
388

389 390 391
    if (!_containsLink(link)) {
        _addLink(link);
    }
dogmaphobic's avatar
dogmaphobic committed
392

393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418
    //-- 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();
        }
    }

Don Gagne's avatar
Don Gagne committed
419
    // Give the plugin a change to adjust the message contents
420 421 422
    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
423

Don Gagne's avatar
Don Gagne committed
424
    switch (message.msgid) {
Don Gagne's avatar
Don Gagne committed
425 426 427 428 429 430 431 432 433 434 435 436
    case MAVLINK_MSG_ID_HOME_POSITION:
        _handleHomePosition(message);
        break;
    case MAVLINK_MSG_ID_HEARTBEAT:
        _handleHeartbeat(message);
        break;
    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
437 438 439 440 441 442
    case MAVLINK_MSG_ID_BATTERY_STATUS:
        _handleBatteryStatus(message);
        break;
    case MAVLINK_MSG_ID_SYS_STATUS:
        _handleSysStatus(message);
        break;
443 444 445 446 447 448 449 450 451 452 453 454
    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;
455 456 457
    case MAVLINK_MSG_ID_VIBRATION:
        _handleVibration(message);
        break;
Don Gagne's avatar
Don Gagne committed
458 459 460
    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        _handleExtendedSysState(message);
        break;
461 462 463
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
464
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
Gus Grubba's avatar
Gus Grubba committed
465
        _handleAutopilotVersion(link, message);
466
        break;
467 468 469
    case MAVLINK_MSG_ID_WIND_COV:
        _handleWindCov(message);
        break;
470 471 472
    case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
        _handleHilActuatorControls(message);
        break;
473 474 475 476 477 478
    case MAVLINK_MSG_ID_LOGGING_DATA:
        _handleMavlinkLoggingData(message);
        break;
    case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
        _handleMavlinkLoggingDataAcked(message);
        break;
479 480 481 482 483 484 485 486 487 488 489 490
    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;
Don Gagne's avatar
Don Gagne committed
491 492 493 494 495 496

    // Following are ArduPilot dialect messages

    case MAVLINK_MSG_ID_WIND:
        _handleWind(message);
        break;
497
    }
dogmaphobic's avatar
dogmaphobic committed
498

Don Gagne's avatar
Don Gagne committed
499
    emit mavlinkMessageReceived(message);
dogmaphobic's avatar
dogmaphobic committed
500

501 502 503
    _uas->receiveMessage(message);
}

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 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562
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);

    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
        if (_useGpsRawIntForPosition) {
            setLatitude(gpsRawInt.lat / (double)1E7);
            setLongitude(gpsRawInt.lon / (double)1E7);
        }
        if (_useGpsRawIntForAltitude) {
            _altitudeRelativeFact.setRawValue(gpsRawInt.alt / 1000.0);
        }
    }

    _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
    _gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? 1e10f : gpsRawInt.eph / 100.0);
    _gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? 1e10f : gpsRawInt.epv / 100.0);
    _gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? 0.0 : gpsRawInt.cog / 100.0);
    _gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type);

    if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
        _setCoordinateValid(true);
    }
}

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

    _useGpsRawIntForPosition = false;
    _useGpsRawIntForAltitude = false;

    setLatitude(globalPositionInt.lat / (double)1E7);
    setLongitude(globalPositionInt.lon / (double)1E7);
    if (!_useAltitudeForAltitude) {
        _altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0);
        _altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0);
    }
}

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

    _useAltitudeForAltitude = true;
    _useGpsRawIntForAltitude = false;
yaoling's avatar
yaoling committed
563 564
    _altitudeRelativeFact.setRawValue(altitude.altitude_relative);
    _altitudeAMSLFact.setRawValue(altitude.altitude_amsl);
565 566 567

}

Gus Grubba's avatar
Gus Grubba committed
568
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
569 570 571 572
{
    mavlink_autopilot_version_t autopilotVersion;
    mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);

Gus Grubba's avatar
Gus Grubba committed
573 574 575 576 577 578
    bool isMavlink2 = (autopilotVersion.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0;
    if(isMavlink2) {
        mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(link->mavlinkChannel());
        mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
    }

579 580 581 582 583 584 585 586
    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
587
        setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
588 589 590
    }
}

591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614
void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
{
    mavlink_hil_actuator_controls_t hil;
    mavlink_msg_hil_actuator_controls_decode(&message, &hil);
    emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
                                    hil.controls[0],
                                    hil.controls[1],
                                    hil.controls[2],
                                    hil.controls[3],
                                    hil.controls[4],
                                    hil.controls[5],
                                    hil.controls[6],
                                    hil.controls[7],
                                    hil.controls[8],
                                    hil.controls[9],
                                    hil.controls[10],
                                    hil.controls[11],
                                    hil.controls[12],
                                    hil.controls[13],
                                    hil.controls[14],
                                    hil.controls[15],
                                    hil.mode);
}

615 616
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
617 618
    bool showError = true;

619 620 621
    mavlink_command_ack_t ack;
    mavlink_msg_command_ack_decode(&message, &ack);

622
    if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) {
Don Gagne's avatar
Don Gagne committed
623
        _mavCommandAckTimer.stop();
624 625 626 627
        showError = _mavCommandQueue[0].showError;
        _mavCommandQueue.removeFirst();
    }

Don Gagne's avatar
Don Gagne committed
628
    emit mavCommandResult(_id, message.compid, ack.command, ack.result, false /* noResponsefromVehicle */);
629 630 631

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

633 634 635 636 637 638 639 640 641 642 643 644 645
        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;
646
        default:
647
            // Do nothing
648
            break;
649
        }
650 651
    }

652
    _sendNextQueuedMavCommand();
653 654
}

Don Gagne's avatar
Don Gagne committed
655 656 657 658 659 660
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) {
Don Gagne's avatar
Don Gagne committed
661
    case MAV_LANDED_STATE_UNDEFINED:
Don Gagne's avatar
Don Gagne committed
662 663 664 665 666 667 668 669 670 671
        break;
    case MAV_LANDED_STATE_ON_GROUND:
        setFlying(false);
        break;
    case MAV_LANDED_STATE_IN_AIR:
        setFlying(true);
        return;
    }
}

672 673 674 675 676 677 678 679 680 681 682 683 684
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);
}

685 686 687 688 689 690 691 692 693 694 695 696 697
void Vehicle::_handleWindCov(mavlink_message_t& message)
{
    mavlink_wind_cov_t wind;
    mavlink_msg_wind_cov_decode(&message, &wind);

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

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

Don Gagne's avatar
Don Gagne committed
698 699 700 701 702 703 704 705 706 707
void Vehicle::_handleWind(mavlink_message_t& message)
{
    mavlink_wind_t wind;
    mavlink_msg_wind_decode(&message, &wind);

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

Don Gagne's avatar
Don Gagne committed
708 709 710 711 712 713 714 715
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 {
716
        // Current is in Amps, current_battery is 10 * milliamperes (1 = 10 milliampere)
717
        _batteryFactGroup.current()->setRawValue((float)sysStatus.current_battery / 100.0f);
Don Gagne's avatar
Don Gagne committed
718 719 720 721 722 723 724
    }
    if (sysStatus.voltage_battery == UINT16_MAX) {
        _batteryFactGroup.voltage()->setRawValue(VehicleBatteryFactGroup::_voltageUnavailable);
    } else {
        _batteryFactGroup.voltage()->setRawValue((double)sysStatus.voltage_battery / 1000.0);
    }
    _batteryFactGroup.percentRemaining()->setRawValue(sysStatus.battery_remaining);
725

726
    if (sysStatus.battery_remaining > 0 && sysStatus.battery_remaining < QGroundControlQmlGlobal::batteryPercentRemainingAnnounce()->rawValue().toInt()) {
727 728
        if (!_lowBatteryAnnounceTimer.isValid() || _lowBatteryAnnounceTimer.elapsed() > _lowBatteryAnnounceRepeatMSecs) {
            _lowBatteryAnnounceTimer.restart();
729
            _say(QString("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining));
730 731
        }
    }
732 733 734 735 736 737 738 739 740 741

    _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
742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772
}

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

Don Gagne's avatar
Don Gagne committed
773 774
void Vehicle::_handleHomePosition(mavlink_message_t& message)
{
Don Gagne's avatar
Don Gagne committed
775 776 777
    bool emitHomePositionChanged =          false;
    bool emitHomePositionAvailableChanged = false;

Don Gagne's avatar
Don Gagne committed
778
    mavlink_home_position_t homePos;
dogmaphobic's avatar
dogmaphobic committed
779

Don Gagne's avatar
Don Gagne committed
780
    mavlink_msg_home_position_decode(&message, &homePos);
Don Gagne's avatar
Don Gagne committed
781 782 783 784

    QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
                                    homePos.longitude / 10000000.0,
                                    homePos.altitude / 1000.0);
Don Gagne's avatar
Don Gagne committed
785
    if (!_homePositionAvailable || newHomePosition != _homePosition) {
Don Gagne's avatar
Don Gagne committed
786 787 788 789 790 791
        emitHomePositionChanged = true;
        _homePosition = newHomePosition;
    }

    if (!_homePositionAvailable) {
        emitHomePositionAvailableChanged = true;
Don Gagne's avatar
Don Gagne committed
792
        _homePositionAvailable = true;
Don Gagne's avatar
Don Gagne committed
793 794 795
    }

    if (emitHomePositionChanged) {
Don Gagne's avatar
Don Gagne committed
796
        qCDebug(VehicleLog) << "New home position" << newHomePosition;
797
        qgcApp()->setLastKnownHomePosition(_homePosition);
Don Gagne's avatar
Don Gagne committed
798 799 800 801 802
        emit homePositionChanged(_homePosition);
    }
    if (emitHomePositionAvailableChanged) {
        emit homePositionAvailableChanged(true);
    }
Don Gagne's avatar
Don Gagne committed
803 804 805 806
}

void Vehicle::_handleHeartbeat(mavlink_message_t& message)
{
807 808
    _connectionActive();

Don Gagne's avatar
Don Gagne committed
809
    mavlink_heartbeat_t heartbeat;
dogmaphobic's avatar
dogmaphobic committed
810

Don Gagne's avatar
Don Gagne committed
811
    mavlink_msg_heartbeat_decode(&message, &heartbeat);
dogmaphobic's avatar
dogmaphobic committed
812

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

Don Gagne's avatar
Don Gagne committed
815 816 817
    if (_armed != newArmed) {
        _armed = newArmed;
        emit armedChanged(_armed);
dogmaphobic's avatar
dogmaphobic committed
818

819 820 821 822 823 824
        // We are transitioning to the armed state, begin tracking trajectory points for the map
        if (_armed) {
            _mapTrajectoryStart();
        } else {
            _mapTrajectoryStop();
        }
Don Gagne's avatar
Don Gagne committed
825 826 827 828 829 830 831 832 833
    }

    if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) {
        _base_mode = heartbeat.base_mode;
        _custom_mode = heartbeat.custom_mode;
        emit flightModeChanged(flightMode());
    }
}

Don Gagne's avatar
Don Gagne committed
834 835 836 837 838 839 840 841 842 843 844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898
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
899 900 901 902
    for (int i=0; i<cMaxRcChannels; i++) {
        pwmValues[i] = -1;
    }

Don Gagne's avatar
Don Gagne committed
903 904 905 906 907 908
    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
909
            channelCount = i + 1;
Don Gagne's avatar
Don Gagne committed
910 911 912 913 914 915 916 917 918 919 920
            pwmValues[i] = channelValue;
        }
    }
    for (int i=9; i<18; i++) {
        pwmValues[i] = -1;
    }

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

921 922
bool Vehicle::_containsLink(LinkInterface* link)
{
Don Gagne's avatar
Don Gagne committed
923
    return _links.contains(link);
924 925 926 927 928 929
}

void Vehicle::_addLink(LinkInterface* link)
{
    if (!_containsLink(link)) {
        qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
930 931
        _links += link;
        _updatePriorityLink();
Don Gagne's avatar
Don Gagne committed
932 933
        connect(qgcApp()->toolbox()->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
        connect(qgcApp()->toolbox()->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
934 935 936
    }
}

Don Gagne's avatar
Don Gagne committed
937
void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
938
{
Don Gagne's avatar
Don Gagne committed
939
    qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
dogmaphobic's avatar
dogmaphobic committed
940

Don Gagne's avatar
Don Gagne committed
941
    _links.removeOne(link);
942
    _updatePriorityLink();
dogmaphobic's avatar
dogmaphobic committed
943

Don Gagne's avatar
Don Gagne committed
944
    if (_links.count() == 0 && !_allLinksInactiveSent) {
Don Gagne's avatar
Don Gagne committed
945
        qCDebug(VehicleLog) << "All links inactive";
Don Gagne's avatar
Don Gagne committed
946 947 948
        // Make sure to not send this more than one time
        _allLinksInactiveSent = true;
        emit allLinksInactive(this);
949 950 951
    }
}

952 953 954 955 956 957 958 959 960 961 962 963 964
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)
{
965 966 967 968 969
    // Make sure this is still a good link
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return;
    }

970 971 972 973 974 975
#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

976
    // Give the plugin a chance to adjust
977
    _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message);
Don Gagne's avatar
Don Gagne committed
978 979 980 981 982 983

    // 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);
984 985
    _messagesSent++;
    emit messagesSentChanged();
986 987
}

988
void Vehicle::_updatePriorityLink(void)
989
{
Gus Grubba's avatar
Gus Grubba committed
990
#ifndef NO_SERIAL_LINK
991 992 993 994 995 996 997
    LinkInterface* newPriorityLink = NULL;

    // Note that this routine specificallty does not clear _priorityLink when there are no links remaining.
    // By doing this we hold a reference on the last link as the Vehicle shuts down. Thus preventing shutdown
    // ordering NULL pointer crashes where priorityLink() is still called during shutdown sequence.
    for (int i=0; i<_links.count(); i++) {
        LinkInterface* link = _links[i];
998 999 1000
        if (link->isConnected()) {
            SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
            if (pSerialLink) {
1001 1002 1003
                LinkConfiguration* config = pSerialLink->getLinkConfiguration();
                if (config) {
                    SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(config);
1004
                    if (pSerialConfig && pSerialConfig->usbDirect()) {
1005 1006 1007 1008 1009
                        if (_priorityLink.data() != link) {
                            newPriorityLink = link;
                            break;
                        }
                        return;
1010 1011 1012
                    }
                }
            }
1013 1014
        }
    }
1015 1016 1017 1018 1019 1020 1021 1022

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

    if (newPriorityLink) {
        _priorityLink = qgcApp()->toolbox()->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
    }
dogmaphobic's avatar
dogmaphobic committed
1023
#endif
1024 1025
}

1026 1027
void Vehicle::setLatitude(double latitude)
{
1028 1029
    _coordinate.setLatitude(latitude);
    emit coordinateChanged(_coordinate);
1030 1031 1032
}

void Vehicle::setLongitude(double longitude){
1033 1034
    _coordinate.setLongitude(longitude);
    emit coordinateChanged(_coordinate);
1035 1036
}

1037 1038
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
1039
    if (qIsInf(roll)) {
Don Gagne's avatar
Don Gagne committed
1040
        _rollFact.setRawValue(0);
1041
    } else {
Don Gagne's avatar
Don Gagne committed
1042
        _rollFact.setRawValue(roll * (180.0 / M_PI));
1043
    }
1044
    if (qIsInf(pitch)) {
Don Gagne's avatar
Don Gagne committed
1045
        _pitchFact.setRawValue(0);
1046
    } else {
Don Gagne's avatar
Don Gagne committed
1047
        _pitchFact.setRawValue(pitch * (180.0 / M_PI));
1048
    }
1049
    if (qIsInf(yaw)) {
Don Gagne's avatar
Don Gagne committed
1050
        _headingFact.setRawValue(0);
1051
    } else {
Don Gagne's avatar
Don Gagne committed
1052
        yaw = yaw * (180.0 / M_PI);
1053
        if (yaw < 0) yaw += 360;
Don Gagne's avatar
Don Gagne committed
1054
        _headingFact.setRawValue(yaw);
1055 1056 1057 1058 1059 1060 1061 1062
    }
}

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
1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093
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);
}

1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106
/*
 * 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
1107 1108 1109 1110 1111 1112 1113 1114 1115
QString Vehicle::formatedMessages()
{
    QString messages;
    foreach(UASMessage* message, qgcApp()->toolbox()->uasMessageHandler()->messages()) {
        messages += message->getFormatedText();
    }
    return messages;
}

dogmaphobic's avatar
dogmaphobic committed
1116 1117 1118 1119 1120
void Vehicle::clearMessages()
{
    qgcApp()->toolbox()->uasMessageHandler()->clearMessages();
}

dogmaphobic's avatar
dogmaphobic committed
1121 1122 1123 1124 1125 1126 1127 1128 1129
void Vehicle::_handletextMessageReceived(UASMessage* message)
{
    if(message)
    {
        _formatedMessage = message->getFormatedText();
        emit formatedMessageChanged();
    }
}

1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152
void Vehicle::_updateState(UASInterface*, QString name, QString)
{
    if (_currentState != name) {
        _currentState = name;
        emit currentStateChanged();
    }
}

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
1153

1154
    UASMessageHandler* pMh = qgcApp()->toolbox()->uasMessageHandler();
1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172 1173 1174 1175 1176 1177 1178 1179 1180 1181 1182 1183 1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220
    Q_ASSERT(pMh);
    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();
    }
}
1221 1222 1223 1224 1225 1226 1227 1228 1229

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

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

1231
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1232

1233
    bool convertOk;
dogmaphobic's avatar
dogmaphobic committed
1234

1235 1236 1237 1238
    _joystickMode = (JoystickMode_t)settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk);
    if (!convertOk) {
        _joystickMode = JoystickModeRC;
    }
dogmaphobic's avatar
dogmaphobic committed
1239

1240 1241 1242 1243
    // Joystick enabled is a global setting so first make sure there are any joysticks connected
    if (qgcApp()->toolbox()->joystickManager()->joysticks().count()) {
        _joystickEnabled = settings.value(_joystickEnabledSettingsKey, false).toBool();
    }
1244 1245 1246 1247 1248
}

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

1250
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1251

1252
    settings.setValue(_joystickModeSettingsKey, _joystickMode);
1253 1254 1255 1256 1257 1258

    // The joystick enabled setting should only be changed if a joystick is present
    // since the checkbox can only be clicked if one is present
    if (qgcApp()->toolbox()->joystickManager()->joysticks().count()) {
        settings.setValue(_joystickEnabledSettingsKey, _joystickEnabled);
    }
1259 1260 1261 1262 1263 1264 1265 1266 1267 1268 1269 1270 1271
}

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
1272

1273 1274 1275 1276 1277 1278 1279 1280
    _joystickMode = (JoystickMode_t)mode;
    _saveSettings();
    emit joystickModeChanged(mode);
}

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

1282
    list << "Normal" << "Attitude" << "Position" << "Force" << "Velocity";
dogmaphobic's avatar
dogmaphobic committed
1283

1284 1285
    return list;
}
1286 1287 1288 1289 1290 1291 1292 1293

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

void Vehicle::setJoystickEnabled(bool enabled)
{
1294
    _joystickEnabled = enabled;
1295 1296
    _startJoystick(_joystickEnabled);
    _saveSettings();
1297
    emit joystickEnabledChanged(_joystickEnabled);
1298 1299 1300 1301
}

void Vehicle::_startJoystick(bool start)
{
1302
    Joystick* joystick = _joystickManager->activeJoystick();
1303 1304 1305 1306 1307 1308 1309 1310 1311 1312 1313 1314 1315 1316 1317 1318 1319 1320 1321
    if (joystick) {
        if (start) {
            if (_joystickEnabled) {
                joystick->startPolling(this);
            }
        } else {
            joystick->stopPolling();
        }
    }
}

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

void Vehicle::setActive(bool active)
{
    _active = active;
dogmaphobic's avatar
dogmaphobic committed
1322

1323 1324
    _startJoystick(_active);
}
1325

1326 1327 1328 1329 1330 1331 1332 1333 1334
bool Vehicle::homePositionAvailable(void)
{
    return _homePositionAvailable;
}

QGeoCoordinate Vehicle::homePosition(void)
{
    return _homePosition;
}
Don Gagne's avatar
Don Gagne committed
1335 1336 1337 1338

void Vehicle::setArmed(bool armed)
{
    // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks.
1339 1340 1341 1342
    sendMavCommand(defaultComponentId(),
                   MAV_CMD_COMPONENT_ARM_DISARM,
                   true,    // show error if fails
                   armed ? 1.0f : 0.0f);
Don Gagne's avatar
Don Gagne committed
1343 1344 1345 1346
}

bool Vehicle::flightModeSetAvailable(void)
{
1347
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
Don Gagne's avatar
Don Gagne committed
1348 1349 1350 1351
}

QStringList Vehicle::flightModes(void)
{
Daniel Agar's avatar
Daniel Agar committed
1352
    return _firmwarePlugin->flightModes(this);
Don Gagne's avatar
Don Gagne committed
1353 1354
}

Don Gagne's avatar
Don Gagne committed
1355
QString Vehicle::flightMode(void) const
Don Gagne's avatar
Don Gagne committed
1356 1357 1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369 1370 1371
{
    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;
1372 1373 1374 1375 1376 1377 1378 1379
        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
1380
    } else {
Don Gagne's avatar
Don Gagne committed
1381
        qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;
Don Gagne's avatar
Don Gagne committed
1382 1383 1384 1385 1386 1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398
    }
}

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

1399 1400 1401 1402 1403 1404 1405 1406
    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
1407
}
1408

1409
void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
1410 1411 1412
{
    mavlink_message_t               msg;
    mavlink_request_data_stream_t   dataStream;
dogmaphobic's avatar
dogmaphobic committed
1413

1414 1415 1416 1417
    dataStream.req_stream_id = stream;
    dataStream.req_message_rate = rate;
    dataStream.start_stop = 1;  // start
    dataStream.target_system = id();
1418
    dataStream.target_component = defaultComponentId();
dogmaphobic's avatar
dogmaphobic committed
1419

1420 1421 1422 1423 1424
    mavlink_msg_request_data_stream_encode_chan(_mavlink->getSystemId(),
                                                _mavlink->getComponentId(),
                                                priorityLink()->mavlinkChannel(),
                                                &msg,
                                                &dataStream);
1425

1426 1427 1428 1429
    if (sendMultiple) {
        // We use sendMessageMultiple since we really want these to make it to the vehicle
        sendMessageMultiple(msg);
    } else {
1430
        sendMessageOnLink(priorityLink(), msg);
1431
    }
1432 1433 1434 1435 1436 1437
}

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

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

1441 1442 1443 1444 1445 1446
        if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) {
            _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex);
        } else {
            _nextSendMessageMultipleIndex++;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
1447

1448 1449 1450 1451 1452 1453 1454 1455
    if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) {
        _nextSendMessageMultipleIndex = 0;
    }
}

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

1457 1458
    info.message =      message;
    info.retryCount =   _sendMessageMultipleRetries;
dogmaphobic's avatar
dogmaphobic committed
1459

1460 1461
    _sendMessageMultipleList.append(info);
}
1462 1463 1464 1465

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

1469 1470 1471
void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
1472 1473 1474 1475 1476 1477 1478
    qgcApp()->showMessage(QString("Error during GeoFence communication with Vehicle: %1").arg(errorMsg));
}

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

1481 1482 1483
void Vehicle::_addNewMapTrajectoryPoint(void)
{
    if (_mapTrajectoryHaveFirstCoordinate) {
1484 1485 1486 1487
        // Keep three minutes of trajectory
        if (_mapTrajectoryList.count() * _mapTrajectoryMsecsBetweenPoints > 3 * 1000 * 60) {
            _mapTrajectoryList.removeAt(0)->deleteLater();
        }
1488
        _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this));
1489 1490
    }
    _mapTrajectoryHaveFirstCoordinate = true;
1491
    _mapTrajectoryLastCoordinate = _coordinate;
1492 1493 1494 1495 1496 1497 1498 1499 1500 1501 1502 1503 1504
}

void Vehicle::_mapTrajectoryStart(void)
{
    _mapTrajectoryHaveFirstCoordinate = false;
    _mapTrajectoryList.clear();
    _mapTrajectoryTimer.start();
}

void Vehicle::_mapTrajectoryStop()
{
    _mapTrajectoryTimer.stop();
}
1505 1506 1507

void Vehicle::_parametersReady(bool parametersReady)
{
1508 1509
    if (parametersReady && !_missionManagerInitialRequestSent) {
        _missionManagerInitialRequestSent = true;
1510 1511
        _missionManager->requestMissionItems();
    }
Lorenz Meier's avatar
Lorenz Meier committed
1512 1513 1514 1515

    if (parametersReady) {
        setJoystickEnabled(_joystickEnabled);
    }
1516
}
1517

Don Gagne's avatar
Don Gagne committed
1518
void Vehicle::disconnectInactiveVehicle(void)
1519
{
Don Gagne's avatar
Don Gagne committed
1520
    // Vehicle is no longer communicating with us, disconnect all links
1521

1522

1523
    LinkManager* linkMgr = qgcApp()->toolbox()->linkManager();
1524
    for (int i=0; i<_links.count(); i++) {