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

FwPosControl: Update behavior of navigating to a waypoint when the previous waypoint is not valid. Go along the line of the current aircraft position to the desired waypoint.

This commit is contained in:
Konrad 2023-02-07 14:53:43 +01:00 committed by Silvan Fuhrer
parent d5025810b4
commit de4b139540
1 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

@ -1026,10 +1026,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
prev_wp(1) = pos_sp_prev.lon;
} else {
// No valid previous waypoint, go for the current wp.
// This is automatically handled by the NPFG libraries.
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
// No valid previous waypoint, go along the line between aircraft and current waypoint
prev_wp = curr_pos;
}
float tecs_fw_thr_min;
@ -1186,10 +1184,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
prev_wp(1) = pos_sp_prev.lon;
} else {
// No valid previous waypoint, go for the current wp.
// This is automatically handled by the NPFG libraries.
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
// No valid previous waypoint, go along the line between aircraft and current waypoint
prev_wp = curr_pos;
}
float airspeed_sp = -1.f;