MultiVehicleManager.cc 12.2 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
#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 "QGroundControlQmlGlobal.h"
17
#include "ParameterManager.h"
18

Don Gagne's avatar
Don Gagne committed
19 20
#ifdef __mobile__
#include "MobileScreenMgr.h"
21 22
#endif

Don Gagne's avatar
Don Gagne committed
23 24
#include <QQmlEngine>

Don Gagne's avatar
Don Gagne committed
25 26
QGC_LOGGING_CATEGORY(MultiVehicleManagerLog, "MultiVehicleManagerLog")

27
const char* MultiVehicleManager::_gcsHeartbeatEnabledKey = "gcsHeartbeatEnabled";
28

29 30
MultiVehicleManager::MultiVehicleManager(QGCApplication* app)
    : QGCTool(app)
31 32 33
    , _activeVehicleAvailable(false)
    , _parameterReadyVehicleAvailable(false)
    , _activeVehicle(NULL)
34
    , _offlineEditingVehicle(NULL)
35 36 37 38
    , _firmwarePluginManager(NULL)
    , _autopilotPluginManager(NULL)
    , _joystickManager(NULL)
    , _mavlinkProtocol(NULL)
39
    , _gcsHeartbeatEnabled(true)
40
{
41 42 43
    QSettings settings;

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

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

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

   _firmwarePluginManager =     _toolbox->firmwarePluginManager();
   _autopilotPluginManager =    _toolbox->autopilotPluginManager();
   _joystickManager =           _toolbox->joystickManager();
   _mavlinkProtocol =           _toolbox->mavlinkProtocol();
61

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

65
   connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &MultiVehicleManager::_vehicleHeartbeatInfo);
66 67 68 69 70

   _offlineEditingVehicle = new Vehicle(static_cast<MAV_AUTOPILOT>(QGroundControlQmlGlobal::offlineEditingFirmwareType()->rawValue().toInt()),
                                        static_cast<MAV_TYPE>(QGroundControlQmlGlobal::offlineEditingVehicleType()->rawValue().toInt()),
                                        _firmwarePluginManager,
                                        this);
71 72
}

73
void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType)
74
{
75 76
    if (_ignoreVehicleIds.contains(vehicleId) || getVehicleById(vehicleId)
            || vehicleId == 0) {
77 78 79 80 81 82 83 84 85
        return;
    }

    qCDebug(MultiVehicleManagerLog()) << "Adding new vehicle link:vehicleId:vehicleMavlinkVersion:vehicleFirmwareType:vehicleType "
                                      << link->getName()
                                      << vehicleId
                                      << vehicleMavlinkVersion
                                      << vehicleFirmwareType
                                      << vehicleType;
dogmaphobic's avatar
dogmaphobic committed
86

87 88 89
    if (vehicleId == _mavlinkProtocol->getSystemId()) {
        _app->showMessage(QString("Warning: A vehicle is using the same system id as QGroundControl: %1").arg(vehicleId));
    }
dogmaphobic's avatar
dogmaphobic committed
90

91 92 93 94 95 96 97 98 99
//    QSettings settings;
//    bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool();
//    if (mavlinkVersionCheck && vehicleMavlinkVersion != MAVLINK_VERSION) {
//        _ignoreVehicleIds += vehicleId;
//        _app->showMessage(QString("The MAVLink protocol version on vehicle #%1 and QGroundControl differ! "
//                                  "It is unsafe to use different MAVLink versions. "
//                                  "QGroundControl therefore refuses to connect to vehicle #%1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(vehicleId).arg(vehicleMavlinkVersion).arg(MAVLINK_VERSION));
//        return;
//    }
dogmaphobic's avatar
dogmaphobic committed
100

101 102
    Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _autopilotPluginManager, _joystickManager);
    connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1);
103
    connect(vehicle->parameterManager(), &ParameterManager::parametersReadyChanged, this, &MultiVehicleManager::_vehicleParametersReadyChanged);
104

105
    _vehicles.append(vehicle);
dogmaphobic's avatar
dogmaphobic committed
106

107 108 109
    // Send QGC heartbeat ASAP, this allows PX4 to start accepting commands
    _sendGCSHeartbeat();

110
    emit vehicleAdded(vehicle);
dogmaphobic's avatar
dogmaphobic committed
111

112
    setActiveVehicle(vehicle);
113

114 115 116
    // Mark link as active
    link->setActive(true);

Don Gagne's avatar
Don Gagne committed
117
#ifdef __mobile__
118
    if(_vehicles.count() == 1) {
Don Gagne's avatar
Don Gagne committed
119
        //-- Once a vehicle is connected, keep screen from going off
120
        qCDebug(MultiVehicleManagerLog) << "QAndroidJniObject::keepScreenOn";
Don Gagne's avatar
Don Gagne committed
121
        MobileScreenMgr::setKeepScreenOn(true);
122 123 124
    }
#endif

125 126 127 128
}

/// 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.
129
void MultiVehicleManager::_deleteVehiclePhase1(Vehicle* vehicle)
130
{
Don Gagne's avatar
Don Gagne committed
131
    qCDebug(MultiVehicleManagerLog) << "_deleteVehiclePhase1" << vehicle;
Don Gagne's avatar
Don Gagne committed
132

Don Gagne's avatar
Don Gagne committed
133
    _vehiclesBeingDeleted << vehicle;
134 135

    // Remove from map
136 137
    bool found = false;
    for (int i=0; i<_vehicles.count(); i++) {
Don Gagne's avatar
Don Gagne committed
138
        if (_vehicles[i] == vehicle) {
139
            _vehicles.removeAt(i);
140 141 142 143 144 145 146
            found = true;
            break;
        }
    }
    if (!found) {
        qWarning() << "Vehicle not found in map!";
    }
dogmaphobic's avatar
dogmaphobic committed
147

148
    vehicle->setActive(false);
Don Gagne's avatar
Don Gagne committed
149
    vehicle->uas()->shutdownVehicle();
dogmaphobic's avatar
dogmaphobic committed
150

151 152 153 154 155 156
    // First we must signal that a vehicle is no longer available.
    _activeVehicleAvailable = false;
    _parameterReadyVehicleAvailable = false;
    emit activeVehicleAvailableChanged(false);
    emit parameterReadyVehicleAvailableChanged(false);
    emit vehicleRemoved(vehicle);
dogmaphobic's avatar
dogmaphobic committed
157

Don Gagne's avatar
Don Gagne committed
158
#ifdef __mobile__
159
    if(_vehicles.count() == 0) {
Don Gagne's avatar
Don Gagne committed
160
        //-- Once no vehicles are connected, we no longer need to keep screen from going off
161
        qCDebug(MultiVehicleManagerLog) << "QAndroidJniObject::restoreScreenOn";
162
        MobileScreenMgr::setKeepScreenOn(false);
163 164 165
    }
#endif

166 167 168 169 170 171 172 173 174
    // We must let the above signals flow through the system as well as get back to the main loop event queue
    // before we can actually delete the Vehicle. The reason is that Qml may be holding on the references to it.
    // 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
    // as well as the unit testing case whichof course has a different signal flow!
    QTimer::singleShot(20, this, &MultiVehicleManager::_deleteVehiclePhase2);
}

Don Gagne's avatar
Don Gagne committed
175
void MultiVehicleManager::_deleteVehiclePhase2(void)
176
{
Don Gagne's avatar
Don Gagne committed
177
    qCDebug(MultiVehicleManagerLog) << "_deleteVehiclePhase2" << _vehiclesBeingDeleted[0];
Don Gagne's avatar
Don Gagne committed
178

179 180
    /// 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
181

182
    Vehicle* newActiveVehicle = NULL;
183 184
    if (_vehicles.count()) {
        newActiveVehicle = qobject_cast<Vehicle*>(_vehicles[0]);
185
    }
dogmaphobic's avatar
dogmaphobic committed
186

187 188
    _activeVehicle = newActiveVehicle;
    emit activeVehicleChanged(newActiveVehicle);
dogmaphobic's avatar
dogmaphobic committed
189

190
    if (_activeVehicle) {
191
        _activeVehicle->setActive(true);
192
        emit activeVehicleAvailableChanged(true);
193
        if (_activeVehicle->parameterManager()->parametersReady()) {
194 195 196
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
dogmaphobic's avatar
dogmaphobic committed
197

Don Gagne's avatar
Don Gagne committed
198 199
    delete _vehiclesBeingDeleted[0];
    _vehiclesBeingDeleted.removeAt(0);
200 201 202 203
}

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

206
    if (vehicle != _activeVehicle) {
Don Gagne's avatar
Don Gagne committed
207
        if (_activeVehicle) {
208
            _activeVehicle->setActive(false);
dogmaphobic's avatar
dogmaphobic committed
209

Don Gagne's avatar
Don Gagne committed
210 211
            // 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
212

Don Gagne's avatar
Don Gagne committed
213 214 215 216 217 218 219
            // 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
220

221 222 223 224 225 226 227 228
        // See explanation in _deleteVehiclePhase1
        _vehicleBeingSetActive = vehicle;
        QTimer::singleShot(20, this, &MultiVehicleManager::_setActiveVehiclePhase2);
    }
}

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

231 232 233
    // Now we signal the new active vehicle
    _activeVehicle = _vehicleBeingSetActive;
    emit activeVehicleChanged(_activeVehicle);
dogmaphobic's avatar
dogmaphobic committed
234

235 236
    // And finally vehicle availability
    if (_activeVehicle) {
237
        _activeVehicle->setActive(true);
238 239
        _activeVehicleAvailable = true;
        emit activeVehicleAvailableChanged(true);
dogmaphobic's avatar
dogmaphobic committed
240

241
        if (_activeVehicle->parameterManager()->parametersReady()) {
242 243 244 245 246 247
            _parameterReadyVehicleAvailable = true;
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
}

248
void MultiVehicleManager::_vehicleParametersReadyChanged(bool parametersReady)
249
{
250
    ParameterManager* paramMgr = dynamic_cast<ParameterManager*>(sender());
dogmaphobic's avatar
dogmaphobic committed
251

252
    if (!paramMgr) {
253 254 255
        qWarning() << "Dynamic cast failed!";
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
256

257
    if (paramMgr->vehicle() == _activeVehicle) {
258 259
        _parameterReadyVehicleAvailable = parametersReady;
        emit parameterReadyVehicleAvailableChanged(parametersReady);
260 261 262
    }
}

263 264 265 266 267 268 269 270 271 272 273
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();
}
274 275 276 277 278 279 280 281 282 283 284 285

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

    return NULL;
}
286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312

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)
{
    for (int i=0; i< _vehicles.count(); i++) {
        Vehicle* vehicle = qobject_cast<Vehicle*>(_vehicles[i]);

        mavlink_message_t message;
        mavlink_msg_heartbeat_pack(_mavlinkProtocol->getSystemId(),
                                   _mavlinkProtocol->getComponentId(),
                                   &message,
313
                                   MAV_TYPE_GCS,            // MAV_TYPE
314 315 316 317
                                   MAV_AUTOPILOT_INVALID,   // MAV_AUTOPILOT
                                   MAV_MODE_MANUAL_ARMED,   // MAV_MODE
                                   0,                       // custom mode
                                   MAV_STATE_ACTIVE);       // MAV_STATE
318
        vehicle->sendMessageOnPriorityLink(message);
319 320
    }
}
321 322 323 324 325 326 327 328 329 330 331 332 333 334 335

bool MultiVehicleManager::linkInUse(LinkInterface* link, Vehicle* skipVehicle)
{
    for (int i=0; i< _vehicles.count(); i++) {
        Vehicle* vehicle = qobject_cast<Vehicle*>(_vehicles[i]);

        if (vehicle != skipVehicle) {
            if (vehicle->containsLink(link)) {
                return true;
            }
        }
    }

    return false;
}