From e16f98c2b68c1f00c38a71317e88ab26a7097f70 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 23 Jan 2023 16:27:33 +0100 Subject: [PATCH] FW Position controller: remove unused climbout arguments Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 29 ++----------------- src/lib/tecs/TECS.hpp | 4 +-- .../FixedwingPositionControl.cpp | 29 ++----------------- .../FixedwingPositionControl.hpp | 6 ++-- 4 files changed, 8 insertions(+), 60 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 85dd3fee64..7602abc6a4 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -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 ¶m, 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; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index a00b605e91..fa0a73e740 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -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, }; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index f166c95dce..d43a266135 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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, diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 70f0bb55b7..d7f59b5a56 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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); /**