diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index faadbe054d..d529d326c5 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1269,6 +1269,21 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const wp_distance_save = 0.0f; } + // create virtual waypoint which is on the desired flight path but + // some distance behind landing waypoint. This will make sure that the plane + // will always follow the desired flight path even if we get close or past + // the landing waypoint + if (pos_sp_prev.valid) { + double lat = pos_sp_curr.lat; + double lon = pos_sp_curr.lon; + + create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon, + pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon); + + curr_wp(0) = (float)lat; + curr_wp(1) = (float)lon; + } + // we want the plane to keep tracking the desired flight path until we start flaring // if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal && @@ -1305,9 +1320,6 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const /* Vertical landing control */ /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; - float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; // default to no terrain estimation, just use landing waypoint altitude float terrain_alt = pos_sp_curr.alt; @@ -1396,6 +1408,9 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const _land_stayonground = true; } + const float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min; + const float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), radians(_parameters.land_flare_pitch_min_deg), @@ -1457,6 +1472,8 @@ FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const } } + const float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), radians(_parameters.pitch_limit_min),