Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

fw pos ctrl: head straight for next waypoint after takeoff

Browse Source
This commit is contained in:
Thomas Stastny
2022-06-13 23:38:46 -05:00
committed by Daniel Agar
parent 721131a135
commit 2d5f1a5c6b
1 changed files with 6 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+6 -10
View File
@@ -1092,15 +1092,13 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
/* current waypoint (the one currently heading for) */ /* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
if (pos_sp_prev.valid) { if (pos_sp_prev.valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
prev_wp(0) = pos_sp_prev.lat; prev_wp(0) = pos_sp_prev.lat;
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 for the current wp. // This is automatically handled by the L1/NPFG libraries.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = pos_sp_curr.lat; prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon; prev_wp(1) = pos_sp_curr.lon;
} }
@@ -1269,15 +1267,13 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
/* current waypoint (the one currently heading for) */ /* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
if (pos_sp_prev.valid) { if (pos_sp_prev.valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
prev_wp(0) = pos_sp_prev.lat; prev_wp(0) = pos_sp_prev.lat;
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 for the current wp. // This is automatically handled by the L1/NPFG libraries.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = pos_sp_curr.lat; prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon; prev_wp(1) = pos_sp_curr.lon;
} }