dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

TECS: fix airspeed filter init in airspeed-less mode 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-12-11 12:09:42 +01:00 committed by Matthias Grob
parent 60e2c6a5cb
commit 9db86f7f0a
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/tecs/TECS.cpp
View File

@ -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,
@ -651,7 +658,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();