AC_PosControl: add accel filter parameter

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -5,7 +5,16 @@
 extern const AP_HAL::HAL& hal;
 
 const AP_Param::GroupInfo AC_PosControl::var_info[] PROGMEM = {
-   // 0 was used for HOVER
+    // 0 was used for HOVER
+
+    // @Param: _ACC_XY_FILT
+    // @DisplayName: XY Acceleration filter cutoff frequency
+    // @Description: Lower values will slow the response of the navigation controller and reduce twitchiness
+    // @Units: Hz
+    // @Range: 0.5 5
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("_ACC_XY_FILT", 1, AC_PosControl, _accel_xy_filt_hz, POSCONTROL_ACCEL_FILTER_HZ),
 
     AP_GROUPEND
 };
@@ -134,12 +143,12 @@ void AC_PosControl::set_alt_target_with_slew(float alt_cm, float dt)
 ///     target will also be stopped if the motors hit their limits or leash length is exceeded
 void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms, float dt, bool force_descend)
 {
-    // jurk is calculated to reach full acceleration in 500ms.
-    float jurk_z = _accel_z_cms * 2.0f;
+    // jerk_z is calculated to reach full acceleration in 1000ms.
+    float jerk_z = _accel_z_cms * POSCONTROL_JERK_RATIO;
 
-    float accel_z_max = min(_accel_z_cms, safe_sqrt(2.0f*fabs(_vel_desired.z - climb_rate_cms)*jurk_z));
+    float accel_z_max = min(_accel_z_cms, safe_sqrt(2.0f*fabs(_vel_desired.z - climb_rate_cms)*jerk_z));
 
-    _accel_last_z_cms += jurk_z * dt;
+    _accel_last_z_cms += jerk_z * dt;
     _accel_last_z_cms = min(accel_z_max, _accel_last_z_cms);
 
     float vel_change_limit = _accel_last_z_cms * dt;
@@ -849,8 +858,8 @@ void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler)
     }
     _accel_target_jerk_limited += accel_change;
 
-    // 5Hz lowpass filter on NE accel
-    float freq_cut = POSCONTROL_ACCEL_FILTER_HZ * ekfNavVelGainScaler;
+    // 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);

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -41,7 +41,8 @@
 #define POSCONTROL_VEL_ERROR_CUTOFF_FREQ        4.0f    // 4hz low-pass filter on velocity 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
+#define POSCONTROL_ACCEL_FILTER_HZ              2.0f    // 5hz low-pass filter on acceleration
+#define POSCONTROL_JERK_RATIO                   1.0f    // Defines the time it takes to reach the requested acceleration
 
 class AC_PosControl
 {
@@ -344,6 +345,9 @@ private:
     AC_P&	    _p_pos_xy;
     AC_PI_2D&   _pi_vel_xy;
 
+    // parameters
+    AP_Float    _accel_xy_filt_hz;      // XY acceleration filter cutoff frequency
+
     // internal variables
     float       _dt;                    // time difference (in seconds) between calls from the main program
     float       _dt_xy;                 // time difference (in seconds) between update_xy_controller and update_vel_controller_xyz calls