diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index b270bd43f9..8536f33976 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -52,7 +52,8 @@ class AlphaFilter { public: AlphaFilter() = default; - explicit AlphaFilter(float alpha) : _alpha(alpha) {} + explicit AlphaFilter(float sample_interval, float time_constant) { setParameters(sample_interval, time_constant); } + explicit AlphaFilter(float time_constant) : _time_constant(time_constant) {}; ~AlphaFilter() = default; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b831d096d1..fc4aa0e41a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -518,7 +518,7 @@ private: Vector3f _ref_body_rate{}; Vector2f _flow_vel_body{}; ///< velocity from corrected flow measurement (body frame)(m/s) - AlphaFilter _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) + AlphaFilter _flow_vel_body_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered velocity from corrected flow measurement (body frame)(m/s) uint32_t _flow_counter{0}; ///< number of flow samples read for initialization Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive @@ -583,14 +583,15 @@ private: // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; - AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) - AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) + static constexpr float _kSensorLpfTimeConstant = 0.09f; + AlphaFilter _accel_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered accelerometer measurement used to align tilt (m/s/s) + AlphaFilter _gyro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) #if defined(CONFIG_EKF2_BAROMETER) estimator_aid_source1d_s _aid_src_baro_hgt {}; // Variables used to perform in flight resets and switch between height sources - AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) + AlphaFilter _baro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered barometric height measurement (m) uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; @@ -600,12 +601,12 @@ private: #if defined(CONFIG_EKF2_MAGNETOMETER) // used by magnetometer fusion mode selection - AlphaFilter _mag_heading_innov_lpf{0.1f}; + AlphaFilter _mag_heading_innov_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time estimator_aid_source3d_s _aid_src_mag{}; - AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) + AlphaFilter _mag_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered magnetometer measurement for instant reset (Gauss) uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation // Variables used to control activation of post takeoff functionality @@ -1011,7 +1012,8 @@ private: #if defined(CONFIG_EKF2_EXTERNAL_VISION) HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref}; - AlphaFilter _ev_q_error_filt{0.001f}; + static constexpr float _kQuatErrorLpfTimeConstant = 10.f; + AlphaFilter _ev_q_error_filt{_dt_ekf_avg, _kQuatErrorLpfTimeConstant}; bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index 608e4443c3..0118e53cd4 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -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; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index f15b4dd574..a1fdaae9f0 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -60,11 +60,11 @@ private: float _yaw_error_ref{0.f}; float _yaw_correction{0.f}; bool _yaw_estimate_converging{false}; - AlphaFilter _yaw_error_lpf{0.01f}; ///< used to create a high-pass filter + AlphaFilter _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