Skip to content

Commit

Permalink
AP_Landing: Never allow cruise speed or head wind modifications to ta…
Browse files Browse the repository at this point in the history
…rget airspeed when using parachute landing.
  • Loading branch information
MattKear committed May 29, 2024
1 parent 2470796 commit c035404
Showing 1 changed file with 8 additions and 3 deletions.
11 changes: 8 additions & 3 deletions libraries/AP_Landing/AP_Landing_Slope.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -404,6 +404,14 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void)
} else {
target_airspeed_cm = 0.5 * (aparm.airspeed_cruise_cm * 0.01 + aparm.airspeed_min);
}

const uint32_t max_airspeed_cm = AP_Landing::allow_max_airspeed_on_land() ? aparm.airspeed_max*100 : aparm.airspeed_cruise_cm;

// Enforce the use of landing speed for parachute landing
if (type == TYPE_PARACHUTE) {
return constrain_int32(target_airspeed_cm, target_airspeed_cm, max_airspeed_cm);
}

switch (type_slope_stage) {
case SlopeStage::NORMAL:
target_airspeed_cm = aparm.airspeed_cruise_cm;
Expand All @@ -422,9 +430,6 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void)
// when landing, add half of head-wind.
const float head_wind_comp = constrain_float(wind_comp, 0.0f, 100.0f)*0.01;
const int32_t head_wind_compensation_cm = head_wind() * head_wind_comp * 100;

const uint32_t max_airspeed_cm = AP_Landing::allow_max_airspeed_on_land() ? aparm.airspeed_max*100 : aparm.airspeed_cruise_cm;

return constrain_int32(target_airspeed_cm + head_wind_compensation_cm, target_airspeed_cm, max_airspeed_cm);

}
Expand Down

0 comments on commit c035404

Please sign in to comment.