From 9e423bea60f273f2775be620f55c912503487584 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Apr 2016 11:56:32 +0200 Subject: [PATCH 1/5] Support REPOSITION MAVLink command in PX4 --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 113 +++++++++++++++++++++++++++- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 8 +- 2 files changed, 119 insertions(+), 2 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index d28f8b6..e2740eb 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -26,6 +26,7 @@ #include "PX4FirmwarePlugin.h" #include "PX4ParameterMetaData.h" +#include "QGCApplication.h" #include "AutoPilotPlugins/PX4/PX4AutoPilotPlugin.h" // FIXME: Hack #include @@ -198,7 +199,7 @@ int PX4FirmwarePlugin::manualControlReservedButtonCount(void) bool PX4FirmwarePlugin::isCapable(FirmwareCapabilities capabilities) { - return (capabilities & (MavCmdPreflightStorageCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities; + return (capabilities & (MavCmdPreflightStorageCapability | GuidedModeCapability | SetFlightModeCapability | PauseVehicleCapability)) == capabilities; } void PX4FirmwarePlugin::initializeVehicle(Vehicle* vehicle) @@ -262,3 +263,113 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) { vehicle->setFlightMode(pauseFlightMode); } + +void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) +{ + vehicle->setFlightMode(rtlFlightMode); +} + +void PX4FirmwarePlugin::guidedModeLand(Vehicle* vehicle) +{ + vehicle->setFlightMode(landingFlightMode); +} + +void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) +{ + Q_UNUSED(altitudeRel); + if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) { + qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known.")); + return; + } + + vehicle->setFlightMode(takeoffFlightMode); + + // For later use +#if 0 + mavlink_message_t msg; + mavlink_command_int_t cmd = {}; + + cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF; + cmd.confirmation = 0; + cmd.frame = MAV_FRAME_LOCAL_OFFSET_NED; + cmd.param1 = 0.0f; + cmd.param2 = 0.0f; + cmd.param3 = 0.0f; + cmd.param4 = 0.0f; + cmd.param5 = 0.0f; + cmd.param6 = 0.0f; + cmd.param7 = -altitudeRel; // AMSL meters + cmd.target_system = vehicle->id(); + cmd.target_component = 0; + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_msg_command_int_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + + vehicle->sendMessage(msg); +#endif +} + +void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) +{ + if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { + qgcApp()->showMessage(QStringLiteral("Unable to go to location, vehicle position not known.")); + return; + } + + mavlink_message_t msg; + mavlink_command_long_t cmd = {}; + + cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; + cmd.confirmation = 0; + cmd.param1 = -1.0f; + cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; + cmd.param3 = 0.0f; + cmd.param4 = NAN; + cmd.param5 = gotoCoord.latitude() * 1e7; + cmd.param6 = gotoCoord.longitude() * 1e7; + cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble(); + cmd.target_system = vehicle->id(); + cmd.target_component = 0; + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + + vehicle->sendMessage(msg); +} + +void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) +{ + if (qIsNaN(vehicle->altitudeRelative()->rawValue().toDouble())) { + qgcApp()->showMessage(QStringLiteral("Unable to change altitude, vehicle altitude not known.")); + return; + } + + mavlink_message_t msg; + mavlink_command_long_t cmd = {}; + + cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; + cmd.confirmation = 0; + cmd.param1 = -1.0f; + cmd.param2 = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE; + cmd.param3 = 0.0f; + cmd.param4 = NAN; + cmd.param5 = NAN; + cmd.param6 = NAN; + cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel; + cmd.target_system = vehicle->id(); + cmd.target_component = 0; + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + + vehicle->sendMessage(msg); +} + +void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) +{ + if (guidedMode) { + vehicle->setFlightMode("Pause"); + } else { + pauseVehicle(vehicle); + } +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 2185809..988b57f 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -45,7 +45,13 @@ public: QStringList flightModes (void) final; QString flightMode (uint8_t base_mode, uint32_t custom_mode) const final; bool setFlightMode (const QString& flightMode, uint8_t* base_mode, uint32_t* custom_mode) final; - void pauseVehicle (Vehicle* vehicle) final; + void setGuidedMode(Vehicle* vehicle, bool guidedMode) final; + void pauseVehicle(Vehicle* vehicle) final; + void guidedModeRTL(Vehicle* vehicle) final; + void guidedModeLand(Vehicle* vehicle) final; + void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final; + void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; + void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final; int manualControlReservedButtonCount(void) final; void initializeVehicle (Vehicle* vehicle) final; bool sendHomePositionToVehicle (void) final; From 3a415a88e4e3073c32191844ea430a3dbfc5f6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Apr 2016 17:28:54 +0200 Subject: [PATCH 2/5] Teach QGC to detect the PX4 guided mode. --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 34 +++++++++++++++++------------ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + 2 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index e2740eb..2753056 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -282,31 +282,31 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) return; } + // tell the system first to take off in its internal, + // airframe specific takeoff action vehicle->setFlightMode(takeoffFlightMode); - // For later use -#if 0 + // then tell it to loiter at the user-selected location + // above the takeoff (home) position mavlink_message_t msg; - mavlink_command_int_t cmd = {}; + mavlink_command_long_t cmd = {}; - cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF; + cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; cmd.confirmation = 0; - cmd.frame = MAV_FRAME_LOCAL_OFFSET_NED; - cmd.param1 = 0.0f; - cmd.param2 = 0.0f; + cmd.param1 = -1.0f; + cmd.param2 = 0.0; cmd.param3 = 0.0f; - cmd.param4 = 0.0f; - cmd.param5 = 0.0f; - cmd.param6 = 0.0f; - cmd.param7 = -altitudeRel; // AMSL meters + cmd.param4 = NAN; + cmd.param5 = NAN; + cmd.param6 = NAN; + cmd.param7 = vehicle->altitudeAMSL()->rawValue().toDouble() + altitudeRel; cmd.target_system = vehicle->id(); cmd.target_component = 0; MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); - mavlink_msg_command_int_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); vehicle->sendMessage(msg); -#endif } void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) @@ -368,8 +368,14 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode) { if (guidedMode) { - vehicle->setFlightMode("Pause"); + vehicle->setFlightMode(pauseFlightMode); } else { pauseVehicle(vehicle); } } + +bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const +{ + // Not supported by generic vehicle + return (vehicle->flightMode() == pauseFlightMode); +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 988b57f..d41af30 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h @@ -52,6 +52,7 @@ public: void guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) final; void guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord) final; void guidedModeChangeAltitude(Vehicle* vehicle, double altitudeRel) final; + bool isGuidedMode(const Vehicle* vehicle) const; int manualControlReservedButtonCount(void) final; void initializeVehicle (Vehicle* vehicle) final; bool sendHomePositionToVehicle (void) final; From 3e6a3218d9246be6e399c3245c4171187364a16a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Apr 2016 18:02:29 +0200 Subject: [PATCH 3/5] Implement proper Pause at current position --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 2753056..e349c37 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -79,7 +79,7 @@ const char* PX4FirmwarePlugin::posCtlFlightMode = "Position Control"; const char* PX4FirmwarePlugin::offboardFlightMode = "Offboard Control"; const char* PX4FirmwarePlugin::readyFlightMode = "Ready"; const char* PX4FirmwarePlugin::takeoffFlightMode = "Takeoff"; -const char* PX4FirmwarePlugin::pauseFlightMode = "Pause"; +const char* PX4FirmwarePlugin::pauseFlightMode = "Hold"; const char* PX4FirmwarePlugin::missionFlightMode = "Mission"; const char* PX4FirmwarePlugin::rtlFlightMode = "Return To Land"; const char* PX4FirmwarePlugin::landingFlightMode = "Landing"; @@ -261,7 +261,30 @@ QObject* PX4FirmwarePlugin::loadParameterMetaData(const QString& metaDataFile) void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) { + // kick it into hold mode vehicle->setFlightMode(pauseFlightMode); + + // then tell it to loiter at the current position + // above the takeoff (home) position + mavlink_message_t msg; + mavlink_command_long_t cmd = {}; + + cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; + cmd.confirmation = 0; + cmd.param1 = -1.0f; + cmd.param2 = 0.0; + cmd.param3 = 0.0f; + cmd.param4 = NAN; + cmd.param5 = NAN; + cmd.param6 = NAN; + cmd.param7 = NAN; + cmd.target_system = vehicle->id(); + cmd.target_component = 0; + + MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol(); + mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + + vehicle->sendMessage(msg); } void PX4FirmwarePlugin::guidedModeRTL(Vehicle* vehicle) From 0138c03961281175fc70e3b6c06b64944407cd4e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Apr 2016 18:21:07 +0200 Subject: [PATCH 4/5] PX4: Compile fixes --- src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index e349c37..e596d83 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc @@ -267,7 +267,7 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle) // then tell it to loiter at the current position // above the takeoff (home) position mavlink_message_t msg; - mavlink_command_long_t cmd = {}; + mavlink_command_long_t cmd; cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; cmd.confirmation = 0; @@ -312,7 +312,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel) // then tell it to loiter at the user-selected location // above the takeoff (home) position mavlink_message_t msg; - mavlink_command_long_t cmd = {}; + mavlink_command_long_t cmd; cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; cmd.confirmation = 0; @@ -340,7 +340,7 @@ void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoord } mavlink_message_t msg; - mavlink_command_long_t cmd = {}; + mavlink_command_long_t cmd; cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; cmd.confirmation = 0; @@ -368,7 +368,7 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu } mavlink_message_t msg; - mavlink_command_long_t cmd = {}; + mavlink_command_long_t cmd; cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION; cmd.confirmation = 0; From 27e727f9b9291a223d807cf4efddc7e18b69fc0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Apr 2016 18:21:21 +0200 Subject: [PATCH 5/5] APM: Compile fixes --- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 748e555..b4906e9 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -134,7 +134,7 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitu } mavlink_message_t msg; - mavlink_command_long_t cmd = {}; + mavlink_command_long_t cmd; cmd.command = (uint16_t)MAV_CMD_NAV_TAKEOFF; cmd.confirmation = 0;