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

TECS: init to airspeed filter to trim airspeed if airspeed-less 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-12-07 10:45:08 +01:00
parent 740bf63fa7
commit 5928d7f067
2 changed files with 16 additions and 5 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,
@ -654,7 +661,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();

5
src/lib/tecs/TECS.hpp
View File

@ -88,8 +88,11 @@ public:
* @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