diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 4228d2c455..5dde6fb2b3 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -74,7 +74,6 @@ void AC_PosControl::set_dt(float delta_sec) // update rate z-axis velocity error and accel error filters _vel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_VEL_ERROR_CUTOFF_FREQ); - _accel_error_filter.set_cutoff_frequency(_dt,POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ); } /// set_dt_xy - sets time delta in seconds for horizontal controller (i.e. 50hz = 0.02) @@ -354,11 +353,10 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) if (_flags.reset_accel_to_throttle) { // Reset Filter _accel_error.z = 0; - _accel_error_filter.reset(0); _flags.reset_accel_to_throttle = false; } else { - // calculate accel error and Filter with fc = 2 Hz - _accel_error.z = _accel_error_filter.apply(accel_target_z - z_accel_meas); + // calculate accel error + _accel_error.z = accel_target_z - z_accel_meas; } // set input to PID @@ -382,7 +380,7 @@ void AC_PosControl::accel_to_throttle(float accel_target_z) float thr_out = p+i+d+_throttle_hover; // send throttle to attitude controller with angle boost - _attitude_control.set_throttle_out(thr_out, true); + _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ); } /// diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 32692492ba..320d0f6d8f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -39,7 +39,7 @@ #define POSCONTROL_ACTIVE_TIMEOUT_MS 200 // position controller is considered active if it has been called within the past 0.2 seconds #define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // 4hz low-pass filter on velocity error -#define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ 2.0f // 2hz low-pass filter on accel error +#define POSCONTROL_THROTTLE_CUTOFF_FREQ 2.0f // 2hz low-pass filter on accel error #define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // 17m/s/s/s jerk limit on horizontal acceleration #define POSCONTROL_ACCEL_FILTER_HZ 5.0f // 5hz low-pass filter on acceleration @@ -370,7 +370,6 @@ private: float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence float _distance_to_target; // distance to position target - for reporting only LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error - LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer 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