MultiVehicleManager.cc 8.81 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
/*=====================================================================
 
 QGroundControl Open Source Ground Control Station
 
 (c) 2009 - 2014 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 
 This file is part of the QGROUNDCONTROL project
 
 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.
 
 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.
 
 You should have received a copy of the GNU General Public License
 along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
 
 ======================================================================*/

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

#include "MultiVehicleManager.h"
#include "AutoPilotPlugin.h"
29 30
#include "MAVLinkProtocol.h"
#include "UAS.h"
31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50

IMPLEMENT_QGC_SINGLETON(MultiVehicleManager, MultiVehicleManager)

MultiVehicleManager::MultiVehicleManager(QObject* parent) :
    QGCSingleton(parent)
    , _activeVehicleAvailable(false)
    , _parameterReadyVehicleAvailable(false)
    , _activeVehicle(NULL)
{
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
    qmlRegisterUncreatableType<MultiVehicleManager>("QGroundControl.MultiVehicleManager", 1, 0, "MultiVehicleManager", "Reference only");
}

MultiVehicleManager::~MultiVehicleManager()
{

}

bool MultiVehicleManager::notifyHeartbeatInfo(LinkInterface* link, int vehicleId, mavlink_heartbeat_t& heartbeat)
{
51
    if (!getVehicleById(vehicleId) && !_ignoreVehicleIds.contains(vehicleId)) {
52 53 54 55 56 57 58 59 60 61 62 63 64 65
        if (vehicleId == MAVLinkProtocol::instance()->getSystemId()) {
            qgcApp()->showToolBarMessage(QString("Warning: A vehicle is using the same system id as QGroundControl: %1").arg(vehicleId));
        }
        
        QSettings settings;
        bool mavlinkVersionCheck = settings.value("VERSION_CHECK_ENABLED", true).toBool();
        if (mavlinkVersionCheck && heartbeat.mavlink_version != MAVLINK_VERSION) {
            _ignoreVehicleIds += vehicleId;
            qgcApp()->showToolBarMessage(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(heartbeat.mavlink_version).arg(MAVLINK_VERSION));
            return false;
        }
        
66
        Vehicle* vehicle = new Vehicle(link, vehicleId, (MAV_AUTOPILOT)heartbeat.autopilot, (MAV_TYPE)heartbeat.type);
67 68 69 70 71 72 73
        
        if (!vehicle) {
            qWarning() << "New Vehicle allocation failed";
            return false;
        }
        
        connect(vehicle, &Vehicle::allLinksDisconnected, this, &MultiVehicleManager::_deleteVehiclePhase1);
74
        connect(vehicle->autopilotPlugin(), &AutoPilotPlugin::parametersReadyChanged, this, &MultiVehicleManager::_autopilotParametersReadyChanged);
75

76
        _vehicles.append(vehicle);
77 78 79 80 81 82 83 84 85 86 87
        
        emit vehicleAdded(vehicle);
        
        setActiveVehicle(vehicle);
    }
    
    return true;
}

/// 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.
88
void MultiVehicleManager::_deleteVehiclePhase1(Vehicle* vehicle)
89 90 91 92
{
    _vehicleBeingDeleted = vehicle;

    // Remove from map
93 94 95 96
    bool found = false;
    for (int i=0; i<_vehicles.count(); i++) {
        if (_vehicles[i] == _vehicleBeingDeleted) {
            _vehicles.removeAt(i);
97 98 99 100 101 102 103 104
            found = true;
            break;
        }
    }
    if (!found) {
        qWarning() << "Vehicle not found in map!";
    }
    
105
    vehicle->setActive(false);
106 107
    vehicle->uas()->clearVehicle();
    
108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129
    // First we must signal that a vehicle is no longer available.
    _activeVehicleAvailable = false;
    _parameterReadyVehicleAvailable = false;
    emit activeVehicleAvailableChanged(false);
    emit parameterReadyVehicleAvailableChanged(false);
    emit vehicleRemoved(vehicle);
    
    // 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);
}

void MultiVehicleManager::_deleteVehiclePhase2  (void)
{
    /// 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.
    
    Vehicle* newActiveVehicle = NULL;
130 131
    if (_vehicles.count()) {
        newActiveVehicle = qobject_cast<Vehicle*>(_vehicles[0]);
132 133 134 135 136 137
    }
    
    _activeVehicle = newActiveVehicle;
    emit activeVehicleChanged(newActiveVehicle);
    
    if (_activeVehicle) {
138
        _activeVehicle->setActive(true);
139
        emit activeVehicleAvailableChanged(true);
140
        if (_activeVehicle->autopilotPlugin()->parametersReady()) {
141 142 143 144 145 146 147 148 149 150
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
    
    _vehicleBeingDeleted->deleteLater();
}

void MultiVehicleManager::setActiveVehicle(Vehicle* vehicle)
{
    if (vehicle != _activeVehicle) {
Don Gagne's avatar
Don Gagne committed
151
        if (_activeVehicle) {
152 153
            _activeVehicle->setActive(false);
            
Don Gagne's avatar
Don Gagne committed
154 155 156 157 158 159 160 161 162 163
            // The sequence of signals is very important in order to not leave Qml elements connected
            // to a non-existent vehicle.
            
            // 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);
        }
164 165 166 167 168 169 170 171 172 173 174 175 176 177 178
        
        // See explanation in _deleteVehiclePhase1
        _vehicleBeingSetActive = vehicle;
        QTimer::singleShot(20, this, &MultiVehicleManager::_setActiveVehiclePhase2);
    }
}

void MultiVehicleManager::_setActiveVehiclePhase2(void)
{
    // Now we signal the new active vehicle
    _activeVehicle = _vehicleBeingSetActive;
    emit activeVehicleChanged(_activeVehicle);
    
    // And finally vehicle availability
    if (_activeVehicle) {
179
        _activeVehicle->setActive(true);
180 181 182
        _activeVehicleAvailable = true;
        emit activeVehicleAvailableChanged(true);
        
183
        if (_activeVehicle->autopilotPlugin()->parametersReady()) {
184 185 186 187 188 189
            _parameterReadyVehicleAvailable = true;
            emit parameterReadyVehicleAvailableChanged(true);
        }
    }
}

190
void MultiVehicleManager::_autopilotParametersReadyChanged(bool parametersReady)
191 192 193 194 195 196 197 198
{
    AutoPilotPlugin* autopilot = dynamic_cast<AutoPilotPlugin*>(sender());
    
    if (!autopilot) {
        qWarning() << "Dynamic cast failed!";
        return;
    }
    
Don Gagne's avatar
Don Gagne committed
199
    if (autopilot->vehicle() == _activeVehicle) {
200 201
        _parameterReadyVehicleAvailable = parametersReady;
        emit parameterReadyVehicleAvailableChanged(parametersReady);
202 203 204 205 206
    }
}

void MultiVehicleManager::setHomePositionForAllVehicles(double lat, double lon, double alt)
{
207 208
    for (int i=0; i< _vehicles.count(); i++) {
        qobject_cast<Vehicle*>(_vehicles[i])->uas()->setHomePosition(lat, lon, alt);
209 210 211
    }
}

212 213 214 215 216 217 218 219 220 221 222
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();
}
223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245

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

QList<Vehicle*> MultiVehicleManager::vehicles(void)
{
    QList<Vehicle*> list;
    
    for (int i=0; i< _vehicles.count(); i++) {
        list += qobject_cast<Vehicle*>(_vehicles[i]);
    }
    
    return list;
}