forked from Archive/PX4-Autopilot
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:
parent
d5025810b4
commit
de4b139540
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue