AC_AttControl: add angle and rate PIDs

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -66,26 +66,39 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ANGLE_BOOST", 12, AC_AttitudeControl, _angle_boost_enabled, 1),
 
+    // @Param: ANG_RLL_P
+    // @DisplayName: Roll axis angle controller P gain
+    // @Description: Roll axis angle controller P gain.  Converts the error between the desired roll angle and actual angle to a desired roll rate
+    // @Range: 3.000 12.000
+    // @User: Standard
+    AP_SUBGROUPINFO(_p_angle_roll, "ANG_RLL_", 13, AC_AttitudeControl, AC_P),
+
+    // @Param: ANG_PIT_P
+    // @DisplayName: Pitch axis angle controller P gain
+    // @Description: Pitch axis angle controller P gain.  Converts the error between the desired pitch angle and actual angle to a desired pitch rate
+    // @Range: 3.000 12.000
+    // @User: Standard
+    AP_SUBGROUPINFO(_p_angle_pitch, "ANG_PIT_", 14, AC_AttitudeControl, AC_P),
+
+    // @Param: ANG_YAW_P
+    // @DisplayName: Yaw axis angle controller P gain
+    // @Description: Yaw axis angle controller P gain.  Converts the error between the desired yaw angle and actual angle to a desired yaw rate
+    // @Range: 3.000 6.000
+    // @User: Standard
+    AP_SUBGROUPINFO(_p_angle_yaw, "ANG_YAW_", 15, AC_AttitudeControl, AC_P),
+
     AP_GROUPEND
 };
 
-void AC_AttitudeControl::set_dt(float delta_sec)
-{
-    _dt = delta_sec;
-    _pid_rate_roll.set_dt(_dt);
-    _pid_rate_pitch.set_dt(_dt);
-    _pid_rate_yaw.set_dt(_dt);
-}
-
 void AC_AttitudeControl::relax_bf_rate_controller()
 {
     // Set reference angular velocity used in angular velocity controller equal
     // to the input angular velocity and reset the angular velocity integrators.
     // This zeros the output of the angular velocity controller.
     _ang_vel_target_rads = _ahrs.get_gyro();
-    _pid_rate_roll.reset_I();
+    get_rate_roll_pid().reset_I();
-    _pid_rate_pitch.reset_I();
+    get_rate_pitch_pid().reset_I();
-    _pid_rate_yaw.reset_I();
+    get_rate_yaw_pid().reset_I();
 
     // Write euler derivatives derived from vehicle angular velocity to
     // _att_target_euler_rate_rads. This resets the state of the input shapers.
@@ -503,18 +516,18 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_rads)
     float rate_error_rads = rate_target_rads - current_rate_rads;
 
     // For legacy reasons, we convert to centi-degrees before inputting to the PID
-    _pid_rate_roll.set_input_filter_d(degrees(rate_error_rads)*100.0f);
+    get_rate_roll_pid().set_input_filter_d(degrees(rate_error_rads)*100.0f);
-    _pid_rate_roll.set_desired_rate(degrees(rate_target_rads)*100.0f);
+    get_rate_roll_pid().set_desired_rate(degrees(rate_target_rads)*100.0f);
 
-    float integrator = _pid_rate_roll.get_integrator();
+    float integrator = get_rate_roll_pid().get_integrator();
 
     // Ensure that integrator can only be reduced if the output is saturated
     if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) {
-        integrator = _pid_rate_roll.get_i();
+        integrator = get_rate_roll_pid().get_i();
     }
 
     // Compute output in range -1 ~ +1
-    float output = (_pid_rate_roll.get_p() + integrator + _pid_rate_roll.get_d()) / 4500.0f;
+    float output = (get_rate_roll_pid().get_p() + integrator + get_rate_roll_pid().get_d()) / 4500.0f;
 
     // Constrain output
     return constrain_float(output, -1.0f, 1.0f);
@@ -526,18 +539,18 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_rads)
     float rate_error_rads = rate_target_rads - current_rate_rads;
 
     // For legacy reasons, we convert to centi-degrees before inputting to the PID
-    _pid_rate_pitch.set_input_filter_d(degrees(rate_error_rads)*100.0f);
+    get_rate_pitch_pid().set_input_filter_d(degrees(rate_error_rads)*100.0f);
-    _pid_rate_pitch.set_desired_rate(degrees(rate_target_rads)*100.0f);
+    get_rate_pitch_pid().set_desired_rate(degrees(rate_target_rads)*100.0f);
 
-    float integrator = _pid_rate_pitch.get_integrator();
+    float integrator = get_rate_pitch_pid().get_integrator();
 
     // Ensure that integrator can only be reduced if the output is saturated
     if (!_motors.limit.roll_pitch || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) {
-        integrator = _pid_rate_pitch.get_i();
+        integrator = get_rate_pitch_pid().get_i();
     }
 
     // Compute output in range -1 ~ +1
-    float output = (_pid_rate_pitch.get_p() + integrator + _pid_rate_pitch.get_d()) / 4500.0f;
+    float output = (get_rate_pitch_pid().get_p() + integrator + get_rate_pitch_pid().get_d()) / 4500.0f;
 
     // Constrain output
     return constrain_float(output, -1.0f, 1.0f);
@@ -549,18 +562,18 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_rads)
     float rate_error_rads = rate_target_rads - current_rate_rads;
 
     // For legacy reasons, we convert to centi-degrees before inputting to the PID
-    _pid_rate_yaw.set_input_filter_all(degrees(rate_error_rads)*100.0f);
+    get_rate_yaw_pid().set_input_filter_all(degrees(rate_error_rads)*100.0f);
-    _pid_rate_yaw.set_desired_rate(degrees(rate_target_rads)*100.0f);
+    get_rate_yaw_pid().set_desired_rate(degrees(rate_target_rads)*100.0f);
 
-    float integrator = _pid_rate_yaw.get_integrator();
+    float integrator = get_rate_yaw_pid().get_integrator();
 
     // Ensure that integrator can only be reduced if the output is saturated
     if (!_motors.limit.yaw || ((integrator > 0 && rate_error_rads < 0) || (integrator < 0 && rate_error_rads > 0))) {
-        integrator = _pid_rate_yaw.get_i();
+        integrator = get_rate_yaw_pid().get_i();
     }
 
     // Compute output in range -1 ~ +1
-    float output = (_pid_rate_yaw.get_p() + integrator + _pid_rate_yaw.get_d()) / 4500.0f;
+    float output = (get_rate_yaw_pid().get_p() + integrator + get_rate_yaw_pid().get_d()) / 4500.0f;
 
     // Constrain output
     return constrain_float(output, -1.0f, 1.0f);
@@ -671,21 +684,21 @@ void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m)
 
 float AC_AttitudeControl::max_rate_step_bf_roll()
 {
-    float alpha = _pid_rate_roll.get_filt_alpha();
+    float alpha = get_rate_roll_pid().get_filt_alpha();
     float alpha_remaining = 1-alpha;
-    return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_roll.kD())/_dt + _pid_rate_roll.kP());
+    return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_roll_pid().kD())/_dt + get_rate_roll_pid().kP());
 }
 
 float AC_AttitudeControl::max_rate_step_bf_pitch()
 {
-    float alpha = _pid_rate_pitch.get_filt_alpha();
+    float alpha = get_rate_pitch_pid().get_filt_alpha();
     float alpha_remaining = 1-alpha;
-    return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_pitch.kD())/_dt + _pid_rate_pitch.kP());
+    return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_pitch_pid().kD())/_dt + get_rate_pitch_pid().kP());
 }
 
 float AC_AttitudeControl::max_rate_step_bf_yaw()
 {
-    float alpha = _pid_rate_yaw.get_filt_alpha();
+    float alpha = get_rate_yaw_pid().get_filt_alpha();
     float alpha_remaining = 1-alpha;
-    return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*_pid_rate_yaw.kD())/_dt + _pid_rate_yaw.kP());
+    return AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX/((alpha_remaining*alpha_remaining*alpha_remaining*alpha*get_rate_yaw_pid().kD())/_dt + get_rate_yaw_pid().kP());
 }

libraries/AC_AttitudeControl/AC_AttitudeControl.h

@@ -13,9 +13,43 @@
 #include <AC_PID/AC_PID.h>
 #include <AC_PID/AC_P.h>
 
+// default rate controller PID gains
+#ifndef AC_ATC_MULTI_RATE_RP_P
+  # define AC_ATC_MULTI_RATE_RP_P           0.150f
+#endif
+#ifndef AC_ATC_MULTI_RATE_RP_I
+  # define AC_ATC_MULTI_RATE_RP_I           0.100f
+#endif
+#ifndef AC_ATC_MULTI_RATE_RP_D
+  # define AC_ATC_MULTI_RATE_RP_D           0.004f
+#endif
+#ifndef AC_ATC_MULTI_RATE_RP_IMAX
+ # define AC_ATC_MULTI_RATE_RP_IMAX         2000.0f
+#endif
+#ifndef AC_ATC_MULTI_RATE_RP_FILT_HZ
+ # define AC_ATC_MULTI_RATE_RP_FILT_HZ      20.0f
+#endif
+#ifndef AC_ATC_MULTI_RATE_YAW_P
+ # define AC_ATC_MULTI_RATE_YAW_P           0.200f
+#endif
+#ifndef AC_ATC_MULTI_RATE_YAW_I
+ # define AC_ATC_MULTI_RATE_YAW_I           0.020f
+#endif
+#ifndef AC_ATC_MULTI_RATE_YAW_D
+ # define AC_ATC_MULTI_RATE_YAW_D           0.0f
+#endif
+#ifndef AC_ATC_MULTI_RATE_YAW_IMAX
+ # define AC_ATC_MULTI_RATE_YAW_IMAX        1000.0f
+#endif
+#ifndef AC_ATC_MULTI_RATE_YAW_FILT_HZ
+ # define AC_ATC_MULTI_RATE_YAW_FILT_HZ     5.0f
+#endif
+
 // TODO: change the name or move to AP_Math? eliminate in favor of degrees(100)?
 #define AC_ATTITUDE_CONTROL_DEGX100                           5729.57795f      // constant to convert from radians to centidegrees
 
+#define AC_ATTITUDE_CONTROL_ANGLE_P                           4.5f             // default angle P gain for roll, pitch and yaw
+
 #define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS             radians(40.0f)   // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
 #define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS             radians(720.0f)  // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
 #define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS              radians(10.0f)   // minimum body-frame acceleration limit for the stability controller (for yaw axis)
@@ -45,22 +79,20 @@ public:
     AC_AttitudeControl( AP_AHRS &ahrs,
                         const AP_Vehicle::MultiCopter &aparm,
                         AP_Motors& motors,
-                        AC_P& pi_angle_roll, AC_P& pi_angle_pitch, AC_P& pi_angle_yaw,
+                        float dt) :
-                        AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw
+        _p_angle_roll(AC_ATTITUDE_CONTROL_ANGLE_P),
-                        ) :
+        _p_angle_pitch(AC_ATTITUDE_CONTROL_ANGLE_P),
-        _dt(AC_ATTITUDE_400HZ_DT),
+        _p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P),
+        _dt(dt),
         _angle_boost(0),
         _att_ctrl_use_accel_limit(true),
         _throttle_in_filt(AC_ATTITUDE_CONTROL_ALTHOLD_LEANANGLE_FILT_HZ),
         _ahrs(ahrs),
         _aparm(aparm),
         _motors(motors),
-        _p_angle_roll(pi_angle_roll),
+        _pid_rate_roll(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
-        _p_angle_pitch(pi_angle_pitch),
+            _pid_rate_pitch(AC_ATC_MULTI_RATE_RP_P, AC_ATC_MULTI_RATE_RP_I, AC_ATC_MULTI_RATE_RP_D, AC_ATC_MULTI_RATE_RP_IMAX, AC_ATC_MULTI_RATE_RP_FILT_HZ, dt),
-        _p_angle_yaw(pi_angle_yaw),
+            _pid_rate_yaw(AC_ATC_MULTI_RATE_YAW_P, AC_ATC_MULTI_RATE_YAW_I, AC_ATC_MULTI_RATE_YAW_D, AC_ATC_MULTI_RATE_YAW_IMAX, AC_ATC_MULTI_RATE_YAW_FILT_HZ, dt)
-        _pid_rate_roll(pid_rate_roll),
-        _pid_rate_pitch(pid_rate_pitch),
-        _pid_rate_yaw(pid_rate_yaw)
         {
             AP_Param::setup_object_defaults(this, var_info);
         }
@@ -68,8 +100,13 @@ public:
     // Empty destructor to suppress compiler warning
     virtual ~AC_AttitudeControl() {}
 
-    // Set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
+    // pid accessors
-    void set_dt(float delta_sec);
+    AC_P& get_angle_roll_p() { return _p_angle_roll; }
+    AC_P& get_angle_pitch_p() { return _p_angle_pitch; }
+    AC_P& get_angle_yaw_p() { return _p_angle_yaw; }
+    virtual AC_PID& get_rate_roll_pid() = 0;
+    virtual AC_PID& get_rate_pitch_pid() = 0;
+    virtual AC_PID& get_rate_yaw_pid() = 0;
 
     // Gets the roll acceleration limit in centidegrees/s/s
     float get_accel_roll_max() { return _accel_roll_max; }
@@ -301,6 +338,14 @@ protected:
     // Enable/Disable angle boost
     AP_Int8             _angle_boost_enabled;
 
+    // angle controller P objects
+    AC_P                _p_angle_roll;
+    AC_P                _p_angle_pitch;
+    AC_P                _p_angle_yaw;
+    AC_PID                _pid_rate_roll;
+    AC_PID                _pid_rate_pitch;
+    AC_PID                _pid_rate_yaw;
+
     // Intersampling period in seconds
     float               _dt;
 
@@ -346,12 +391,6 @@ protected:
     const AP_AHRS&      _ahrs;
     const AP_Vehicle::MultiCopter &_aparm;
     AP_Motors&          _motors;
-    AC_P&               _p_angle_roll;
-    AC_P&               _p_angle_pitch;
-    AC_P&               _p_angle_yaw;
-    AC_PID&             _pid_rate_roll;
-    AC_PID&             _pid_rate_pitch;
-    AC_PID&             _pid_rate_yaw;
 };
 
 #define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude),	\