From 2734c44533dbfd360f4fbf4a5f3a47c237f8280f Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 27 Nov 2023 14:37:55 +0100 Subject: [PATCH] FlightTaskAuto: set state to None if prev and current sp are equal Otherwise if is set to Offtrack, which in turn leads to weird behavior. E.g. when triggering Land while flying fast forward, the vehcile doesn't descned to the land point it keeps getting a velocity setpoint from the smoother that pushes it away from the land point. Signed-off-by: Silvan Fuhrer --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 8168f0bca5..0ae583dd2f 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -636,7 +636,11 @@ State FlightTaskAuto::_getCurrentState() State return_state = State::none; - if (u_prev_to_target_xy * pos_to_target_xy < 0.0f) { + if (u_prev_to_target_xy.length() < FLT_EPSILON) { + // Previous and target are the same point, so we better don't try to do any special line following + return_state = State::none; + + } else if (u_prev_to_target_xy * pos_to_target_xy < 0.0f) { // Target is behind return_state = State::target_behind;