Browse Source

fw pos ctrl: head straight for next waypoint after takeoff

main
Thomas Stastny 3 years ago committed by Daniel Agar
parent
commit
2d5f1a5c6b
  1. 16
      src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

16
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@ -1092,15 +1092,13 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co @@ -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 @@ -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;
}

Loading…
Cancel
Save