MultiVehicleManager.cc 14.4 KB
Newer Older
1 2
/****************************************************************************
 *
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8
 *
 * 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
#include "MultiVehicleManager.h"
#include "AutoPilotPlugin.h"
12 13
#include "MAVLinkProtocol.h"
#include "UAS.h"
14
#include "QGCApplication.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
15
#include "FollowMe.h"
16
#include "ParameterManager.h"
17
#include "SettingsManager.h"
18 19
#include "QGCCorePlugin.h"
#include "QGCOptions.h"
20
#include "LinkManager.h"
21

22
#if defined (__ios__) || defined(__android__)
Don Gagne's avatar
Don Gagne committed
23
#include "MobileScreenMgr.h"
24 25
#endif

Don Gagne's avatar
Don Gagne committed
26 27
#include <QQmlEngine>

Don Gagne's avatar
Don Gagne committed
28 29
QGC_LOGGING_CATEGORY(MultiVehicleManagerLog, "MultiVehicleManagerLog")

30
const char* MultiVehicleManager::_gcsHeartbeatEnabledKey = "gcsHeartbeatEnabled";
31

32 33
MultiVehicleManager::MultiVehicleManager(QGCApplication* app, QGCToolbox* toolbox)
    : QGCTool(app, toolbox)
34 35
    , _activeVehicleAvailable(false)
    , _parameterReadyVehicleAvailable(false)
36 37 38 39 40
    , _activeVehicle(nullptr)
    , _offlineEditingVehicle(nullptr)
    , _firmwarePluginManager(nullptr)
    , _joystickManager(nullptr)
    , _mavlinkProtocol(nullptr)
41
    , _gcsHeartbeatEnabled(true)
42
{
43 44 45
    QSettings settings;

    _gcsHeartbeatEnabled = settings.value(_gcsHeartbeatEnabledKey, true).toBool();
46

47 48 49 50 51 52
    _gcsHeartbeatTimer.setInterval(_gcsHeartbeatRateMSecs);
    _gcsHeartbeatTimer.setSingleShot(false);
    connect(&_gcsHeartbeatTimer, &QTimer::timeout, this, &MultiVehicleManager::_sendGCSHeartbeat);
    if (_gcsHeartbeatEnabled) {
        _gcsHeartbeatTimer.start();
    }
53 54
}

55
void MultiVehicleManager::setToolbox(QGCToolbox *toolbox)
56
{
57
    QGCTool::setToolbox(toolbox);
58

59 60 61
    _firmwarePluginManager =     _toolbox->firmwarePluginManager();
    _joystickManager =           _toolbox->joystickManager();
    _mavlinkProtocol =           _toolbox->mavlinkProtocol();
62

63 64
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
    qmlRegisterUncreatableType<MultiVehicleManager>("QGroundControl.MultiVehicleManager", 1, 0, "MultiVehicleManager", "Reference only");
dogmaphobic's avatar
dogmaphobic committed
65

66
    connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &MultiVehicleManager::_vehicleHeartbeatInfo);
67

68
    _offlineEditingVehicle = new Vehicle(Vehicle::MAV_AUTOPILOT_TRACK, Vehicle::MAV_TYPE_TRACK, _firmwarePluginManager, this);
69 70
}

71
void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType)
72
{
73 74 75 76 77 78 79 80
    // Special case PX4 Flow since depending on firmware it can have different settings. We force to the PX4 Firmware settings.
    if (link->isPX4Flow()) {
        vehicleId = 81;
        componentId = 50;//MAV_COMP_ID_AUTOPILOT1;
        vehicleFirmwareType = MAV_AUTOPILOT_GENERIC;
        vehicleType = 0;
    }

81
    if (componentId != MAV_COMP_ID_AUTOPILOT1) {
82 83 84
        // Special case for PX4 Flow
        if (vehicleId != 81 || componentId != 50) {
            // Don't create vehicles for components other than the autopilot
85 86
            qCDebug(MultiVehicleManagerLog()) << "Ignoring heartbeat from unknown component port:vehicleId:componentId:fwType:vehicleType"
                                              << link->linkConfiguration()->name()
87 88 89 90
                                              << vehicleId
                                              << componentId
                                              << vehicleFirmwareType
                                              << vehicleType;
91
            return;
92 93 94
        }
    }

95 96 97
    if (_vehicles.count() > 0 && !qgcApp()->toolbox()->corePlugin()->options()->multiVehicleEnabled()) {
        return;
    }
98
    if (_ignoreVehicleIds.contains(vehicleId) || getVehicleById(vehicleId) || vehicleId == 0) {
99 100 101
        return;
    }

102 103 104 105 106 107 108 109 110 111 112 113
    switch (vehicleType) {
    case MAV_TYPE_GCS:
    case MAV_TYPE_ONBOARD_CONTROLLER:
    case MAV_TYPE_GIMBAL:
    case MAV_TYPE_ADSB:
        // These are not vehicles, so don't create a vehicle for them
        return;
    default:
        // All other MAV_TYPEs create vehicles
        break;
    }

114
    qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:componentId:vehicleFirmwareType:vehicleType "
115
                                      << link->linkConfiguration()->name()
116
                                      << vehicleId
117
                                      << componentId
118 119
                                      << vehicleFirmwareType
                                      << vehicleType;
dogmaphobic's avatar
dogmaphobic committed
120

121
    if (vehicleId == _mavlinkProtocol->getSystemId()) {
122
        _app->showAppMessage(tr("Warning: A vehicle is using the same system id as %1: %2").arg(qgcApp()->applicationName()).arg(vehicleId));
123
    }
dogmaphobic's avatar
dogmaphobic committed
124

125
    Vehicle* vehicle = new Vehicle(link, vehicleId, componentId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _joystickManager);
126 127 128
    connect(vehicle,                        &Vehicle::requestProtocolVersion,           this, &MultiVehicleManager::_requestProtocolVersion);
    connect(vehicle->vehicleLinkManager(),  &VehicleLinkManager::allLinksRemoved,       this, &MultiVehicleManager::_deleteVehiclePhase1);
    connect(vehicle->parameterManager(),    &ParameterManager::parametersReadyChanged,  this, &MultiVehicleManager::_vehicleParametersReadyChanged);
129

130
    _vehicles.append(vehicle);
dogmaphobic's avatar
dogmaphobic committed
131

132 133 134
    // Send QGC heartbeat ASAP, this allows PX4 to start accepting commands
    _sendGCSHeartbeat();

135 136
    qgcApp()->toolbox()->settingsManager()->appSettings()->defaultFirmwareType()->setRawValue(vehicleFirmwareType);

137
    emit vehicleAdded(vehicle);
dogmaphobic's avatar
dogmaphobic committed
138

139
    if (_vehicles.count() > 1) {
140
        qgcApp()->showAppMessage(tr("Connected to Vehicle %1").arg(vehicleId));
141 142 143
    } else {
        setActiveVehicle(vehicle);
    }
144

145
#if defined (__ios__) || defined(__android__)
146
    if(_vehicles.count() == 1) {
Don Gagne's avatar
Don Gagne committed
147
        //-- Once a vehicle is connected, keep screen from going off
148
        qCDebug(MultiVehicleManagerLog) << "QAndroidJniObject::keepScreenOn";
Don Gagne's avatar
Don Gagne committed
149
        MobileScreenMgr::setKeepScreenOn(true);
150 151 152
    }
#endif

153 154
}

155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
/// This slot is connected to the Vehicle::requestProtocolVersion signal such that the vehicle manager
/// tries to switch MAVLink to v2 if all vehicles support it
void MultiVehicleManager::_requestProtocolVersion(unsigned version)
{
    unsigned maxversion = 0;

    if (_vehicles.count() == 0) {
        _mavlinkProtocol->setVersion(version);
        return;
    }

    for (int i=0; i<_vehicles.count(); i++) {

        Vehicle *v = qobject_cast<Vehicle*>(_vehicles[i]);
        if (v && v->maxProtoVersion() > maxversion) {
170
            maxversion = v->maxProtoVersion();
171 172 173 174 175 176 177 178
        }
    }

    if (_mavlinkProtocol->getCurrentVersion() != maxversion) {
        _mavlinkProtocol->setVersion(maxversion);
    }
}

179 180
/// This slot is connected to the Vehicle::allLinksDestroyed signal such that the Vehicle is deleted
/// and all other right things happen when the Vehicle goes away.
181
void MultiVehicleManager::_deleteVehiclePhase1(Vehicle* vehicle)
182
{
Don Gagne's avatar
Don Gagne committed
183
    qCDebug(MultiVehicleManagerLog) << "_deleteVehiclePhase1" << vehicle;
Don Gagne's avatar
Don Gagne committed
184

Don Gagne's avatar
Don Gagne committed
185
    _vehiclesBeingDeleted << vehicle;
186 187

    // Remove from map
188 189
    bool found = false;
    for (int i=0; i<_vehicles.count(); i++) {
Don Gagne's avatar
Don Gagne committed
190
        if (_vehicles[i] == vehicle) {
191
            _vehicles.removeAt(i);
192 193 194 195 196 197 198
            found = true;
            break;
        }
    }
    if (!found) {
        qWarning() << "Vehicle not found in map!";
    }
dogmaphobic's avatar
dogmaphobic committed
199

Don Gagne's avatar
Don Gagne committed
200
    vehicle->uas()->shutdownVehicle();
dogmaphobic's avatar
dogmaphobic committed
201

202 203 204 205 206 207
    // First we must signal that a vehicle is no longer available.
    _activeVehicleAvailable = false;
    _parameterReadyVehicleAvailable = false;
    emit activeVehicleAvailableChanged(false);
    emit parameterReadyVehicleAvailableChanged(false);
    emit vehicleRemoved(vehicle);
208
    vehicle->prepareDelete();
dogmaphobic's avatar
dogmaphobic committed
209

210
#if defined (__ios__) || defined(__android__)
211
    if(_vehicles.count() == 0) {
Don Gagne's avatar
Don Gagne committed
212
        //-- Once no vehicles are connected, we no longer need to keep screen from going off
213
        qCDebug(MultiVehicleManagerLog) << "QAndroidJniObject::restoreScreenOn";
214
        MobileScreenMgr::setKeepScreenOn(false);
215 216 217
    }
#endif

218
    // We must let the above signals flow through the system as well as get back to the main loop event queue
219
    // before we can actually delete the Vehicle. The reason is that Qml may be holding on to references to it.
220 221 222
    // Even though the above signals should unload any Qml which has references, that Qml will not be destroyed
    // until we get back to the main loop. So we set a short timer which will then fire after Qt has finished
    // doing all of its internal nastiness to clean up the Qml. This works for both the normal running case
223
    // as well as the unit testing case which of course has a different signal flow!
224 225 226
    QTimer::singleShot(20, this, &MultiVehicleManager::_deleteVehiclePhase2);
}

Don Gagne's avatar
Don Gagne committed
227
void MultiVehicleManager::_deleteVehiclePhase2(void)
228
{
Don Gagne's avatar
Don Gagne committed
229
    qCDebug(MultiVehicleManagerLog) << "_deleteVehiclePhase2" << _vehiclesBeingDeleted[0];
Don Gagne's avatar
Don Gagne committed
230

231 232
    /// Qml has been notified of vehicle about to go away and should be disconnected from it by now.
    /// This means we can now clear the active vehicle property and delete the Vehicle for real.
dogmaphobic's avatar
dogmaphobic committed
233

234
    Vehicle* newActiveVehicle = nullptr;
235 236
    if (_vehicles.count()) {
        newActiveVehicle = qobject_cast<Vehicle*>(_vehicles[0]);
237
    }
dogmaphobic's avatar
dogmaphobic committed
238

239 240
    _activeVehicle = newActiveVehicle;
    emit activeVehicleChanged(newActiveVehicle);
dogmaphobic's avatar
dogmaphobic committed
241

242 243
    if (_activeVehicle) {
        emit activeVehicleAvailableChanged(true);
244
        if (_activeVehicle->parameterManager()->parametersReady()) {
245 246 247
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
dogmaphobic's avatar
dogmaphobic committed
248

Don Gagne's avatar
Don Gagne committed
249 250
    delete _vehiclesBeingDeleted[0];
    _vehiclesBeingDeleted.removeAt(0);
251 252 253 254
}

void MultiVehicleManager::setActiveVehicle(Vehicle* vehicle)
{
Don Gagne's avatar
Don Gagne committed
255 256
    qCDebug(MultiVehicleManagerLog) << "setActiveVehicle" << vehicle;

257
    if (vehicle != _activeVehicle) {
Don Gagne's avatar
Don Gagne committed
258 259 260
        if (_activeVehicle) {
            // The sequence of signals is very important in order to not leave Qml elements connected
            // to a non-existent vehicle.
dogmaphobic's avatar
dogmaphobic committed
261

Don Gagne's avatar
Don Gagne committed
262 263 264 265 266 267 268
            // First we must signal that there is no active vehicle available. This will disconnect
            // any existing ui from the currently active vehicle.
            _activeVehicleAvailable = false;
            _parameterReadyVehicleAvailable = false;
            emit activeVehicleAvailableChanged(false);
            emit parameterReadyVehicleAvailableChanged(false);
        }
dogmaphobic's avatar
dogmaphobic committed
269

270 271 272 273 274 275 276 277
        // See explanation in _deleteVehiclePhase1
        _vehicleBeingSetActive = vehicle;
        QTimer::singleShot(20, this, &MultiVehicleManager::_setActiveVehiclePhase2);
    }
}

void MultiVehicleManager::_setActiveVehiclePhase2(void)
{
Don Gagne's avatar
Don Gagne committed
278
    qCDebug(MultiVehicleManagerLog) << "_setActiveVehiclePhase2 _vehicleBeingSetActive" << _vehicleBeingSetActive;
Don Gagne's avatar
Don Gagne committed
279

280 281 282 283 284 285 286 287
    //-- Keep track of current vehicle's coordinates
    if(_activeVehicle) {
        disconnect(_activeVehicle, &Vehicle::coordinateChanged, this, &MultiVehicleManager::_coordinateChanged);
    }
    if(_vehicleBeingSetActive) {
        connect(_vehicleBeingSetActive, &Vehicle::coordinateChanged, this, &MultiVehicleManager::_coordinateChanged);
    }

288 289 290
    // Now we signal the new active vehicle
    _activeVehicle = _vehicleBeingSetActive;
    emit activeVehicleChanged(_activeVehicle);
dogmaphobic's avatar
dogmaphobic committed
291

292 293 294 295
    // And finally vehicle availability
    if (_activeVehicle) {
        _activeVehicleAvailable = true;
        emit activeVehicleAvailableChanged(true);
dogmaphobic's avatar
dogmaphobic committed
296

297
        if (_activeVehicle->parameterManager()->parametersReady()) {
298 299 300 301 302 303
            _parameterReadyVehicleAvailable = true;
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
}

304 305 306 307 308 309
void MultiVehicleManager::_coordinateChanged(QGeoCoordinate coordinate)
{
    _lastKnownLocation = coordinate;
    emit lastKnownLocationChanged();
}

310
void MultiVehicleManager::_vehicleParametersReadyChanged(bool parametersReady)
311
{
312
    auto* paramMgr = qobject_cast<ParameterManager*>(sender());
dogmaphobic's avatar
dogmaphobic committed
313

314
    if (!paramMgr) {
315 316 317
        qWarning() << "Dynamic cast failed!";
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
318

319
    if (paramMgr->vehicle() == _activeVehicle) {
320 321
        _parameterReadyVehicleAvailable = parametersReady;
        emit parameterReadyVehicleAvailableChanged(parametersReady);
322 323 324
    }
}

325 326 327 328 329 330 331 332 333 334 335
void MultiVehicleManager::saveSetting(const QString &name, const QString& value)
{
    QSettings settings;
    settings.setValue(name, value);
}

QString MultiVehicleManager::loadSetting(const QString &name, const QString& defaultValue)
{
    QSettings settings;
    return settings.value(name, defaultValue).toString();
}
336 337 338 339 340 341 342 343 344 345

Vehicle* MultiVehicleManager::getVehicleById(int vehicleId)
{
    for (int i=0; i< _vehicles.count(); i++) {
        Vehicle* vehicle = qobject_cast<Vehicle*>(_vehicles[i]);
        if (vehicle->id() == vehicleId) {
            return vehicle;
        }
    }

346
    return nullptr;
347
}
348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367

void MultiVehicleManager::setGcsHeartbeatEnabled(bool gcsHeartBeatEnabled)
{
    if (gcsHeartBeatEnabled != _gcsHeartbeatEnabled) {
        _gcsHeartbeatEnabled = gcsHeartBeatEnabled;
        emit gcsHeartBeatEnabledChanged(gcsHeartBeatEnabled);

        QSettings settings;
        settings.setValue(_gcsHeartbeatEnabledKey, gcsHeartBeatEnabled);

        if (gcsHeartBeatEnabled) {
            _gcsHeartbeatTimer.start();
        } else {
            _gcsHeartbeatTimer.stop();
        }
    }
}

void MultiVehicleManager::_sendGCSHeartbeat(void)
{
368 369 370
    LinkManager*                    linkManager = qgcApp()->toolbox()->linkManager();
    QList<SharedLinkInterfacePtr>   sharedLinks = linkManager->links();

371
    // Send a heartbeat out on each link
372 373 374
    for (int i=0; i<sharedLinks.count(); i++) {
        LinkInterface* link = sharedLinks[i].get();
        if (link->isConnected() && !link->linkConfiguration()->isHighLatency()) {
375 376 377 378 379 380 381 382 383 384 385 386 387
            mavlink_message_t message;
            mavlink_msg_heartbeat_pack_chan(_mavlinkProtocol->getSystemId(),
                                            _mavlinkProtocol->getComponentId(),
                                            link->mavlinkChannel(),
                                            &message,
                                            MAV_TYPE_GCS,            // MAV_TYPE
                                            MAV_AUTOPILOT_INVALID,   // MAV_AUTOPILOT
                                            MAV_MODE_MANUAL_ARMED,   // MAV_MODE
                                            0,                       // custom mode
                                            MAV_STATE_ACTIVE);       // MAV_STATE

            uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
            int len = mavlink_msg_to_send_buffer(buffer, &message);
388
            link->writeBytesThreadSafe((const char*)buffer, len);
389
        }
390 391
    }
}