Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

FW Position Control: improve auto takeoff behavior without launch detection #23526

Merged
merged 3 commits into from
Aug 13, 2024
Merged

FW Position Control: improve auto takeoff behavior without launch detection #23526

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1b2c6d8
FW Position control: do not invalidate airspeed from negative readings
sfuhrer Aug 9, 2024
2ec5d93
FW Position Control: disable underspeed handling during auto takeoff
sfuhrer Aug 9, 2024
a25ed79
FW Position Control: make explicit when underspeed detection logic is…
sfuhrer Aug 12, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 25 additions & 11 deletions src/modules/fw_pos_control/FixedwingPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -212,8 +212,7 @@ FixedwingPositionControl::airspeed_poll()
_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed

if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
&& (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) {
&& PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {

airspeed_valid = true;

Expand Down Expand Up @@ -974,6 +973,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 @@ -984,7 +984,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);

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

float yaw_body = _yaw;
const bool disable_underspeed_handling = false;

tecs_update_pitch_throttle(control_interval,
position_sp_alt,
Expand All @@ -1181,7 +1182,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);
const float pitch_body = get_tecs_pitch();

Expand Down Expand Up @@ -1542,6 +1543,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 @@ -1550,7 +1553,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_performance_model.getMaximumClimbRate(_air_density));
_performance_model.getMaximumClimbRate(_air_density),
disable_underspeed_handling);

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

Expand Down Expand Up @@ -1626,6 +1630,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 @@ -1635,7 +1640,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_param_fw_thr_min.get(),
max_takeoff_throttle,
_param_sinkrate_target.get(),
_performance_model.getMaximumClimbRate(_air_density));
_performance_model.getMaximumClimbRate(_air_density),
disable_underspeed_handling);

if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
// explicitly set idle throttle until motors are enabled
Expand Down Expand Up @@ -1812,6 +1818,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 @@ -1822,7 +1829,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 @@ -2028,6 +2035,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 @@ -2038,7 +2046,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 @@ -2084,6 +2092,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 bool disable_underspeed_handling = false;

tecs_update_pitch_throttle(control_interval,
_current_altitude, // is not controlled, control descend rate
target_airspeed,
Expand All @@ -2093,7 +2103,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 @@ -2150,6 +2160,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 @@ -2159,7 +2171,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);

float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
Expand Down Expand Up @@ -2250,6 +2262,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 @@ -2259,7 +2273,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
Loading