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

Fix TECS init when switching into a altitude controlled mode only once in air #22500

Merged
merged 4 commits into from
Dec 8, 2023
Merged

Fix TECS init when switching into a altitude controlled mode only once in air #22500

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6a2444e
TECS: set _ratio_underspeed to 0 if airspeed disabled
sfuhrer Dec 7, 2023
6fa922c
TECS: init to airspeed filter to trim airspeed if airspeed-less
sfuhrer Dec 7, 2023
cccb870
TECS: make sure to constrain pitch to current min/max pitch
sfuhrer Dec 7, 2023
7c7e006
TECS: init control params to reasonable values
sfuhrer Dec 6, 2023
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
19 changes: 14 additions & 5 deletions src/lib/tecs/TECS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,17 @@ using namespace time_literals;

static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}

void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
void TECSAirspeedFilter::initialize(const float equivalent_airspeed, const float equivalent_airspeed_trim,
const bool airspeed_sensor_available)
{
_airspeed_state.speed = equivalent_airspeed;
_airspeed_state.speed_rate = 0.0f;
if (airspeed_sensor_available && PX4_ISFINITE(equivalent_airspeed)) {
_airspeed_state.speed = equivalent_airspeed;

} else {
_airspeed_state.speed = equivalent_airspeed_trim;
}

_airspeed_state.speed_rate = 0.f;
}

void TECSAirspeedFilter::update(const float dt, const Input &input, const Param &param,
Expand Down Expand Up @@ -353,7 +360,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt

void TECSControl::_detectUnderspeed(const Input &input, const Param &param, const Flag &flag)
{
if (!flag.detect_underspeed_enabled) {
if (!flag.detect_underspeed_enabled || !flag.airspeed_enabled) {
_ratio_undersped = 0.0f;
return;
}
Expand Down Expand Up @@ -412,6 +419,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific
const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON);
_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
_pitch_setpoint + pitch_increment);
_pitch_setpoint = constrain(_pitch_setpoint, param.pitch_min, param.pitch_max);

//Debug Output
_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
Expand Down Expand Up @@ -654,7 +662,8 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude,
.alt_rate = altitude_rate};
_altitude_reference_model.initialize(current_state);
_airspeed_filter.initialize(equivalent_airspeed);
_airspeed_filter.initialize(equivalent_airspeed, _airspeed_filter_param.equivalent_airspeed_trim,
_control_flag.airspeed_enabled);

TECSControl::Setpoint control_setpoint;
control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
Expand Down
11 changes: 7 additions & 4 deletions src/lib/tecs/TECS.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,8 +88,11 @@ class TECSAirspeedFilter
* @brief Initialize filter
*
* @param[in] equivalent_airspeed is the equivalent airspeed in [m/s].
* @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s].
* @param[in] airspeed_sensor_available boolean if the airspeed sensor is available.
*/
void initialize(float equivalent_airspeed);
void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim,
const bool airspeed_sensor_available);

/**
* @brief Update filter
Expand Down Expand Up @@ -696,9 +699,9 @@ class TECS
.max_climb_rate = 5.0f,
.vert_accel_limit = 0.0f,
.equivalent_airspeed_trim = 15.0f,
.tas_min = 3.0f,
.pitch_max = 5.0f,
.pitch_min = -5.0f,
.tas_min = 10.0f,
.pitch_max = 0.5f,
.pitch_min = -0.5f,
.throttle_trim = 0.0f,
.throttle_trim_adjusted = 0.f,
.throttle_max = 1.0f,
Expand Down
Loading