fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 20:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-12-08 16:26:10 +01:00
parent 740bf63fa7
commit 5928d7f067
2 changed files with 16 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+12 -4
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();
src/lib/tecs/TECS.hpp
+4 -1
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