fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

FWLatLong: fix resetting of _min_airspeed_from_guidance

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2025-05-20 15:48:36 +02:00
parent 23a4854e82
commit cb2c711e58
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View File

@ -238,7 +238,9 @@ void FwLateralLongitudinalControl::Run()
float lateral_accel_sp {NAN};
const Vector2f airspeed_vector = _lateral_control_state.ground_speed - _lateral_control_state.wind_speed;
if (PX4_ISFINITE(_lat_control_sp.course)) {
if (PX4_ISFINITE(_lat_control_sp.course) && !PX4_ISFINITE(_lat_control_sp.airspeed_direction)) {
// only use the course setpoint if it's finite but airspeed_direction is not
airspeed_direction_sp = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(
_lat_control_sp.course, _lateral_control_state.wind_speed,
airspeed_sp_eas);
@ -251,14 +253,16 @@ void FwLateralLongitudinalControl::Run()
max_true_airspeed, _param_fw_gnd_spd_min.get())
/ _long_control_state.eas2tas;
} else if (PX4_ISFINITE(_lat_control_sp.airspeed_direction)) {
// If the airspeed_direction is finite we use that instead of the course.
airspeed_direction_sp = _lat_control_sp.airspeed_direction;
_min_airspeed_from_guidance = 0.f; // reset if no longer in course control
} else {
_min_airspeed_from_guidance = 0.f; // reset if no longer in course control
}
if (PX4_ISFINITE(_lat_control_sp.airspeed_direction)) {
airspeed_direction_sp = _lat_control_sp.airspeed_direction;
}
if (PX4_ISFINITE(airspeed_direction_sp)) {
const float heading = atan2f(airspeed_vector(1), airspeed_vector(0));
lateral_accel_sp = _airspeed_direction_control.controlHeading(airspeed_direction_sp, heading,