From 738b1967ad88cfc23b3500fd06fb121a5163c930 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 16 Apr 2015 12:57:28 -0700 Subject: [PATCH] AC_PosControl: use LowPassFilterVector2f --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 15 ++++++--------- libraries/AC_AttitudeControl/AC_PosControl.h | 2 +- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 637f93ba44..f68e28b7bf 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -54,7 +54,7 @@ AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, _alt_max(0.0f), _distance_to_target(0.0f), _accel_target_jerk_limited(0.0f,0.0f), - _accel_target_filtered(0.0f,0.0f) + _accel_target_filter(POSCONTROL_ACCEL_FILTER_HZ) { AP_Param::setup_object_defaults(this, var_info); @@ -841,8 +841,7 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler) if (_flags.reset_accel_to_lean_xy) { _accel_target_jerk_limited.x = _accel_target.x; _accel_target_jerk_limited.y = _accel_target.y; - _accel_target_filtered.x = _accel_target.x; - _accel_target_filtered.y = _accel_target.y; + _accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y)); _flags.reset_accel_to_lean_xy = false; } @@ -859,14 +858,12 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler) _accel_target_jerk_limited += accel_change; // lowpass filter on NE accel - float freq_cut = min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler); - float alpha = constrain_float(dt/(dt + 1.0f/(2.0f*(float)M_PI*freq_cut)),0.0f,1.0f); - _accel_target_filtered.x += alpha * (_accel_target_jerk_limited.x - _accel_target_filtered.x); - _accel_target_filtered.y += alpha * (_accel_target_jerk_limited.y - _accel_target_filtered.y); + _accel_target_filter.set_cutoff_frequency(min(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler)); + Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt); // rotate accelerations into body forward-right frame - accel_forward = _accel_target_filtered.x*_ahrs.cos_yaw() + _accel_target_filtered.y*_ahrs.sin_yaw(); - accel_right = -_accel_target_filtered.x*_ahrs.sin_yaw() + _accel_target_filtered.y*_ahrs.cos_yaw(); + accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw(); + accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller _pitch_target = constrain_float(fast_atan(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 90079bd70e..1bfa24afae 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -383,6 +383,6 @@ private: LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error Vector2f _accel_target_jerk_limited; // acceleration target jerk limited to 100deg/s/s - Vector2f _accel_target_filtered; // acceleration target filtered with 5hz low pass filter + LowPassFilterVector2f _accel_target_filter; // acceleration target filter }; #endif // AC_POSCONTROL_H