Vehicle.cc 76.7 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 16

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

QGC_LOGGING_CATEGORY(VehicleLog, "VehicleLog")

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

37 38
extern const char* guided_mode_not_supported_by_vehicle;

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

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

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

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

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

146
    _mavlink = qgcApp()->toolbox()->mavlinkProtocol();
dogmaphobic's avatar
dogmaphobic committed
147

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

150
    connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
Don Gagne's avatar
Don Gagne committed
151
    connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
152
    connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
153
    _uas = new UAS(_mavlink, this, _firmwarePluginManager);
dogmaphobic's avatar
dogmaphobic committed
154

155 156
    setLatitude(_uas->getLatitude());
    setLongitude(_uas->getLongitude());
dogmaphobic's avatar
dogmaphobic committed
157

Don Gagne's avatar
Don Gagne committed
158 159 160 161
    connect(_uas, &UAS::latitudeChanged,                this, &Vehicle::setLatitude);
    connect(_uas, &UAS::longitudeChanged,               this, &Vehicle::setLongitude);
    connect(_uas, &UAS::imageReady,                     this, &Vehicle::_imageReady);
    connect(this, &Vehicle::remoteControlRSSIChanged,   this, &Vehicle::_remoteControlRSSIChanged);
dogmaphobic's avatar
dogmaphobic committed
162 163 164 165

    _firmwarePlugin     = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
    _autopilotPlugin    = _autopilotPluginManager->newAutopilotPluginForVehicle(this);

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

169
    // Refresh timer
170
    connect(_refreshTimer, &QTimer::timeout, this, &Vehicle::_checkUpdate);
171 172
    _refreshTimer->setInterval(UPDATE_TIMER);
    _refreshTimer->start(UPDATE_TIMER);
173

Don Gagne's avatar
Don Gagne committed
174 175 176 177 178
    // PreArm Error self-destruct timer
    connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
    _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
    _prearmErrorTimer.setSingleShot(true);

179 180 181 182 183
    // Connection Lost time
    _connectionLostTimer.setInterval(Vehicle::_connectionLostTimeoutMSecs);
    _connectionLostTimer.setSingleShot(false);
    _connectionLostTimer.start();
    connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout);
dogmaphobic's avatar
dogmaphobic committed
184

185
    _mav = uas();
186

187
    // Listen for system messages
dogmaphobic's avatar
dogmaphobic committed
188 189
    connect(qgcApp()->toolbox()->uasMessageHandler(), &UASMessageHandler::textMessageCountChanged,  this, &Vehicle::_handleTextMessage);
    connect(qgcApp()->toolbox()->uasMessageHandler(), &UASMessageHandler::textMessageReceived,      this, &Vehicle::_handletextMessageReceived);
190 191 192 193
    // 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)));
194 195 196 197

    connect(_mav, &UASInterface::speedChanged, this, &Vehicle::_updateSpeed);
    connect(_mav, &UASInterface::altitudeChanged, this, &Vehicle::_updateAltitude);
    connect(_mav, &UASInterface::navigationControllerErrorsChanged,this, &Vehicle::_updateNavigationControllerErrors);
198
    connect(_mav, &UASInterface::NavigationControllerDataChanged,   this, &Vehicle::_updateNavigationControllerData);
dogmaphobic's avatar
dogmaphobic committed
199

200
    _loadSettings();
dogmaphobic's avatar
dogmaphobic committed
201

202
    _missionManager = new MissionManager(this);
203 204 205
    connect(_missionManager, &MissionManager::error,                    this, &Vehicle::_missionManagerError);
    connect(_missionManager, &MissionManager::newMissionItemsAvailable, this, &Vehicle::_newMissionItemsAvailable);

206 207
    _parameterManager = new ParameterManager(this);
    connect(_parameterManager, &ParameterManager::parametersReadyChanged, this, &Vehicle::_parametersReady);
208

209
    // GeoFenceManager needs to access ParameterManager so make sure to create after
210 211
    _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
    connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
212 213 214 215
    connect(_geoFenceManager, &GeoFenceManager::loadComplete, this, &Vehicle::_newGeoFenceAvailable);

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

217
    // Ask the vehicle for firmware version info. This must be MAV_COMP_ID_ALL since we don't know default component id yet.
218 219 220 221 222 223 224 225 226 227

    mavlink_message_t       versionMsg;
    mavlink_command_long_t  versionCmd;

    versionCmd.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
    versionCmd.confirmation = 0;
    versionCmd.param1 = 1; // Request firmware version
    versionCmd.param2 = versionCmd.param3 = versionCmd.param4 = versionCmd.param5 = versionCmd.param6 = versionCmd.param7 = 0;
    versionCmd.target_system = id();
    versionCmd.target_component = MAV_COMP_ID_ALL;
228 229 230 231 232
    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                         _mavlink->getComponentId(),
                                         priorityLink()->mavlinkChannel(),
                                         &versionMsg,
                                         &versionCmd);
233
    sendMessageMultiple(versionMsg);
234

235
    _firmwarePlugin->initializeVehicle(this);
dogmaphobic's avatar
dogmaphobic committed
236

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

240 241
    _mapTrajectoryTimer.setInterval(_mapTrajectoryMsecsBetweenPoints);
    connect(&_mapTrajectoryTimer, &QTimer::timeout, this, &Vehicle::_addNewMapTrajectoryPoint);
Don Gagne's avatar
Don Gagne committed
242

243 244 245
    // Invalidate the timer to signal first announce
    _lowBatteryAnnounceTimer.invalidate();

Don Gagne's avatar
Don Gagne committed
246 247 248 249 250 251 252 253 254 255 256
    // 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
257 258
    _addFactGroup(&_gpsFactGroup,       _gpsFactGroupName);
    _addFactGroup(&_batteryFactGroup,   _batteryFactGroupName);
Don Gagne's avatar
Don Gagne committed
259
    _addFactGroup(&_windFactGroup,      _windFactGroupName);
260
    _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
Don Gagne's avatar
Don Gagne committed
261

Don Gagne's avatar
Don Gagne committed
262
    _gpsFactGroup.setVehicle(this);
Don Gagne's avatar
Don Gagne committed
263
    _batteryFactGroup.setVehicle(this);
Don Gagne's avatar
Don Gagne committed
264
    _windFactGroup.setVehicle(this);
265
    _vibrationFactGroup.setVehicle(this);
266 267
}

268 269 270 271 272
// Disconnected Vehicle for offline editing
Vehicle::Vehicle(MAV_AUTOPILOT              firmwareType,
                 MAV_TYPE                   vehicleType,
                 FirmwarePluginManager*     firmwarePluginManager,
                 QObject*                   parent)
273 274 275
    : FactGroup(_vehicleUIUpdateRateMSecs, ":/json/Vehicle/VehicleFact.json", parent)
    , _id(0)
    , _active(false)
276
    , _offlineEditingVehicle(true)
277 278
    , _firmwareType(firmwareType)
    , _vehicleType(vehicleType)
279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299
    , _firmwarePlugin(NULL)
    , _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)
    , _navigationAltitudeError(0.0f)
    , _navigationSpeedError(0.0f)
    , _navigationCrosstrackError(0.0f)
    , _navigationTargetBearing(0.0f)
    , _refreshTimer(new QTimer(this))
    , _updateCount(0)
300 301
    , _rcRSSI(255)
    , _rcRSSIstore(255)
302
    , _autoDisconnect(false)
303 304 305 306 307
    , _flying(false)
    , _onboardControlSensorsPresent(0)
    , _onboardControlSensorsEnabled(0)
    , _onboardControlSensorsHealth(0)
    , _onboardControlSensorsUnhealthy(0)
308 309 310
    , _connectionLost(false)
    , _connectionLostEnabled(true)
    , _missionManager(NULL)
311
    , _missionManagerInitialRequestSent(false)
312
    , _geoFenceManager(NULL)
313 314 315
    , _geoFenceManagerInitialRequestSent(false)
    , _rallyPointManager(NULL)
    , _rallyPointManagerInitialRequestSent(false)
316
    , _parameterManager(NULL)
317 318 319 320
    , _armed(false)
    , _base_mode(0)
    , _custom_mode(0)
    , _nextSendMessageMultipleIndex(0)
321
    , _firmwarePluginManager(firmwarePluginManager)
322 323 324 325 326 327 328 329 330 331
    , _autopilotPluginManager(NULL)
    , _joystickManager(NULL)
    , _flowImageIndex(0)
    , _allLinksInactiveSent(false)
    , _messagesReceived(0)
    , _messagesSent(0)
    , _messagesLost(0)
    , _messageSeq(0)
    , _compID(0)
    , _heardFrom(false)
332 333 334
    , _firmwareMajorVersion(versionNotSetValue)
    , _firmwareMinorVersion(versionNotSetValue)
    , _firmwarePatchVersion(versionNotSetValue)
335 336 337 338 339 340 341 342 343 344 345 346 347
    , _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)
{
348
    _firmwarePlugin = _firmwarePluginManager->firmwarePluginForAutopilot(_firmwareType, _vehicleType);
349
    _firmwarePlugin->initializeVehicle(this);
350 351 352 353

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

354
    _parameterManager = new ParameterManager(this);
355

356
    // GeoFenceManager needs to access ParameterManager so make sure to create after
357 358
    _geoFenceManager = _firmwarePlugin->newGeoFenceManager(this);
    connect(_geoFenceManager, &GeoFenceManager::error, this, &Vehicle::_geoFenceManagerError);
359

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

363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384
    // 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);
}

385 386
Vehicle::~Vehicle()
{
Don Gagne's avatar
Don Gagne committed
387 388
    qCDebug(VehicleLog) << "~Vehicle" << this;

389 390
    delete _missionManager;
    _missionManager = NULL;
391

392 393 394
    delete _autopilotPlugin;
    _autopilotPlugin = NULL;

395 396
    delete _mav;
    _mav = NULL;
dogmaphobic's avatar
dogmaphobic committed
397

398 399
}

400 401 402 403 404 405 406 407 408 409
void
Vehicle::resetCounters()
{
    _messagesReceived   = 0;
    _messagesSent       = 0;
    _messagesLost       = 0;
    _messageSeq         = 0;
    _heardFrom          = false;
}

410 411
void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
{
412

Don Gagne's avatar
Don Gagne committed
413
    if (message.sysid != _id && message.sysid != 0) {
414 415
        return;
    }
416

417 418 419
    if (!_containsLink(link)) {
        _addLink(link);
    }
dogmaphobic's avatar
dogmaphobic committed
420

421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446
    //-- 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
447
    // Give the plugin a change to adjust the message contents
448 449 450
    if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) {
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
451

Don Gagne's avatar
Don Gagne committed
452
    switch (message.msgid) {
Don Gagne's avatar
Don Gagne committed
453 454 455 456 457 458 459 460 461 462 463 464
    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
465 466 467 468 469 470
    case MAVLINK_MSG_ID_BATTERY_STATUS:
        _handleBatteryStatus(message);
        break;
    case MAVLINK_MSG_ID_SYS_STATUS:
        _handleSysStatus(message);
        break;
471 472 473 474 475 476 477 478 479 480 481 482
    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;
483 484 485
    case MAVLINK_MSG_ID_VIBRATION:
        _handleVibration(message);
        break;
Don Gagne's avatar
Don Gagne committed
486 487 488
    case MAVLINK_MSG_ID_EXTENDED_SYS_STATE:
        _handleExtendedSysState(message);
        break;
489 490 491
    case MAVLINK_MSG_ID_COMMAND_ACK:
        _handleCommandAck(message);
        break;
492
    case MAVLINK_MSG_ID_AUTOPILOT_VERSION:
Gus Grubba's avatar
Gus Grubba committed
493
        _handleAutopilotVersion(link, message);
494
        break;
495 496 497
    case MAVLINK_MSG_ID_WIND_COV:
        _handleWindCov(message);
        break;
498 499 500
    case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
        _handleHilActuatorControls(message);
        break;
501 502 503 504 505 506
    case MAVLINK_MSG_ID_LOGGING_DATA:
        _handleMavlinkLoggingData(message);
        break;
    case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
        _handleMavlinkLoggingDataAcked(message);
        break;
Don Gagne's avatar
Don Gagne committed
507 508 509 510 511 512

    // Following are ArduPilot dialect messages

    case MAVLINK_MSG_ID_WIND:
        _handleWind(message);
        break;
513
    }
dogmaphobic's avatar
dogmaphobic committed
514

Don Gagne's avatar
Don Gagne committed
515
    emit mavlinkMessageReceived(message);
dogmaphobic's avatar
dogmaphobic committed
516

517 518 519
    _uas->receiveMessage(message);
}

Gus Grubba's avatar
Gus Grubba committed
520
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
521 522 523 524
{
    mavlink_autopilot_version_t autopilotVersion;
    mavlink_msg_autopilot_version_decode(&message, &autopilotVersion);

Gus Grubba's avatar
Gus Grubba committed
525 526 527 528 529 530
    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;
    }

531 532 533 534 535 536 537 538
    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
539
        setFirmwareVersion(majorVersion, minorVersion, patchVersion, versionType);
540 541 542
    }
}

543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566
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);
}

567 568 569 570 571 572 573
void Vehicle::_handleCommandAck(mavlink_message_t& message)
{
    mavlink_command_ack_t ack;
    mavlink_msg_command_ack_decode(&message, &ack);

    emit commandLongAck(message.compid, ack.command, ack.result);

574 575 576 577 578 579 580 581
    // Disregard failures for these (handled above)
    switch (ack.command) {
        case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
        case MAV_CMD_LOGGING_START:
        case MAV_CMD_LOGGING_STOP:
            return;
        default:
            break;
582 583
    }

584
    QString commandName = qgcApp()->toolbox()->missionCommandTree()->friendlyName((MAV_CMD)ack.command);
585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604

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

Don Gagne's avatar
Don Gagne committed
605 606 607 608 609 610
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
611
    case MAV_LANDED_STATE_UNDEFINED:
Don Gagne's avatar
Don Gagne committed
612 613 614 615 616 617 618 619 620 621
        break;
    case MAV_LANDED_STATE_ON_GROUND:
        setFlying(false);
        break;
    case MAV_LANDED_STATE_IN_AIR:
        setFlying(true);
        return;
    }
}

622 623 624 625 626 627 628 629 630 631 632 633 634
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);
}

635 636 637 638 639 640 641 642 643 644 645 646 647
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
648 649 650 651 652 653 654 655 656 657
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
658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673
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 {
        _batteryFactGroup.current()->setRawValue((double)sysStatus.current_battery * 10);
    }
    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);
674

675
    if (sysStatus.battery_remaining > 0 && sysStatus.battery_remaining < QGroundControlQmlGlobal::batteryPercentRemainingAnnounce()->rawValue().toInt()) {
676 677
        if (!_lowBatteryAnnounceTimer.isValid() || _lowBatteryAnnounceTimer.elapsed() > _lowBatteryAnnounceRepeatMSecs) {
            _lowBatteryAnnounceTimer.restart();
678
            _say(QString("%1 low battery: %2 percent remaining").arg(_vehicleIdSpeech()).arg(sysStatus.battery_remaining));
679 680
        }
    }
681 682 683 684 685 686 687 688 689 690

    _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
691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721
}

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
722 723
void Vehicle::_handleHomePosition(mavlink_message_t& message)
{
Don Gagne's avatar
Don Gagne committed
724 725 726
    bool emitHomePositionChanged =          false;
    bool emitHomePositionAvailableChanged = false;

Don Gagne's avatar
Don Gagne committed
727
    mavlink_home_position_t homePos;
dogmaphobic's avatar
dogmaphobic committed
728

Don Gagne's avatar
Don Gagne committed
729
    mavlink_msg_home_position_decode(&message, &homePos);
Don Gagne's avatar
Don Gagne committed
730 731 732 733

    QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
                                    homePos.longitude / 10000000.0,
                                    homePos.altitude / 1000.0);
Don Gagne's avatar
Don Gagne committed
734
    if (!_homePositionAvailable || newHomePosition != _homePosition) {
Don Gagne's avatar
Don Gagne committed
735 736 737 738 739 740
        emitHomePositionChanged = true;
        _homePosition = newHomePosition;
    }

    if (!_homePositionAvailable) {
        emitHomePositionAvailableChanged = true;
Don Gagne's avatar
Don Gagne committed
741
        _homePositionAvailable = true;
Don Gagne's avatar
Don Gagne committed
742 743 744
    }

    if (emitHomePositionChanged) {
Don Gagne's avatar
Don Gagne committed
745
        qCDebug(VehicleLog) << "New home position" << newHomePosition;
746
        qgcApp()->setLastKnownHomePosition(_homePosition);
Don Gagne's avatar
Don Gagne committed
747 748 749 750 751
        emit homePositionChanged(_homePosition);
    }
    if (emitHomePositionAvailableChanged) {
        emit homePositionAvailableChanged(true);
    }
Don Gagne's avatar
Don Gagne committed
752 753 754 755
}

void Vehicle::_handleHeartbeat(mavlink_message_t& message)
{
756 757
    _connectionActive();

Don Gagne's avatar
Don Gagne committed
758
    mavlink_heartbeat_t heartbeat;
dogmaphobic's avatar
dogmaphobic committed
759

Don Gagne's avatar
Don Gagne committed
760
    mavlink_msg_heartbeat_decode(&message, &heartbeat);
dogmaphobic's avatar
dogmaphobic committed
761

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

Don Gagne's avatar
Don Gagne committed
764 765 766
    if (_armed != newArmed) {
        _armed = newArmed;
        emit armedChanged(_armed);
dogmaphobic's avatar
dogmaphobic committed
767

768 769 770 771 772 773
        // 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
774 775 776 777 778 779 780 781 782
    }

    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
783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823 824 825 826 827 828 829 830 831 832 833 834 835 836 837 838 839 840 841 842 843 844 845 846 847
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
848 849 850 851
    for (int i=0; i<cMaxRcChannels; i++) {
        pwmValues[i] = -1;
    }

Don Gagne's avatar
Don Gagne committed
852 853 854 855 856 857
    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
858
            channelCount = i + 1;
Don Gagne's avatar
Don Gagne committed
859 860 861 862 863 864 865 866 867 868 869
            pwmValues[i] = channelValue;
        }
    }
    for (int i=9; i<18; i++) {
        pwmValues[i] = -1;
    }

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

870 871
bool Vehicle::_containsLink(LinkInterface* link)
{
Don Gagne's avatar
Don Gagne committed
872
    return _links.contains(link);
873 874 875 876 877
}

void Vehicle::_addLink(LinkInterface* link)
{
    if (!_containsLink(link)) {
Don Gagne's avatar
Don Gagne committed
878
        _links += link;
879
        qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
Don Gagne's avatar
Don Gagne committed
880 881
        connect(qgcApp()->toolbox()->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
        connect(qgcApp()->toolbox()->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
882 883 884
    }
}

Don Gagne's avatar
Don Gagne committed
885
void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
886
{
Don Gagne's avatar
Don Gagne committed
887
    qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
dogmaphobic's avatar
dogmaphobic committed
888

Don Gagne's avatar
Don Gagne committed
889
    _links.removeOne(link);
dogmaphobic's avatar
dogmaphobic committed
890

Don Gagne's avatar
Don Gagne committed
891
    if (_links.count() == 0 && !_allLinksInactiveSent) {
Don Gagne's avatar
Don Gagne committed
892
        qCDebug(VehicleLog) << "All links inactive";
Don Gagne's avatar
Don Gagne committed
893 894 895
        // Make sure to not send this more than one time
        _allLinksInactiveSent = true;
        emit allLinksInactive(this);
896 897 898
    }
}

899 900 901 902 903 904 905 906 907 908 909 910 911
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)
{
912 913 914 915 916
    // Make sure this is still a good link
    if (!link || !_links.contains(link) || !link->isConnected()) {
        return;
    }

917 918 919 920 921 922
#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

923
    // Give the plugin a chance to adjust
924
    _firmwarePlugin->adjustOutgoingMavlinkMessage(this, link, &message);
Don Gagne's avatar
Don Gagne committed
925 926 927 928 929 930

    // 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);
931 932
    _messagesSent++;
    emit messagesSentChanged();
933 934 935 936 937
}

/// @return Direct usb connection link to board if one, NULL if none
LinkInterface* Vehicle::priorityLink(void)
{
dogmaphobic's avatar
dogmaphobic committed
938
#ifndef __ios__
939 940 941 942 943 944 945 946 947 948 949 950
    foreach (LinkInterface* link, _links) {
        if (link->isConnected()) {
            SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
            if (pSerialLink) {
                LinkConfiguration* pLinkConfig = pSerialLink->getLinkConfiguration();
                if (pLinkConfig) {
                    SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(pLinkConfig);
                    if (pSerialConfig && pSerialConfig->usbDirect()) {
                        return link;
                    }
                }
            }
951 952
        }
    }
dogmaphobic's avatar
dogmaphobic committed
953
#endif
954
    return _links.count() ? _links[0] : NULL;
955 956
}

957 958
void Vehicle::setLatitude(double latitude)
{
959 960
    _coordinate.setLatitude(latitude);
    emit coordinateChanged(_coordinate);
961 962 963
}

void Vehicle::setLongitude(double longitude){
964 965
    _coordinate.setLongitude(longitude);
    emit coordinateChanged(_coordinate);
966 967
}

968 969
void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64)
{
970
    if (qIsInf(roll)) {
Don Gagne's avatar
Don Gagne committed
971
        _rollFact.setRawValue(0);
972
    } else {
Don Gagne's avatar
Don Gagne committed
973
        _rollFact.setRawValue(roll * (180.0 / M_PI));
974
    }
975
    if (qIsInf(pitch)) {
Don Gagne's avatar
Don Gagne committed
976
        _pitchFact.setRawValue(0);
977
    } else {
Don Gagne's avatar
Don Gagne committed
978
        _pitchFact.setRawValue(pitch * (180.0 / M_PI));
979
    }
980
    if (qIsInf(yaw)) {
Don Gagne's avatar
Don Gagne committed
981
        _headingFact.setRawValue(0);
982
    } else {
Don Gagne's avatar
Don Gagne committed
983
        yaw = yaw * (180.0 / M_PI);
984
        if (yaw < 0) yaw += 360;
Don Gagne's avatar
Don Gagne committed
985
        _headingFact.setRawValue(yaw);
986 987 988 989 990 991 992 993 994 995
    }
}

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

void Vehicle::_updateSpeed(UASInterface*, double groundSpeed, double airSpeed, quint64)
{
Don Gagne's avatar
Don Gagne committed
996 997
    _groundSpeedFact.setRawValue(groundSpeed);
    _airSpeedFact.setRawValue(airSpeed);
998 999
}

Don Gagne's avatar
Don Gagne committed
1000 1001
void Vehicle::_updateAltitude(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, quint64)
{
Don Gagne's avatar
Don Gagne committed
1002 1003 1004
    _altitudeAMSLFact.setRawValue(altitudeAMSL);
    _altitudeRelativeFact.setRawValue(altitudeRelative);
    _climbRateFact.setRawValue(climbRate);
1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018
}

void Vehicle::_updateNavigationControllerErrors(UASInterface*, double altitudeError, double speedError, double xtrackError) {
    _navigationAltitudeError   = altitudeError;
    _navigationSpeedError      = speedError;
    _navigationCrosstrackError = xtrackError;
}

void Vehicle::_updateNavigationControllerData(UASInterface *uas, float, float, float, float targetBearing, float) {
    if (_mav == uas) {
        _navigationTargetBearing = targetBearing;
    }
}

Don Gagne's avatar
Don Gagne committed
1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049
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);
}

1050 1051 1052 1053 1054 1055 1056 1057
/*
 * Internal
 */

void Vehicle::_checkUpdate()
{
    // Update current location
    if(_mav) {
1058 1059
        if(latitude() != _mav->getLatitude()) {
            setLatitude(_mav->getLatitude());
1060
        }
1061 1062
        if(longitude() != _mav->getLongitude()) {
            setLongitude(_mav->getLongitude());
1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075
        }
    }
}

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
1076 1077 1078 1079 1080 1081 1082 1083 1084
QString Vehicle::formatedMessages()
{
    QString messages;
    foreach(UASMessage* message, qgcApp()->toolbox()->uasMessageHandler()->messages()) {
        messages += message->getFormatedText();
    }
    return messages;
}

dogmaphobic's avatar
dogmaphobic committed
1085 1086 1087 1088 1089
void Vehicle::clearMessages()
{
    qgcApp()->toolbox()->uasMessageHandler()->clearMessages();
}

dogmaphobic's avatar
dogmaphobic committed
1090 1091 1092 1093 1094 1095 1096 1097 1098
void Vehicle::_handletextMessageReceived(UASMessage* message)
{
    if(message)
    {
        _formatedMessage = message->getFormatedText();
        emit formatedMessageChanged();
    }
}

1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121
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
1122

1123
    UASMessageHandler* pMh = qgcApp()->toolbox()->uasMessageHandler();
1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138 1139 1140 1141 1142 1143 1144 1145 1146 1147 1148 1149 1150 1151 1152 1153 1154 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
    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();
    }
}
1190 1191 1192 1193 1194 1195 1196 1197 1198

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

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

1200
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1201

1202
    bool convertOk;
dogmaphobic's avatar
dogmaphobic committed
1203

1204 1205 1206 1207
    _joystickMode = (JoystickMode_t)settings.value(_joystickModeSettingsKey, JoystickModeRC).toInt(&convertOk);
    if (!convertOk) {
        _joystickMode = JoystickModeRC;
    }
dogmaphobic's avatar
dogmaphobic committed
1208

1209 1210 1211 1212
    // 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();
    }
1213 1214 1215 1216 1217
}

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

1219
    settings.beginGroup(QString(_settingsGroup).arg(_id));
dogmaphobic's avatar
dogmaphobic committed
1220

1221
    settings.setValue(_joystickModeSettingsKey, _joystickMode);
1222 1223 1224 1225 1226 1227

    // 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);
    }
1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239 1240
}

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
1241

1242 1243 1244 1245 1246 1247 1248 1249
    _joystickMode = (JoystickMode_t)mode;
    _saveSettings();
    emit joystickModeChanged(mode);
}

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

1251
    list << "Normal" << "Attitude" << "Position" << "Force" << "Velocity";
dogmaphobic's avatar
dogmaphobic committed
1252

1253 1254
    return list;
}
1255 1256 1257 1258 1259 1260 1261 1262

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

void Vehicle::setJoystickEnabled(bool enabled)
{
1263
    _joystickEnabled = enabled;
1264 1265
    _startJoystick(_joystickEnabled);
    _saveSettings();
1266
    emit joystickEnabledChanged(_joystickEnabled);
1267 1268 1269 1270
}

void Vehicle::_startJoystick(bool start)
{
1271
    Joystick* joystick = _joystickManager->activeJoystick();
1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285 1286 1287 1288 1289 1290
    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
1291

1292 1293
    _startJoystick(_active);
}
1294

1295 1296 1297 1298 1299 1300 1301 1302 1303
bool Vehicle::homePositionAvailable(void)
{
    return _homePositionAvailable;
}

QGeoCoordinate Vehicle::homePosition(void)
{
    return _homePosition;
}
Don Gagne's avatar
Don Gagne committed
1304 1305 1306 1307

void Vehicle::setArmed(bool armed)
{
    // We specifically use COMMAND_LONG:MAV_CMD_COMPONENT_ARM_DISARM since it is supported by more flight stacks.
dogmaphobic's avatar
dogmaphobic committed
1308

Don Gagne's avatar
Don Gagne committed
1309 1310
    mavlink_message_t msg;
    mavlink_command_long_t cmd;
dogmaphobic's avatar
dogmaphobic committed
1311

Don Gagne's avatar
Don Gagne committed
1312 1313 1314 1315 1316 1317 1318 1319 1320 1321
    cmd.command = (uint16_t)MAV_CMD_COMPONENT_ARM_DISARM;
    cmd.confirmation = 0;
    cmd.param1 = armed ? 1.0f : 0.0f;
    cmd.param2 = 0.0f;
    cmd.param3 = 0.0f;
    cmd.param4 = 0.0f;
    cmd.param5 = 0.0f;
    cmd.param6 = 0.0f;
    cmd.param7 = 0.0f;
    cmd.target_system = id();
1322
    cmd.target_component = defaultComponentId();
dogmaphobic's avatar
dogmaphobic committed
1323

1324 1325 1326 1327 1328
    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
                                         _mavlink->getComponentId(),
                                         priorityLink()->mavlinkChannel(),
                                         &msg,
                                         &cmd);
dogmaphobic's avatar
dogmaphobic committed
1329

1330
    sendMessageOnLink(priorityLink(), msg);
Don Gagne's avatar
Don Gagne committed
1331 1332 1333 1334
}

bool Vehicle::flightModeSetAvailable(void)
{
1335
    return _firmwarePlugin->isCapable(this, FirmwarePlugin::SetFlightModeCapability);
Don Gagne's avatar
Don Gagne committed
1336 1337 1338 1339
}

QStringList Vehicle::flightModes(void)
{
Daniel Agar's avatar
Daniel Agar committed
1340
    return _firmwarePlugin->flightModes(this);
Don Gagne's avatar
Don Gagne committed
1341 1342
}

Don Gagne's avatar
Don Gagne committed
1343
QString Vehicle::flightMode(void) const
Don Gagne's avatar
Don Gagne committed
1344 1345 1346 1347 1348 1349 1350 1351 1352 1353 1354 1355 1356 1357 1358 1359
{
    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;
1360 1361 1362 1363 1364 1365 1366 1367
        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
1368
    } else {
Don Gagne's avatar
Don Gagne committed
1369
        qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;
Don Gagne's avatar
Don Gagne committed
1370 1371 1372 1373 1374 1375 1376 1377 1378 1379 1380 1381 1382 1383 1384 1385 1386
    }
}

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

1387 1388 1389 1390 1391 1392 1393 1394
    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
1395
}
1396

1397
void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
1398 1399 1400
{
    mavlink_message_t               msg;
    mavlink_request_data_stream_t   dataStream;
dogmaphobic's avatar
dogmaphobic committed
1401

1402 1403 1404 1405
    dataStream.req_stream_id = stream;
    dataStream.req_message_rate = rate;
    dataStream.start_stop = 1;  // start
    dataStream.target_system = id();
1406
    dataStream.target_component = defaultComponentId();
dogmaphobic's avatar
dogmaphobic committed
1407

1408 1409 1410 1411 1412
    mavlink_msg_request_data_stream_encode_chan(_mavlink->getSystemId(),
                                                _mavlink->getComponentId(),
                                                priorityLink()->mavlinkChannel(),
                                                &msg,
                                                &dataStream);
1413

1414 1415 1416 1417
    if (sendMultiple) {
        // We use sendMessageMultiple since we really want these to make it to the vehicle
        sendMessageMultiple(msg);
    } else {
1418
        sendMessageOnLink(priorityLink(), msg);
1419
    }
1420 1421 1422 1423 1424 1425
}

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

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

1429 1430 1431 1432 1433 1434
        if (--_sendMessageMultipleList[_nextSendMessageMultipleIndex].retryCount <= 0) {
            _sendMessageMultipleList.removeAt(_nextSendMessageMultipleIndex);
        } else {
            _nextSendMessageMultipleIndex++;
        }
    }
dogmaphobic's avatar
dogmaphobic committed
1435

1436 1437 1438 1439 1440 1441 1442 1443
    if (_nextSendMessageMultipleIndex >= _sendMessageMultipleList.count()) {
        _nextSendMessageMultipleIndex = 0;
    }
}

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

1445 1446
    info.message =      message;
    info.retryCount =   _sendMessageMultipleRetries;
dogmaphobic's avatar
dogmaphobic committed
1447

1448 1449
    _sendMessageMultipleList.append(info);
}
1450 1451 1452 1453

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

1457 1458 1459
void Vehicle::_geoFenceManagerError(int errorCode, const QString& errorMsg)
{
    Q_UNUSED(errorCode);
1460 1461 1462 1463 1464 1465 1466
    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));
1467 1468
}

1469 1470 1471
void Vehicle::_addNewMapTrajectoryPoint(void)
{
    if (_mapTrajectoryHaveFirstCoordinate) {
1472 1473 1474 1475
        // Keep three minutes of trajectory
        if (_mapTrajectoryList.count() * _mapTrajectoryMsecsBetweenPoints > 3 * 1000 * 60) {
            _mapTrajectoryList.removeAt(0)->deleteLater();
        }
1476
        _mapTrajectoryList.append(new CoordinateVector(_mapTrajectoryLastCoordinate, _coordinate, this));
1477 1478
    }
    _mapTrajectoryHaveFirstCoordinate = true;
1479
    _mapTrajectoryLastCoordinate = _coordinate;
1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492
}

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

void Vehicle::_mapTrajectoryStop()
{
    _mapTrajectoryTimer.stop();
}
1493 1494 1495

void Vehicle::_parametersReady(bool parametersReady)
{
1496 1497
    if (parametersReady && !_missionManagerInitialRequestSent) {
        _missionManagerInitialRequestSent = true;
1498 1499
        _missionManager->requestMissionItems();
    }
Lorenz Meier's avatar
Lorenz Meier committed
1500 1501 1502 1503

    if (parametersReady) {
        setJoystickEnabled(_joystickEnabled);
    }
1504
}
1505

Don Gagne's avatar
Don Gagne committed
1506
void Vehicle::disconnectInactiveVehicle(void)
1507
{
Don Gagne's avatar
Don Gagne committed
1508
    // Vehicle is no longer communicating with us, disconnect all links
1509

1510

1511
    LinkManager* linkMgr = qgcApp()->toolbox()->linkManager();
1512
    for (int i=0; i<_links.count(); i++) {
1513 1514 1515 1516 1517
        // 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.
        if (!qgcApp()->toolbox()->multiVehicleManager()->linkInUse(_links[i], this)) {
            linkMgr->disconnectLink(_links[i]);
        }
1518 1519
    }
}
1520

dogmaphobic's avatar
dogmaphobic committed
1521 1522 1523 1524 1525 1526 1527 1528 1529 1530
void Vehicle::_imageReady(UASInterface*)
{
    if(_uas)
    {
        QImage img = _uas->getImage();
        qgcApp()->toolbox()->imageProvider()->setImage(&img, _id);
        _flowImageIndex++;
        emit flowImageIndexChanged();
    }
}
Don Gagne's avatar
Don Gagne committed
1531 1532 1533

void Vehicle::_remoteControlRSSIChanged(uint8_t rssi)
{