diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index fa5bc8f570c..3f9b67aa305 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -174,9 +174,8 @@ void TECS::_update_speed_setpoint() _TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max); - // calculate the demanded rate of change of speed proportional to speed error - // and apply performance limits - _TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _speed_error_gain, velRateMin, velRateMax); + // calculate the demanded true airspeed rate of change based on first order response of true airspeed error + _TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax); } @@ -211,10 +210,11 @@ void TECS::_update_height_setpoint(float desired, float state) // Apply a first order noise filter _hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev; - // Calculate the demanded climb rate proportional to height error plus a feedforward term to provide - // tight tracking during steady climb and descent manoeuvres. + // Use a first order system to calculate a height rate setpoint from the current height error. + // Additionally, allow to add feedforward from heigh setpoint change _hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff * (_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt; + _hgt_setpoint_adj_prev = _hgt_setpoint_adj; // Limit the rate of change of height demand to respect vehicle performance limits @@ -233,7 +233,7 @@ void TECS::_detect_underspeed() return; } - if (((_tas_state < _TAS_min * 0.9f) && (_throttle_setpoint >= _throttle_setpoint_max * 0.95f)) + if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f)) || ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) { _underspeed_detected = true; @@ -249,6 +249,21 @@ void TECS::_update_energy_estimates() _SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy _SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy + // Calculate total energy error + _STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate; + + const float SKE_weighting = get_SKE_weighting(); + + // Calculate the weighting applied to control of specific potential energy error + const float SPE_weighting = 2.0f - SKE_weighting; + + // Calculate the specific energy balance demand which specifies how the available total + // energy should be allocated to speed (kinetic energy) and height (potential energy) + float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting; + + // Calculate the specific energy balance error + _SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting); + // Calculate specific energy rate demands in units of (m**2/sec**3) _SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change _SKE_rate_setpoint = _tas_state * _TAS_rate_setpoint; // kinetic energy rate of change @@ -264,28 +279,30 @@ void TECS::_update_energy_estimates() void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat) { - // Calculate total energy error - _STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate; - - // Calculate demanded rate of change of total energy, respecting vehicle limits - float STE_rate_setpoint = constrain((_SPE_rate_setpoint + _SKE_rate_setpoint), _STE_rate_min, _STE_rate_max); + // Calculate demanded rate of change of total energy, respecting vehicle limits. + // We will constrain the value below. + float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint; // Calculate the total energy rate error, applying a first order IIR filter // to reduce the effect of accelerometer noise - _STE_rate_error_filter.update(-_SPE_rate - _SKE_rate); + _STE_rate_error_filter.update(-_SPE_rate - _SKE_rate + _SPE_rate_setpoint + _SKE_rate_setpoint); _STE_rate_error = _STE_rate_error_filter.getState(); + float throttle_setpoint; + // Calculate the throttle demand if (_underspeed_detected) { // always use full throttle to recover from an underspeed condition - _throttle_setpoint = 1.0f; + throttle_setpoint = _throttle_setpoint_max; } else { // Adjust the demanded total energy rate to compensate for induced drag rise in turns. // Assume induced drag scales linearly with normal load factor. // The additional normal load factor is given by (1/cos(bank angle) - 1) - float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); - STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f); + const float cosPhi = constrain(sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))), 0.1f, 1.0f); + STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / cosPhi - 1.0f); + + STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max); // Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle // as the starting point. Assume: @@ -304,31 +321,22 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix:: } - // Calculate gain scaler from specific energy error to throttle - float STE_to_throttle = 1.0f / (_throttle_time_constant * (_STE_rate_max - _STE_rate_min)); + // Calculate gain scaler from specific energy rate error to throttle + const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min); // Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits - _throttle_setpoint = (_STE_error + _STE_rate_error * _throttle_damping_gain) * STE_to_throttle + throttle_predicted; - _throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); - - // Rate limit the throttle demand - if (fabsf(_throttle_slewrate) > 0.01f) { - float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate; - _throttle_setpoint = constrain(_throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit, - _last_throttle_setpoint + throttle_increment_limit); - } - - _last_throttle_setpoint = _throttle_setpoint; + throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted; + throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); if (_integrator_gain > 0.0f) { // Calculate throttle integrator state upper and lower limits with allowance for // 10% throttle saturation to accommodate noise on the demand. - float integ_state_max = _throttle_setpoint_max - _throttle_setpoint + 0.1f; - float integ_state_min = _throttle_setpoint_min - _throttle_setpoint - 0.1f; + float integ_state_max = _throttle_setpoint_max - throttle_setpoint + 0.1f; + float integ_state_min = _throttle_setpoint_min - throttle_setpoint - 0.1f; // Calculate a throttle demand from the integrated total energy error // This will be added to the total throttle demand to compensate for steady state errors - _throttle_integ_state = _throttle_integ_state + (_STE_error * _integrator_gain) * _dt * STE_to_throttle; + _throttle_integ_state = _throttle_integ_state + (_STE_rate_error * _integrator_gain) * _dt * STE_rate_to_throttle; if (_climbout_mode_active) { // During climbout, set the integrator to maximum throttle to prevent transient throttle drop @@ -346,16 +354,23 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix:: if (airspeed_sensor_enabled()) { // Add the integrator feedback during closed loop operation with an airspeed sensor - _throttle_setpoint = _throttle_setpoint + _throttle_integ_state; + throttle_setpoint += _throttle_integ_state; } else { // when flying without an airspeed sensor, use the predicted throttle only - _throttle_setpoint = throttle_predicted; + throttle_setpoint = throttle_predicted; } - - _throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); } + + // Rate limit the throttle demand + if (fabsf(_throttle_slewrate) > 0.01f) { + const float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate; + throttle_setpoint = constrain(throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit, + _last_throttle_setpoint + throttle_increment_limit); + } + + _last_throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); } void TECS::_detect_uncommanded_descent() @@ -367,14 +382,15 @@ void TECS::_detect_uncommanded_descent() */ // Calculate rate of change of total specific energy - float STE_rate = _SPE_rate + _SKE_rate; + const float STE_rate = _SPE_rate + _SKE_rate; // If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode - bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f) && (STE_rate < 0.0f) - && (_throttle_setpoint >= _throttle_setpoint_max * 0.9f); + const bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f) + && (STE_rate < 0.0f) + && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.9f); // If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode - bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f)); + const bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f)); if (enter_mode) { _uncommanded_descent_recovery = true; @@ -397,39 +413,31 @@ void TECS::_update_pitch_setpoint() * rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding. * The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements. */ - const float SKE_weighting = get_SKE_weighting(); // Calculate the weighting applied to control of specific potential energy error const float SPE_weighting = 2.0f - SKE_weighting; - // Calculate the specific energy balance demand which specifies how the available total - // energy should be allocated to speed (kinetic energy) and height (potential energy) - float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting; - // Calculate the specific energy balance rate demand - float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting; + const float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting; - // Calculate the specific energy balance and balance rate error - _SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting); + // Calculate the specific energy balance rate error _SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting); // Calculate derivative from change in climb angle to rate of change of specific energy balance - float climb_angle_to_SEB_rate = _tas_state * _pitch_time_constant * CONSTANTS_ONE_G; + const float climb_angle_to_SEB_rate = _tas_state * CONSTANTS_ONE_G; if (_integrator_gain > 0.0f) { // Calculate pitch integrator input term - float pitch_integ_input = _SEB_error * _integrator_gain; + float pitch_integ_input = _SEB_rate_error * _integrator_gain; // Prevent the integrator changing in a direction that will increase pitch demand saturation - // Decay the integrator at the control loop time constant if the pitch demand from the previous time step is saturated + // Decay the integrator using a 1 second time constant if the pitch demand from the previous time step is saturated if (_pitch_setpoint_unc > _pitch_setpoint_max) { - pitch_integ_input = min(pitch_integ_input, - min((_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f)); + pitch_integ_input = min(pitch_integ_input, (_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate); } else if (_pitch_setpoint_unc < _pitch_setpoint_min) { - pitch_integ_input = max(pitch_integ_input, - max((_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f)); + pitch_integ_input = max(pitch_integ_input, (_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate); } // Update the pitch integrator state. @@ -440,7 +448,7 @@ void TECS::_update_pitch_setpoint() } // Calculate a specific energy correction that doesn't include the integrator contribution - float SEB_correction = _SEB_error + _SEB_rate_error * _pitch_damping_gain + SEB_rate_setpoint * _pitch_time_constant; + float SEB_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + SEB_rate_setpoint; // 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 @@ -449,23 +457,16 @@ void TECS::_update_pitch_setpoint() SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate; } - // Sum the correction terms and convert to a pitch angle demand. This calculation assumes: + // Convert the specific energy balance 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 // pitch transients due to control action or turbulence. - _pitch_setpoint_unc = (SEB_correction + _pitch_integ_state) / climb_angle_to_SEB_rate; + _pitch_setpoint_unc = SEB_correction / climb_angle_to_SEB_rate; _pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max); // Comply with the specified vertical acceleration limit by applying a pitch rate limit - float ptchRateIncr = _dt * _vert_accel_limit / _tas_state; - - if ((_pitch_setpoint - _last_pitch_setpoint) > ptchRateIncr) { - _pitch_setpoint = _last_pitch_setpoint + ptchRateIncr; - - } else if ((_pitch_setpoint - _last_pitch_setpoint) < -ptchRateIncr) { - _pitch_setpoint = _last_pitch_setpoint - ptchRateIncr; - } - + const float ptchRateIncr = _dt * _vert_accel_limit / _tas_state; + _pitch_setpoint = constrain(_pitch_setpoint, _last_pitch_setpoint - ptchRateIncr, _last_pitch_setpoint + ptchRateIncr); _last_pitch_setpoint = _pitch_setpoint; } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 9e5943c8aa0..1f7e6694f4f 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -86,7 +86,7 @@ public: float throttle_min, float throttle_setpoint_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); - float get_throttle_setpoint() { return _throttle_setpoint; } + float get_throttle_setpoint() { return _last_throttle_setpoint; } float get_pitch_setpoint() { return _pitch_setpoint; } float get_speed_weight() { return _pitch_speed_weight; } @@ -102,7 +102,6 @@ public: void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; } // setters for controller parameters - void set_time_const(float time_const) { _pitch_time_constant = time_const; } void set_integrator_gain(float gain) { _integrator_gain = gain; } void set_min_sink_rate(float rate) { _min_sink_rate = rate; } @@ -110,7 +109,7 @@ public: void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; } void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; } - void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; } + void set_height_error_time_constant(float time_const) { _height_error_gain = 1.0f / math::max(time_const, 0.1f); } void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; } void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; } @@ -120,9 +119,8 @@ public: void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; } void set_speed_weight(float weight) { _pitch_speed_weight = weight; } - void set_speedrate_p(float speedrate_p) { _speed_error_gain = speedrate_p; } + void set_airspeed_error_time_constant(float time_const) { _airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); } - void set_time_const_throt(float time_const_throt) { _throttle_time_constant = time_const_throt; } void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; } void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; } @@ -206,17 +204,15 @@ private: float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec) float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec) float _max_sink_rate{2.0f}; ///< maximum safe sink rate (m/sec) - float _pitch_time_constant{5.0f}; ///< control time constant used by the pitch demand calculation (sec) - float _throttle_time_constant{8.0f}; ///< control time constant used by the throttle demand calculation (sec) float _pitch_damping_gain{0.0f}; ///< damping gain of the pitch demand calculation (sec) float _throttle_damping_gain{0.0f}; ///< damping gain of the throttle demand calculation (sec) float _integrator_gain{0.0f}; ///< integrator gain used by the throttle and pitch demand calculation float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2) float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3) float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation - float _height_error_gain{0.0f}; ///< gain from height error to demanded climb rate (1/sec) + float _height_error_gain{0.2f}; ///< height error inverse time constant [1/s] float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate - float _speed_error_gain{0.0f}; ///< gain from speed error to demanded speed rate (1/sec) + float _airspeed_error_gain{0.1f}; ///< airspeed error inverse time constant [1/s] float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec) @@ -224,7 +220,6 @@ private: float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s) // controller outputs - float _throttle_setpoint{0.0f}; ///< normalized throttle demand (0..1) float _pitch_setpoint{0.0f}; ///< pitch angle demand (radians) // complimentary filter states diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 9d7d91d550a..b714e97d1ec 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -98,8 +98,6 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_indicated_airspeed_min(_param_fw_airspd_min.get()); _tecs.set_indicated_airspeed_max(_param_fw_airspd_max.get()); - _tecs.set_time_const_throt(_param_fw_t_thro_const.get()); - _tecs.set_time_const(_param_fw_t_time_const.get()); _tecs.set_min_sink_rate(_param_fw_t_sink_min.get()); _tecs.set_throttle_damp(_param_fw_t_thr_damp.get()); _tecs.set_integrator_gain(_param_fw_t_integ_gain.get()); @@ -108,9 +106,9 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get()); _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); _tecs.set_pitch_damping(_param_fw_t_ptch_damp.get()); - _tecs.set_heightrate_p(_param_fw_t_hrate_p.get()); + _tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get()); _tecs.set_heightrate_ff(_param_fw_t_hrate_ff.get()); - _tecs.set_speedrate_p(_param_fw_t_srate_p.get()); + _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); _tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get()); @@ -639,7 +637,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 /* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */ _tecs.set_speed_weight(_param_fw_t_spdweight.get()); - _tecs.set_time_const_throt(_param_fw_t_thro_const.get()); + //_tecs.set_time_const_throt(_param_fw_t_thro_const.get()); Vector2f curr_wp{0.0f, 0.0f}; Vector2f prev_wp{0.0f, 0.0f}; @@ -845,7 +843,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 // We're in a loiter directly before a landing WP. Enable our landing configuration (flaps, // landing airspeed and potentially tighter throttle control) already such that we don't // have to do this switch (which can cause significant altitude errors) close to the ground. - _tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); + //_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); mission_airspeed = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get(); _att_sp.apply_flaps = true; } @@ -865,7 +863,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _att_sp.roll_body = 0.0f; } - _tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); + //_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); } tecs_update_pitch_throttle(now, alt_sp, @@ -1331,7 +1329,7 @@ FixedwingPositionControl::control_landing(const hrt_abstime &now, const Vector2f _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND; // Enable tighter throttle control for landings - _tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); + //_tecs.set_time_const_throt(_param_fw_thrtc_sc.get() * _param_fw_t_thro_const.get()); // save time at which we started landing and reset abort_landing if (_time_started_landing == 0) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index f5bea730f48..1329d46c9f3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -395,7 +395,7 @@ private: (ParamFloat) _param_fw_t_clmb_max, (ParamFloat) _param_fw_t_hrate_ff, - (ParamFloat) _param_fw_t_hrate_p, + (ParamFloat) _param_fw_t_h_error_tc, (ParamFloat) _param_fw_t_integ_gain, (ParamFloat) _param_fw_t_ptch_damp, (ParamFloat) _param_fw_t_rll2thr, @@ -403,10 +403,8 @@ private: (ParamFloat) _param_fw_t_sink_min, (ParamFloat) _param_fw_t_spd_omega, (ParamFloat) _param_fw_t_spdweight, - (ParamFloat) _param_fw_t_srate_p, + (ParamFloat) _param_fw_t_tas_error_tc, (ParamFloat) _param_fw_t_thr_damp, - (ParamFloat) _param_fw_t_thro_const, - (ParamFloat) _param_fw_t_time_const, (ParamFloat) _param_fw_t_vert_acc, (ParamFloat) _param_ste_rate_time_const, (ParamFloat) _param_tas_rate_time_const, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index ab1e01e10a2..1e5c574bb4e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -520,38 +520,6 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); */ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); -/** - * TECS time constant - * - * This is the time constant of the TECS control algorithm (in seconds). - * Smaller values make it faster to respond, larger values make it slower - * to respond. - * - * @unit s - * @min 1.0 - * @max 10.0 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); - -/** - * TECS Throttle time constant - * - * This is the time constant of the TECS throttle control algorithm (in seconds). - * Smaller values make it faster to respond, larger values make it slower - * to respond. - * - * @unit s - * @min 1.0 - * @max 10.0 - * @decimal 1 - * @increment 0.5 - * @group FW TECS - */ -PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); - /** * Throttle damping factor * @@ -678,15 +646,14 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); /** - * Height rate proportional factor + * Altitude error time constant. * - * @min 0.0 - * @max 1.0 + * @min 2.0 * @decimal 2 - * @increment 0.05 + * @increment 0.5 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f); /** * Height rate feed forward @@ -700,15 +667,14 @@ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.8f); /** - * Speed rate P factor + * True airspeed error time constant. * - * @min 0.0 - * @max 2.0 + * @min 2.0 * @decimal 2 - * @increment 0.01 + * @increment 0.5 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.02f); +PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 10.0f); /** * Minimum groundspeed @@ -737,7 +703,7 @@ PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); * @group FW L1 Control */ PARAM_DEFINE_INT32(FW_POSCTL_INV_ST, 0); - + /** * Specific total energy rate first order filter time constant. *