diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 7e5cdbfa2406b9b1f19bffed3e22e07494e5fe83..a83db20f8903dd29a48de24ad446bc80edc0f06e 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -109,7 +109,7 @@ void MultiVehicleManager::_deleteVehiclePhase1(Vehicle* vehicle) } vehicle->setActive(false); - vehicle->uas()->clearVehicle(); + vehicle->uas()->shutdownVehicle(); // First we must signal that a vehicle is no longer available. _activeVehicleAvailable = false; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 9aa7a11012789c939f1e1b5d96c1d497301f4e73..26d4654daa44dbd9013625079283e9561316bfdc 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1057,7 +1057,7 @@ void Vehicle::setFlightMode(const QString& flightMode) mavlink_msg_set_mode_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, id(), newBaseMode, custom_mode); sendMessage(msg); } else { - qCWarning(VehicleLog) << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode; + qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode; } } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 7e8be3234bc41609c10ac02826f00c5366c51e66..a6b658107358f6eed8bab559f9ca9afe8767eaa8 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -193,23 +193,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi statusTimeout.start(500); } -/** -* Saves the settings of name, airframe, autopilot type and battery specifications -* by calling writeSettings. -*/ -UAS::~UAS() -{ -#ifndef __mobile__ - stopHil(); - if (simulation) { - // wait for the simulator to exit - simulation->wait(); - simulation->disconnectSimulation(); - simulation->deleteLater(); - } -#endif -} - /** * @ return the id of the uas */ @@ -2216,3 +2199,17 @@ void UAS::_say(const QString& text, int severity) if (!qgcApp()->runningUnitTests()) qgcApp()->toolbox()->audioOutput()->say(text, severity); } + +void UAS::shutdownVehicle(void) +{ +#ifndef __mobile__ + stopHil(); + if (simulation) { + // wait for the simulator to exit + simulation->wait(); + simulation->disconnectSimulation(); + simulation->deleteLater(); + } +#endif + _vehicle = NULL; +} diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 80b35954eaf38c7846bf19087f215d7a4c8a3ef8..fe5e957d7206ed427568f42754eb8fbf3587d33d 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -64,7 +64,6 @@ class UAS : public UASInterface Q_OBJECT public: UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager); - ~UAS(); float lipoFull; ///< 100% charged voltage float lipoEmpty; ///< Discharged voltage @@ -100,7 +99,8 @@ public: Q_PROPERTY(double satRawVDOP READ getSatRawVDOP NOTIFY satRawVDOPChanged) Q_PROPERTY(double satRawCOG READ getSatRawCOG NOTIFY satRawCOGChanged) - void clearVehicle(void) { _vehicle = NULL; } + /// Vehicle is about to go away + void shutdownVehicle(void); void setGroundSpeed(double val) {