Commit 064a34c6 authored by Lorenz Meier's avatar Lorenz Meier

Firmware plugin: Sequence guided mode correctly, widen modes in which guided...

Firmware plugin: Sequence guided mode correctly, widen modes in which guided support should be available
parent 0a84814f
......@@ -117,8 +117,6 @@ QStringList PX4FirmwarePlugin::flightModes(void)
{
QStringList flightModes;
// FIXME: fixed-wing/multi-rotor differences?
for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
const struct Modes2Name* pModes2Name = &rgModes2Name[i];
......@@ -138,8 +136,6 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c
union px4_custom_mode px4_mode;
px4_mode.data = custom_mode;
// FIXME: fixed-wing/multi-rotor differences?
bool found = false;
for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
const struct Modes2Name* pModes2Name = &rgModes2Name[i];
......@@ -265,8 +261,9 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
// kick it into hold mode
vehicle->setFlightMode(pauseFlightMode);
QGC::SLEEP::msleep(200);
// then tell it to loiter at the current position
// above the takeoff (home) position
mavlink_message_t msg;
mavlink_command_long_t cmd;
......@@ -306,19 +303,16 @@ 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);
MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
// then tell it to loiter at the user-selected location
// above the takeoff (home) position
// Set destination altitude
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.param2 = 0.0f;
cmd.param3 = 0.0f;
cmd.param4 = NAN;
cmd.param5 = NAN;
......@@ -327,10 +321,13 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double 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);
QGC::SLEEP::msleep(200);
// trigger take off
vehicle->setFlightMode(takeoffFlightMode);
}
void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
......@@ -401,5 +398,6 @@ void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
{
// Not supported by generic vehicle
return (vehicle->flightMode() == pauseFlightMode);
return (vehicle->flightMode() == pauseFlightMode || vehicle->flightMode() == takeoffFlightMode
|| vehicle->flightMode() == landingFlightMode);
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment