AC_PosControl: 4hz filter on z-axis velocity error

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -86,6 +86,7 @@ void AC_PosControl::set_dt(float delta_sec)
     _pid_alt_accel.set_d_lpf_alpha(POSCONTROL_ACCEL_Z_DTERM_FILTER, _dt);
 
     // 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);
 }
 
@@ -337,10 +338,12 @@ void AC_PosControl::rate_to_accel_z()
     if (_flags.reset_rate_to_accel_z) {
         // Reset Filter
         _vel_error.z = 0;
+        _vel_error_filter.reset(0);
         desired_accel = 0;
         _flags.reset_rate_to_accel_z = false;
     } else {
-        _vel_error.z = (_vel_target.z - curr_vel.z);
+        // calculate rate error and filter with cut off frequency of 2 Hz
+        _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z);
     }
 
     // calculate p

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -40,6 +40,7 @@
 
 #define POSCONTROL_VEL_UPDATE_TIME              0.020f  // 50hz update rate on high speed CPUs (Pixhawk, Flymaple)
 
+#define POSCONTROL_VEL_ERROR_CUTOFF_FREQ        4.0     // 4hz low-pass filter on velocity error
 #define POSCONTROL_ACCEL_ERROR_CUTOFF_FREQ      2.0     // 2hz low-pass filter on accel error
 
 class AC_PosControl
@@ -369,6 +370,7 @@ private:
     float       _distance_to_target;    // distance to position target - for reporting only
     uint8_t     _xy_step;               // used to decide which portion of horizontal position controller to run during this iteration
     float       _dt_xy;                 // time difference in seconds between horizontal position updates
+    LowPassFilterFloat _vel_error_filter;   // low-pass-filter on z-axis velocity error
     LowPassFilterFloat _accel_error_filter; // low-pass-filter on z-axis accelerometer error
 
     // velocity controller internal variables