From 8714b216b0a074b487b72495016a67524d9a71af Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 22 Aug 2018 12:47:20 +0200 Subject: [PATCH] FlightTaskFailsafe: comments and land with land speed if velocity in z is valid but altitude not --- src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp index 011ce0d4c8..71b7548f7f 100644 --- a/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp +++ b/src/lib/FlightTasks/tasks/Failsafe/FlightTaskFailsafe.cpp @@ -41,7 +41,7 @@ bool FlightTaskFailsafe::activate() bool ret = FlightTask::activate(); _position_setpoint = _position; _velocity_setpoint *= 0.0f; - _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.5f); + _thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -MPC_THR_HOVER.get() * 0.6f); _yaw_setpoint = _yaw; _yawspeed_setpoint = 0.0f; return ret; @@ -50,19 +50,24 @@ bool FlightTaskFailsafe::activate() bool FlightTaskFailsafe::update() { if (PX4_ISFINITE(_position(0)) && PX4_ISFINITE(_position(1))) { + // stay at current position setpoint _velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f; _thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f; } else if (PX4_ISFINITE(_velocity(0)) && PX4_ISFINITE(_velocity(1))) { + // don't move along xy _position_setpoint(0) = _position_setpoint(1) = NAN; _thrust_setpoint(0) = _thrust_setpoint(1) = NAN; } if (PX4_ISFINITE(_position(2))) { + // stay at current altitude setpoint _velocity_setpoint(2) = 0.0f; _thrust_setpoint(2) = NAN; } else if (PX4_ISFINITE(_velocity(2))) { + // land with landspeed + _velocity_setpoint(2) = MPC_LAND_SPEED.get(); _position_setpoint(2) = NAN; _thrust_setpoint(2) = NAN; }