fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 14:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

AlphaFilter: set time constant instead of alpha

Browse Source 
Then the update function can set the dt at every iteration if needed
This commit is contained in:
bresch
2024-12-18 14:11:08 +01:00
committed by Matthias Grob
parent 37401d6fd1
commit fa5a781e20
4 changed files with 16 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp
+3 -4
View File
@@ -57,9 +57,8 @@ void StickYaw::ekfResetHandler(const float delta_yaw)
void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw,
const float yaw, const float deltatime, const float unaided_yaw)
{
_yaw_error_lpf.setParameters(deltatime, _kYawErrorTimeConstant);
const float yaw_correction_prev = _yaw_correction;
const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw);
const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw, deltatime);
if (reset_setpoint) {
yaw_setpoint = NAN;
@@ -71,7 +70,7 @@ void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint
yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev);
}
bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw)
bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw, const float deltatime)
{
if (!PX4_ISFINITE(unaided_yaw)) {
_yaw_correction = 0.f;
@@ -84,7 +83,7 @@ bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw)
// Run it through a high-pass filter to detect transients
const float yaw_error_hpf = wrap_pi(yaw_error - _yaw_error_lpf.getState());
_yaw_error_lpf.update(yaw_error);
_yaw_error_lpf.update(yaw_error, deltatime);
const bool was_converging = _yaw_estimate_converging;
_yaw_estimate_converging = fabsf(yaw_error_hpf) > _kYawErrorChangeThreshold;
src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp
+2 -2
View File
@@ -60,11 +60,11 @@ private:
float _yaw_error_ref{0.f};
float _yaw_correction{0.f};
bool _yaw_estimate_converging{false};
AlphaFilter<float> _yaw_error_lpf{0.01f}; ///< used to create a high-pass filter
AlphaFilter<float> _yaw_error_lpf{_kYawErrorTimeConstant}; ///< used to create a high-pass filter
static constexpr float _kYawErrorTimeConstant{1.f}; ///< time constant of the high-pass filter used to detect yaw convergence
static constexpr float _kYawErrorChangeThreshold{radians(1.f)}; ///< we consider the yaw estimate as "converging" when above this threshold
bool updateYawCorrection(float yaw, float unaided_yaw);
bool updateYawCorrection(float yaw, float unaided_yaw, float deltatime);
/**
* Lock yaw when not currently turning