added a parameter for leak rate of I term

bnsgeyer · Apr 16, 2017 · cb2cd03 · cb2cd03
1 parent 34c5dda
commit cb2cd03
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 4 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
@@ -129,6 +129,13 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] = {
     // @Increment: 1
     AP_SUBGROUPINFO(_pid_rate_yaw, "RAT_YAW_", 4, AC_AttitudeControl_Heli, AC_HELI_PID),
 
+    // @Param: I_LEAK_RATE
+    // @DisplayName: I LEAK RATE
+    // @Description: Rate at which I term will leak for PID
+    // @Range: 0 1
+    // @User: Advanced
+    AP_GROUPINFO("I_LEAK_RATE",    5, AC_AttitudeControl_Heli, _rate_integrator_leak_rate, AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE),
+
     AP_GROUPEND
 };
 
@@ -286,7 +293,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
     // update i term as long as we haven't breached the limits or the I term will certainly reduce
     if (!_flags_heli.limit_roll || ((roll_i>0&&rate_roll_error_rads<0)||(roll_i<0&&rate_roll_error_rads>0))){
 		if (_flags_heli.leaky_i){
-			roll_i = _pid_rate_roll.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
+			roll_i = _pid_rate_roll.get_leaky_i(_rate_integrator_leak_rate);
 		}else{
 			roll_i = _pid_rate_roll.get_i();
 		}
@@ -298,7 +305,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target
     // update i term as long as we haven't breached the limits or the I term will certainly reduce
     if (!_flags_heli.limit_pitch || ((pitch_i>0&&rate_pitch_error_rads<0)||(pitch_i<0&&rate_pitch_error_rads>0))){
 		if (_flags_heli.leaky_i) {
-			pitch_i = _pid_rate_pitch.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);
+			pitch_i = _pid_rate_pitch.get_leaky_i(_rate_integrator_leak_rate);
 		}else{
 			pitch_i = _pid_rate_pitch.get_i();
 		}
@@ -385,7 +392,7 @@ float AC_AttitudeControl_Heli::rate_target_to_motor_yaw(float rate_target_rads)
         if (((AP_MotorsHeli&)_motors).rotor_runup_complete()) {
             i = _pid_rate_yaw.get_i();
         } else {
-            i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE);    // If motor is not running use leaky I-term to avoid excessive build-up
+            i = ((AC_HELI_PID&)_pid_rate_yaw).get_leaky_i(_rate_integrator_leak_rate);    // If motor is not running use leaky I-term to avoid excessive build-up
         }
     }
 

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
@@ -141,7 +141,6 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
     // internal variables
     float _hover_roll_trim_scalar = 0;              // scalar used to suppress Hover Roll Trim
 
-
     // This represents an euler axis-angle rotation vector from the vehicles
     // estimated attitude to the reference (setpoint) attitude used in the attitude
     // controller, in radians in the vehicle body frame of reference.
@@ -153,6 +152,7 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl {
     AC_HELI_PID     _pid_rate_roll;
     AC_HELI_PID     _pid_rate_pitch;
     AC_HELI_PID     _pid_rate_yaw;
+    AP_Float        _rate_integrator_leak_rate;
 
     // LPF filters to act on Rate Feedforward terms to linearize output.
     // Due to complicated aerodynamic effects, feedforwards acting too fast can lead