Skip to content

Commit

Permalink
FW Position Control: make explicit when underspeed detection logic is…
Browse files Browse the repository at this point in the history
… en-/disabled

Signed-off-by: Silvan Fuhrer <[email protected]>
  • Loading branch information
sfuhrer committed Aug 12, 2024
1 parent 70010df commit 4d7b014
Showing 1 changed file with 22 additions and 9 deletions.
31 changes: 22 additions & 9 deletions src/modules/fw_pos_control/FixedwingPositionControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -969,6 +969,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
// but not letting it drift too far away.
const float descend_rate = -0.5f;
const bool disable_underspeed_handling = false;

tecs_update_pitch_throttle(control_interval,
_current_altitude,
Expand All @@ -979,7 +980,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
disable_underspeed_handling,
descend_rate);

_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
Expand Down Expand Up @@ -1161,6 +1162,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;

_att_sp.yaw_body = _yaw;
const bool disable_underspeed_handling = false;

tecs_update_pitch_throttle(control_interval,
position_sp_alt,
Expand All @@ -1171,7 +1173,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
disable_underspeed_handling,
pos_sp_curr.vz);
}

Expand Down Expand Up @@ -1516,6 +1518,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_tecs.resetIntegrals();
}

const bool disable_underspeed_handling = true;

tecs_update_pitch_throttle(control_interval,
altitude_setpoint_amsl,
target_airspeed,
Expand All @@ -1525,7 +1529,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_performance_model.getMaximumClimbRate(_air_density),
true); // disable underspeed handling
disable_underspeed_handling);

_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation

Expand Down Expand Up @@ -1594,6 +1598,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo

const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ?
_param_fw_thr_idle.get() : _param_fw_thr_max.get();
const bool disable_underspeed_handling = true;

tecs_update_pitch_throttle(control_interval,
altitude_setpoint_amsl,
Expand All @@ -1604,7 +1609,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
max_takeoff_throttle,
_param_sinkrate_target.get(),
_performance_model.getMaximumClimbRate(_air_density),
true); // disable underspeed handling
disable_underspeed_handling);

if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
// explicitly set idle throttle until motors are enabled
Expand Down Expand Up @@ -1772,6 +1777,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() +
(1.0f - flare_ramp_interpolator_sqrt) *
_param_fw_thr_max.get();
const bool disable_underspeed_handling = true;

tecs_update_pitch_throttle(control_interval,
altitude_setpoint,
Expand All @@ -1782,7 +1788,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
true,
disable_underspeed_handling,
height_rate_setpoint);

/* set the attitude and throttle commands */
Expand Down Expand Up @@ -1975,6 +1981,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() +
(1.0f - flare_ramp_interpolator_sqrt) *
_param_fw_thr_max.get();
const bool disable_underspeed_handling = true;

tecs_update_pitch_throttle(control_interval,
_current_altitude, // is not controlled, control descend rate
Expand All @@ -1985,7 +1992,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
true,
disable_underspeed_handling,
height_rate_setpoint);

/* set the attitude and throttle commands */
Expand Down Expand Up @@ -2031,6 +2038,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f);
const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()),
_param_fw_t_sink_max.get());
const float disable_underspeed_handling = false;

tecs_update_pitch_throttle(control_interval,
_current_altitude, // is not controlled, control descend rate
target_airspeed,
Expand All @@ -2040,7 +2049,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
_param_fw_thr_max.get(),
desired_max_sinkrate,
_param_climbrate_target.get(),
false,
disable_underspeed_handling,
-glide_slope_sink_rate); // heightrate = -sinkrate

/* set the attitude and throttle commands */
Expand Down Expand Up @@ -2093,6 +2102,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
throttle_max = 0.0f;
}

const bool disable_underspeed_handling = false;

tecs_update_pitch_throttle(control_interval,
_current_altitude,
calibrated_airspeed_sp,
Expand All @@ -2102,7 +2113,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
disable_underspeed_handling,
height_rate_sp);

_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
Expand Down Expand Up @@ -2185,6 +2196,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
}
}

const bool disable_underspeed_handling = false;

tecs_update_pitch_throttle(control_interval,
_current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude.
calibrated_airspeed_sp,
Expand All @@ -2194,7 +2207,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
disable_underspeed_handling,
height_rate_sp);

if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH ||
Expand Down

0 comments on commit 4d7b014

Please sign in to comment.