Commit 16cd2a52 authored by DonLakeFlyer's avatar DonLakeFlyer

Specify altitude for guided takeoff

parent 97891f2d
......@@ -906,30 +906,40 @@ void APMFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
vehicle->sendMessageOnLink(vehicle->priorityLink(), msg);
}
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
void APMFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{
_guidedModeTakeoff(vehicle);
_guidedModeTakeoff(vehicle, altitudeRel);
}
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{
if (!vehicle->multiRotor() && !vehicle->vtol()) {
qgcApp()->showMessage(tr("Vehicle does not support guided takeoff"));
return false;
}
double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
if (qIsNaN(vehicleAltitudeAMSL)) {
qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
return false;
}
QString takeoffAltParam(vehicle->vtol() ? QStringLiteral("Q_RTL_ALT") : QStringLiteral("PILOT_TKOFF_ALT"));
float paramDivisor = vehicle->vtol() ? 1.0 : 100.0; // PILOT_TAKEOFF_ALT is in centimeters
float takeoffAlt = 0;
float takeoffAltRel = 0;
if (vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
takeoffAlt = takeoffAltFact->rawValue().toDouble();
takeoffAltRel = takeoffAltFact->rawValue().toDouble();
}
if (takeoffAlt <= 0) {
takeoffAlt = 2.5;
if (takeoffAltRel <= 0) {
takeoffAltRel = 2.5;
} else {
takeoffAlt /= paramDivisor; // centimeters -> meters
takeoffAltRel /= paramDivisor; // centimeters -> meters
}
if (!qIsNaN(altitudeRel) && altitudeRel > takeoffAltRel) {
takeoffAltRel = altitudeRel;
}
if (!_setFlightModeAndValidate(vehicle, "Guided")) {
......@@ -947,7 +957,7 @@ bool APMFirmwarePlugin::_guidedModeTakeoff(Vehicle* vehicle)
MAV_CMD_NAV_TAKEOFF,
true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
takeoffAlt);
takeoffAltRel); // Relative altitude
return true;
}
......@@ -958,7 +968,7 @@ void APMFirmwarePlugin::startMission(Vehicle* vehicle)
double currentAlt = vehicle->altitudeRelative()->rawValue().toDouble();
if (!vehicle->flying()) {
if (_guidedModeTakeoff(vehicle)) {
if (_guidedModeTakeoff(vehicle, qQNaN())) {
// Wait for vehicle to get off ground before switching to auto (10 seconds)
bool didTakeoff = false;
......
......@@ -76,7 +76,7 @@ public:
AutoPilotPlugin* autopilotPlugin (Vehicle* vehicle) override;
bool isCapable (const Vehicle *vehicle, FirmwareCapabilities capabilities) override;
void setGuidedMode (Vehicle* vehicle, bool guidedMode) override;
void guidedModeTakeoff (Vehicle* vehicle) override;
void guidedModeTakeoff (Vehicle* vehicle, double altitudeRel) override;
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
void startMission (Vehicle* vehicle) override;
QStringList flightModes (Vehicle* vehicle) override;
......@@ -123,7 +123,7 @@ private:
void _handleIncomingHeartbeat(Vehicle* vehicle, mavlink_message_t* message);
void _handleOutgoingParamSet(Vehicle* vehicle, LinkInterface* outgoingLink, mavlink_message_t* message);
void _soloVideoHandshake(Vehicle* vehicle);
bool _guidedModeTakeoff(Vehicle* vehicle);
bool _guidedModeTakeoff(Vehicle* vehicle, double altitudeRel);
// Any instance data here must be global to all vehicles
// Vehicle specific data should go into APMFirmwarePluginInstanceData
......
......@@ -256,10 +256,11 @@ void FirmwarePlugin::guidedModeLand(Vehicle* vehicle)
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
void FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
{
// Not supported by generic vehicle
Q_UNUSED(vehicle);
Q_UNUSED(takeoffAltRel);
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
}
......
......@@ -128,7 +128,7 @@ public:
virtual void guidedModeLand(Vehicle* vehicle);
/// Command vehicle to takeoff from current location to a firmware specific height.
virtual void guidedModeTakeoff(Vehicle* vehicle);
virtual void guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel);
/// Command the vehicle to start the mission
virtual void startMission(Vehicle* vehicle);
......
......@@ -383,11 +383,12 @@ void PX4FirmwarePlugin::_mavCommandResult(int vehicleId, int component, int comm
}
}
void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double takeoffAltRel)
{
QString takeoffAltParam("MIS_TAKEOFF_ALT");
if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
double vehicleAltitudeAMSL = vehicle->altitudeAMSL()->rawValue().toDouble();
if (qIsNaN(vehicleAltitudeAMSL)) {
qgcApp()->showMessage(tr("Unable to takeoff, vehicle position not known."));
return;
}
......@@ -396,7 +397,9 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
qgcApp()->showMessage(tr("Unable to takeoff, MIS_TAKEOFF_ALT parameter missing."));
return;
}
Fact* takeoffAlt = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
double takeoffAltRelFromVehicle = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam)->rawValue().toDouble();
double takeoffAltAMSL = qMax(takeoffAltRel, takeoffAltRelFromVehicle) + vehicleAltitudeAMSL;
connect(vehicle, &Vehicle::mavCommandResult, this, &PX4FirmwarePlugin::_mavCommandResult);
vehicle->sendMavCommand(vehicle->defaultComponentId(),
......@@ -405,7 +408,7 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
-1, // No pitch requested
0, 0, // param 2-4 unused
NAN, NAN, NAN, // No yaw, lat, lon
vehicle->altitudeAMSL()->rawValue().toDouble() + takeoffAlt->rawValue().toDouble());
takeoffAltAMSL); // AMSL altitude
}
void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
......
......@@ -45,7 +45,7 @@ public:
void pauseVehicle (Vehicle* vehicle) override;
void guidedModeRTL (Vehicle* vehicle) override;
void guidedModeLand (Vehicle* vehicle) override;
void guidedModeTakeoff (Vehicle* vehicle) override;
void guidedModeTakeoff (Vehicle* vehicle, double takeoffAltRel) override;
void guidedModeOrbit (Vehicle* vehicle, const QGeoCoordinate& centerCoord = QGeoCoordinate(), double radius = NAN, double velocity = NAN, double altitude = NAN) override;
void guidedModeGotoLocation (Vehicle* vehicle, const QGeoCoordinate& gotoCoord) override;
void guidedModeChangeAltitude (Vehicle* vehicle, double altitudeRel) override;
......
......@@ -208,6 +208,8 @@ Item {
confirmDialog.title = takeoffTitle
confirmDialog.message = takeoffMessage
confirmDialog.hideTrigger = Qt.binding(function() { return !showTakeoff })
altitudeSlider.reset()
altitudeSlider.visible = true
break;
case actionStartMission:
confirmDialog.title = startMissionTitle
......@@ -302,7 +304,7 @@ Item {
_activeVehicle.guidedModeLand()
break
case actionTakeoff:
_activeVehicle.guidedModeTakeoff()
_activeVehicle.guidedModeTakeoff(actionData)
break
case actionResumeMission:
case actionResumeMissionUploadFail:
......
......@@ -2222,14 +2222,14 @@ void Vehicle::guidedModeLand(void)
_firmwarePlugin->guidedModeLand(this);
}
void Vehicle::guidedModeTakeoff(void)
void Vehicle::guidedModeTakeoff(double altitudeRelative)
{
if (!guidedModeSupported()) {
qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
return;
}
setGuidedMode(true);
_firmwarePlugin->guidedModeTakeoff(this);
_firmwarePlugin->guidedModeTakeoff(this, altitudeRelative);
}
void Vehicle::startMission(void)
......
......@@ -386,7 +386,7 @@ public:
Q_INVOKABLE void guidedModeLand(void);
/// Command vehicle to takeoff from current location
Q_INVOKABLE void guidedModeTakeoff(void);
Q_INVOKABLE void guidedModeTakeoff(double altitudeRelative);
/// Command vehicle to move to specified location (altitude is included and relative)
Q_INVOKABLE void guidedModeGotoLocation(const QGeoCoordinate& gotoCoord);
......
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