diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 86221320c5dfaa39d9388384e58a339d95d8adc9..2ce96ceaecbba39fb4a4f66e411e838a1917d91a 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -139,7 +139,7 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle) void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) { - if (!_armVehicle(vehicle)) { + if (!_armVehicleAndValidate(vehicle)) { qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm.")); return; } diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 5ed512200a895f9d75340447e9ec12c907e9c674..056dd4eec601d2abf3535e54d08214fe1c119b6b 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -435,7 +435,7 @@ bool FirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* vehicle) return vehicle->multiRotor() ? false : true; } -bool FirmwarePlugin::_armVehicle(Vehicle* vehicle) +bool FirmwarePlugin::_armVehicleAndValidate(Vehicle* vehicle) { if (!vehicle->armed()) { vehicle->setArmed(true); diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 022317d573fa708592aba0ac3aedbea732e1422a..7f8ee7d98ef25b2161057401c04b9def24a943c3 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -303,7 +303,7 @@ public: protected: // Arms the vehicle, waiting for the arm state to change. // @return: true - vehicle armed, false - vehicle failed to arm - bool _armVehicle(Vehicle* vehicle); + bool _armVehicleAndValidate(Vehicle* vehicle); private: QVariantList _toolBarIndicatorList; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 65aa0ff451b5d6af7bb292a6eb9c844b50e57601..aa271e14c4277683400c3e3f88fa39a2ea419148 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -352,6 +352,29 @@ void PX4FirmwarePlugin::guidedModeOrbit(Vehicle* vehicle, const QGeoCoordinate& centerCoord.isValid() ? centerCoord.altitude() : NAN); } +void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle) +{ + Q_UNUSED(vehicleId); + Q_UNUSED(component); + Q_UNUSED(noReponseFromVehicle); + + Vehicle* vehicle = dynamic_cast(sender()); + if (!vehicle) { + qWarning() << "Dynamic cast failed!"; + return; + } + + if (command == MAV_CMD_NAV_TAKEOFF && result == MAV_RESULT_ACCEPTED) { + // Now that we are in takeoff mode we can arm the vehicle which will cause it to takeoff. + // We specifically don't retry arming if it fails. This way we don't fight with the user if + // They are trying to disarm. + disconnect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult); + if (!vehicle->armed()) { + vehicle->setArmed(true); + } + } +} + void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) { QString takeoffAltParam("MIS_TAKEOFF_ALT"); @@ -367,11 +390,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle) } Fact* takeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam); - if (!_armVehicle(vehicle)) { - qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm.")); - return; - } - + connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult); vehicle->sendMavCommand(vehicle->defaultComponentId(), MAV_CMD_NAV_TAKEOFF, true, // show error is fails @@ -436,7 +455,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu void PX4FirmwarePlugin::startMission(Vehicle* vehicle) { - if (!_armVehicle(vehicle)) { + if (!_armVehicleAndValidate(vehicle)) { qgcApp()->showMessage(tr("Unable to start mission: Vehicle failed to arm.")); return; } diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 9e38ad582644c87b70994fb741321d6abe1aa502..67f9caf8ea7b9b910581541341d7f1e165897c6e 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -103,6 +103,9 @@ protected: QString _followMeFlightMode; QString _simpleFlightMode; +private slots: + void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle); + private: void _handleAutopilotVersion(Vehicle* vehicle, mavlink_message_t* message);