fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 07:51:37 +08:00
Code Issues Packages Projects Releases Wiki 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.

Browse Source
This commit is contained in:
Konrad
2023-02-09 17:51:55 +01:00
committed by Silvan Fuhrer
parent d5025810b4
commit de4b139540
1 changed files with 4 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
+4 -8
View File
@@ -1026,10 +1026,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
prev_wp(1) = pos_sp_prev.lon; prev_wp(1) = pos_sp_prev.lon;
} else { } else {
// No valid previous waypoint, go for the current wp. // No valid previous waypoint, go along the line between aircraft and current waypoint
// This is automatically handled by the NPFG libraries. prev_wp = curr_pos;
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
} }
float tecs_fw_thr_min; 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; prev_wp(1) = pos_sp_prev.lon;
} else { } else {
// No valid previous waypoint, go for the current wp. // No valid previous waypoint, go along the line between aircraft and current waypoint
// This is automatically handled by the NPFG libraries. prev_wp = curr_pos;
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
} }
float airspeed_sp = -1.f; float airspeed_sp = -1.f;