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

fw pos ctrl: head straight for next waypoint after takeoff

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -1092,15 +1092,13 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
/* current waypoint (the one currently heading for) */
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(1) = pos_sp_prev.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
// No valid previous waypoint, go for the current wp.
// This is automatically handled by the L1/NPFG libraries.
prev_wp(0) = pos_sp_curr.lat;
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) */
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(1) = pos_sp_prev.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
// No valid previous waypoint, go for the current wp.
// This is automatically handled by the L1/NPFG libraries.
prev_wp(0) = pos_sp_curr.lat;
prev_wp(1) = pos_sp_curr.lon;
}