diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index c3499ff3a4..1c42f5838c 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -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,