dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-11-27 14:37:55 +01:00
parent 6a34b63b60
commit 2734c44533
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -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;