fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 03:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position controller: remove unused climbout arguments

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-01-26 17:04:43 +01:00
parent aae431e4d5
commit e16f98c2b6
4 changed files with 8 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+3 -26
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -54,8 +54,6 @@ static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt
void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
{
_airspeed_state.speed = equivalent_airspeed;
_airspeed_state.speed_rate = 0.0f;
}
@@ -373,10 +371,7 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
// Calculate the weight applied to control of specific kinetic energy error
float pitch_speed_weight = constrain(param.pitch_speed_weight, 0.0f, 2.0f);
if (flag.climbout_mode_active && flag.airspeed_enabled) {
pitch_speed_weight = 2.0f;
} else if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) {
if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) {
pitch_speed_weight = 2.0f * _ratio_undersped + (1.0f - _ratio_undersped) * pitch_speed_weight;
} else if (!flag.airspeed_enabled) {
@@ -463,7 +458,6 @@ void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rat
float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
const Flag &flag) const
{
// Pitch setpoint
// Calculate a specific energy correction that doesn't include the integrator contribution
float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + _pitch_integ_state +
param.seb_rate_ff *
@@ -472,13 +466,6 @@ float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValu
// Calculate derivative from change in climb angle to rate of change of specific energy balance
const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator
// having to catch up before the nose can be raised to reduce excess speed during climbout.
if (flag.climbout_mode_active) {
SEB_rate_correction += param.pitch_min * climb_angle_to_SEB_rate;
}
// Convert the specific energy balance rate correction to a target pitch angle. This calculation assumes:
// a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop.
// b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of
@@ -562,12 +549,6 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit
// This will be added to the total throttle demand to compensate for steady state errors
_throttle_integ_state = _throttle_integ_state + throttle_integ_input;
if (flag.climbout_mode_active) {
// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
// at end of climbout when we transition to closed loop throttle control
_throttle_integ_state = param.throttle_max - _throttle_setpoint;
}
} else {
_throttle_integ_state = 0.0f;
}
@@ -728,7 +709,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
}
void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
float eas_to_tas, float throttle_min, float throttle_setpoint_max,
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
{
@@ -748,7 +729,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
_control_param.throttle_trim = throttle_trim;
_control_param.throttle_max = throttle_setpoint_max;
_control_param.throttle_min = throttle_min;
_control_flag.climbout_mode_active = climb_out_setpoint;
if (dt < DT_MIN) {
// Update intervall too small, do not update. Assume constant states/output in this case.
@@ -804,9 +784,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
} else if (_uncommanded_descent_recovery) {
_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (climb_out_setpoint) {
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// This is the default operation mode
_tecs_mode = ECL_TECS_MODE_NORMAL;
src/lib/tecs/TECS.hpp
+1 -3
View File
@@ -269,7 +269,6 @@ public:
*/
struct Flag {
bool airspeed_enabled; ///< Flag if the airspeed sensor is enabled.
bool climbout_mode_active; ///< Flag if climbout mode is activated.
bool detect_underspeed_enabled; ///< Flag if underspeed detection is enabled.
};
public:
@@ -586,7 +585,7 @@ public:
*
*/
void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
float eas_to_tas, float throttle_min, float throttle_setpoint_max,
float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
@@ -726,7 +725,6 @@ private:
TECSControl::Flag _control_flag{
.airspeed_enabled = false,
.climbout_mode_active = false,
.detect_underspeed_enabled = false,
};
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+2 -27
View File
@@ -526,7 +526,6 @@ FixedwingPositionControl::status_publish()
pos_ctrl_status.nav_bearing = NAN;
pos_ctrl_status.target_bearing = NAN;
pos_ctrl_status.xtrack_error = NAN;
}
if (_param_fw_use_npfg.get()) {
@@ -944,8 +943,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
false,
_param_fw_p_lim_min.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get());
@@ -979,8 +976,6 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
false,
_param_fw_p_lim_min.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
@@ -1167,8 +1162,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
false,
radians(_param_fw_p_lim_min.get()),
_param_sinkrate_target.get(),
_param_climbrate_target.get());
}
@@ -1226,8 +1219,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
false,
radians(_param_fw_p_lim_min.get()),
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
tecs_status_s::TECS_MODE_NORMAL,
@@ -1348,8 +1339,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
false,
radians(_param_fw_p_lim_min.get()),
_param_sinkrate_target.get(),
_param_climbrate_target.get());
}
@@ -1483,8 +1472,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
pitch_max,
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
false,
pitch_min,
_param_sinkrate_target.get(),
_param_fw_t_clmb_max.get());
@@ -1564,8 +1551,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
max_takeoff_throttle,
false,
radians(_takeoff_pitch_min.get()),
_param_sinkrate_target.get(),
_param_fw_t_clmb_max.get());
@@ -1760,8 +1745,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
pitch_max_rad,
_param_fw_thr_idle.get(),
throttle_max,
false,
pitch_min_rad,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
true,
@@ -1832,8 +1815,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
false,
radians(_param_fw_p_lim_min.get()),
desired_max_sinkrate,
_param_climbrate_target.get());
@@ -1895,8 +1876,6 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
false,
min_pitch,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
@@ -2004,8 +1983,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
false,
min_pitch,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
false,
@@ -2438,8 +2415,8 @@ float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float
void
FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode,
float climbout_pitch_min_rad, const float desired_max_sinkrate, const float desired_max_climbrate,
float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max,
const float desired_max_sinkrate, const float desired_max_climbrate,
bool disable_underspeed_detection, float hgt_rate_sp)
{
_tecs_is_running = true;
@@ -2514,8 +2491,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
airspeed_sp,
_airspeed,
_eas2tas,
climbout_mode,
climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
throttle_min,
throttle_max,
throttle_trim_comp,
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+2 -4
View File
@@ -701,16 +701,14 @@ private:
* @param pitch_max_rad Nominal pitch angle command maximum [rad]
* @param throttle_min Minimum throttle command [0,1]
* @param throttle_max Maximum throttle command [0,1]
* @param climbout_mode True if TECS should engage climbout mode
* @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad]
* @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s]
* @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s]
* @param disable_underspeed_detection True if underspeed detection should be disabled
* @param hgt_rate_sp Height rate setpoint [m/s]
*/
void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad,
float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode,
float climbout_pitch_min_rad, const float desired_max_sink_rate, const float desired_max_climb_rate,
float pitch_max_rad, float throttle_min, float throttle_max,
const float desired_max_sink_rate, const float desired_max_climb_rate,
bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
/**