AC_PosControl: add high vibration compensation

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -58,6 +58,10 @@ extern const AP_HAL::HAL& hal;
  # define POSCONTROL_VEL_XY_FILT_D_HZ           5.0f    // horizontal velocity controller input filter for D
 #endif
 
+// vibration compensation gains
+#define POSCONTROL_VIBE_COMP_P_GAIN 0.250f
+#define POSCONTROL_VIBE_COMP_I_GAIN 0.125f
+
 const AP_Param::GroupInfo AC_PosControl::var_info[] = {
     // 0 was used for HOVER
 
@@ -595,7 +599,21 @@ void AC_PosControl::run_z_controller()
         _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
     }
 
-    float thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f +_motors.get_throttle_hover();
+    float thr_out;
+    if (_vibe_comp_enabled) {
+        _flags.freeze_ff_z = true;
+        _accel_desired.z = 0.0f;
+        const float thr_per_accelz_cmss = _motors.get_throttle_hover() / (GRAVITY_MSS * 100.0f);
+        // during vibration compensation use feed forward with manually calculated gain
+        // ToDo: clear pid_info P, I and D terms for logging
+        if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_z.get_i()) && is_negative(_vel_error.z)) || (is_negative(_pid_accel_z.get_i()) && is_positive(_vel_error.z)))) {
+            _pid_accel_z.set_integrator(_pid_accel_z.get_i() + _dt * thr_per_accelz_cmss * 1000.0f * _vel_error.z * _p_vel_z.kP() * POSCONTROL_VIBE_COMP_I_GAIN);
+        }
+        thr_out = POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * _accel_target.z + _pid_accel_z.get_i() * 0.001f;
+    } else {
+        thr_out = _pid_accel_z.update_all(_accel_target.z, z_accel_meas, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f;
+    }
+    thr_out += _motors.get_throttle_hover();
 
     // send throttle to attitude controller with angle boost
     _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);

libraries/AC_AttitudeControl/AC_PosControl.h

@@ -304,6 +304,9 @@ public:
                         char *failure_msg,
                         const uint8_t failure_msg_len);
 
+    // enable or disable high vibration compensation
+    void set_vibe_comp(bool on_off) { _vibe_comp_enabled = on_off; }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
@@ -423,4 +426,7 @@ protected:
     // ekf reset handling
     uint32_t    _ekf_xy_reset_ms;      // system time of last recorded ekf xy position reset
     uint32_t    _ekf_z_reset_ms;       // system time of last recorded ekf altitude reset
+
+    // high vibration handling
+    bool        _vibe_comp_enabled;     // true when high vibration compensation is on
 };