MultiVehicleManager.cc 11.9 KB
Newer Older
1
/*=====================================================================
dogmaphobic's avatar
dogmaphobic committed
2

3
 QGroundControl Open Source Ground Control Station
dogmaphobic's avatar
dogmaphobic committed
4

5
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
dogmaphobic's avatar
dogmaphobic committed
6

7
 This file is part of the QGROUNDCONTROL project
dogmaphobic's avatar
dogmaphobic committed
8

9 10 11 12
 QGROUNDCONTROL is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
dogmaphobic's avatar
dogmaphobic committed
13

14 15 16 17
 QGROUNDCONTROL is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
dogmaphobic's avatar
dogmaphobic committed
18

19 20
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
dogmaphobic's avatar
dogmaphobic committed
21

22 23 24 25 26 27 28
 ======================================================================*/

/// @file
///     @author Don Gagne <don@thegagnes.com>

#include "MultiVehicleManager.h"
#include "AutoPilotPlugin.h"
29 30
#include "MAVLinkProtocol.h"
#include "UAS.h"
31
#include "QGCApplication.h"
Jimmy Johnson's avatar
Jimmy Johnson committed
32
#include "FollowMe.h"
33

Don Gagne's avatar
Don Gagne committed
34 35
#ifdef __mobile__
#include "MobileScreenMgr.h"
36 37
#endif

Don Gagne's avatar
Don Gagne committed
38 39
#include <QQmlEngine>

Don Gagne's avatar
Don Gagne committed
40 41
QGC_LOGGING_CATEGORY(MultiVehicleManagerLog, "MultiVehicleManagerLog")

42
const char* MultiVehicleManager::_gcsHeartbeatEnabledKey = "gcsHeartbeatEnabled";
43

44 45
MultiVehicleManager::MultiVehicleManager(QGCApplication* app)
    : QGCTool(app)
46 47 48
    , _activeVehicleAvailable(false)
    , _parameterReadyVehicleAvailable(false)
    , _activeVehicle(NULL)
Don Gagne's avatar
Don Gagne committed
49
    , _disconnectedVehicle(NULL)
50 51 52 53
    , _firmwarePluginManager(NULL)
    , _autopilotPluginManager(NULL)
    , _joystickManager(NULL)
    , _mavlinkProtocol(NULL)
54
    , _gcsHeartbeatEnabled(true)
55
{
56 57 58
    QSettings settings;

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

60 61 62 63 64 65
    _gcsHeartbeatTimer.setInterval(_gcsHeartbeatRateMSecs);
    _gcsHeartbeatTimer.setSingleShot(false);
    connect(&_gcsHeartbeatTimer, &QTimer::timeout, this, &MultiVehicleManager::_sendGCSHeartbeat);
    if (_gcsHeartbeatEnabled) {
        _gcsHeartbeatTimer.start();
    }
Don Gagne's avatar
Don Gagne committed
66 67

    _disconnectedVehicle = new Vehicle(this);
68 69
}

70
void MultiVehicleManager::setToolbox(QGCToolbox *toolbox)
71
{
72 73 74 75 76 77
   QGCTool::setToolbox(toolbox);

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

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

82
   connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &MultiVehicleManager::_vehicleHeartbeatInfo);
83 84
}

85
void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicleId, int vehicleMavlinkVersion, int vehicleFirmwareType, int vehicleType)
86
{
87 88 89 90 91 92 93 94 95 96
    if (_ignoreVehicleIds.contains(vehicleId) || getVehicleById(vehicleId)) {
        return;
    }

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

98 99 100
    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
101

102 103 104 105 106 107 108 109 110
    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
111

112 113 114
    Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)vehicleFirmwareType, (MAV_TYPE)vehicleType, _firmwarePluginManager, _autopilotPluginManager, _joystickManager);
    connect(vehicle, &Vehicle::allLinksInactive, this, &MultiVehicleManager::_deleteVehiclePhase1);
    connect(vehicle->autopilotPlugin(), &AutoPilotPlugin::parametersReadyChanged, this, &MultiVehicleManager::_autopilotParametersReadyChanged);
115

116
    _vehicles.append(vehicle);
dogmaphobic's avatar
dogmaphobic committed
117

118
    emit vehicleAdded(vehicle);
dogmaphobic's avatar
dogmaphobic committed
119

120
    setActiveVehicle(vehicle);
121

122 123 124
    // Mark link as active
    link->setActive(true);

Don Gagne's avatar
Don Gagne committed
125
#ifdef __mobile__
126
    if(_vehicles.count() == 1) {
Don Gagne's avatar
Don Gagne committed
127
        //-- Once a vehicle is connected, keep screen from going off
128
        qCDebug(MultiVehicleManagerLog) << "QAndroidJniObject::keepScreenOn";
Don Gagne's avatar
Don Gagne committed
129
        MobileScreenMgr::setKeepScreenOn(true);
130 131 132
    }
#endif

133 134 135 136
}

/// 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.
137
void MultiVehicleManager::_deleteVehiclePhase1(Vehicle* vehicle)
138
{
Don Gagne's avatar
Don Gagne committed
139
    qCDebug(MultiVehicleManagerLog) << "_deleteVehiclePhase1" << vehicle;
Don Gagne's avatar
Don Gagne committed
140

Don Gagne's avatar
Don Gagne committed
141
    _vehiclesBeingDeleted << vehicle;
142 143

    // Remove from map
144 145
    bool found = false;
    for (int i=0; i<_vehicles.count(); i++) {
Don Gagne's avatar
Don Gagne committed
146
        if (_vehicles[i] == vehicle) {
147
            _vehicles.removeAt(i);
148 149 150 151 152 153 154
            found = true;
            break;
        }
    }
    if (!found) {
        qWarning() << "Vehicle not found in map!";
    }
dogmaphobic's avatar
dogmaphobic committed
155

156
    vehicle->setActive(false);
Don Gagne's avatar
Don Gagne committed
157
    vehicle->uas()->shutdownVehicle();
dogmaphobic's avatar
dogmaphobic committed
158

159 160 161 162 163 164
    // 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
165

Don Gagne's avatar
Don Gagne committed
166
#ifdef __mobile__
167
    if(_vehicles.count() == 0) {
Don Gagne's avatar
Don Gagne committed
168
        //-- Once no vehicles are connected, we no longer need to keep screen from going off
169
        qCDebug(MultiVehicleManagerLog) << "QAndroidJniObject::restoreScreenOn";
Don Gagne's avatar
Don Gagne committed
170
        MobileScreenMgr::setKeepScreenOn(true);
171 172 173
    }
#endif

174 175 176 177 178 179 180 181 182
    // 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
183
void MultiVehicleManager::_deleteVehiclePhase2(void)
184
{
Don Gagne's avatar
Don Gagne committed
185
    qCDebug(MultiVehicleManagerLog) << "_deleteVehiclePhase2" << _vehiclesBeingDeleted[0];
Don Gagne's avatar
Don Gagne committed
186

187 188
    /// 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
189

190
    Vehicle* newActiveVehicle = NULL;
191 192
    if (_vehicles.count()) {
        newActiveVehicle = qobject_cast<Vehicle*>(_vehicles[0]);
193
    }
dogmaphobic's avatar
dogmaphobic committed
194

195 196
    _activeVehicle = newActiveVehicle;
    emit activeVehicleChanged(newActiveVehicle);
dogmaphobic's avatar
dogmaphobic committed
197

198
    if (_activeVehicle) {
199
        _activeVehicle->setActive(true);
200
        emit activeVehicleAvailableChanged(true);
201
        if (_activeVehicle->autopilotPlugin()->parametersReady()) {
202 203 204
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
dogmaphobic's avatar
dogmaphobic committed
205

Don Gagne's avatar
Don Gagne committed
206 207
    delete _vehiclesBeingDeleted[0];
    _vehiclesBeingDeleted.removeAt(0);
208 209 210 211
}

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

214
    if (vehicle != _activeVehicle) {
Don Gagne's avatar
Don Gagne committed
215
        if (_activeVehicle) {
216
            _activeVehicle->setActive(false);
dogmaphobic's avatar
dogmaphobic committed
217

Don Gagne's avatar
Don Gagne committed
218 219
            // 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
220

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

229 230 231 232 233 234 235 236
        // See explanation in _deleteVehiclePhase1
        _vehicleBeingSetActive = vehicle;
        QTimer::singleShot(20, this, &MultiVehicleManager::_setActiveVehiclePhase2);
    }
}

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

239 240 241
    // Now we signal the new active vehicle
    _activeVehicle = _vehicleBeingSetActive;
    emit activeVehicleChanged(_activeVehicle);
dogmaphobic's avatar
dogmaphobic committed
242

243 244
    // And finally vehicle availability
    if (_activeVehicle) {
245
        _activeVehicle->setActive(true);
246 247
        _activeVehicleAvailable = true;
        emit activeVehicleAvailableChanged(true);
dogmaphobic's avatar
dogmaphobic committed
248

249
        if (_activeVehicle->autopilotPlugin()->parametersReady()) {
250 251 252 253 254 255
            _parameterReadyVehicleAvailable = true;
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
}

256
void MultiVehicleManager::_autopilotParametersReadyChanged(bool parametersReady)
257 258
{
    AutoPilotPlugin* autopilot = dynamic_cast<AutoPilotPlugin*>(sender());
dogmaphobic's avatar
dogmaphobic committed
259

260 261 262 263
    if (!autopilot) {
        qWarning() << "Dynamic cast failed!";
        return;
    }
dogmaphobic's avatar
dogmaphobic committed
264

Don Gagne's avatar
Don Gagne committed
265
    if (autopilot->vehicle() == _activeVehicle) {
266 267
        _parameterReadyVehicleAvailable = parametersReady;
        emit parameterReadyVehicleAvailableChanged(parametersReady);
268 269 270
    }
}

271 272 273 274 275 276 277 278 279 280 281
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();
}
282 283 284 285 286 287 288 289 290 291 292 293

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;
}
294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328

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,
                                   MAV_TYPE_GCS,                // MAV_TYPE
                                   MAV_AUTOPILOT_INVALID,   // MAV_AUTOPILOT
                                   MAV_MODE_MANUAL_ARMED,   // MAV_MODE
                                   0,                       // custom mode
                                   MAV_STATE_ACTIVE);       // MAV_STATE
        vehicle->sendMessage(message);
    }
}