AC_PosControl: remove accel error filter and set throttle output filter instead

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);
 }
 
 ///

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