From 00913ffe1b7db97a5c07f57fe733c3d045e324b5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 16 Feb 2014 00:00:05 +0900 Subject: [PATCH] Copter: throttle rate to simple P controller Saves at least 15bytes of RAM --- ArduCopter/ArduCopter.pde | 8 ++------ ArduCopter/Parameters.h | 24 +++++++++++------------- ArduCopter/Parameters.pde | 4 ++-- ArduCopter/config.h | 10 ---------- ArduCopter/defines.h | 1 - 5 files changed, 15 insertions(+), 32 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4c9d10a418..a33e64b621 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -684,7 +684,7 @@ AC_AttitudeControl attitude_control(ahrs, ins, aparm, motors, g.p_stabilize_roll g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw); #endif AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control, - g.p_alt_hold, g.pid_throttle_rate, g.pid_throttle_accel, + g.p_alt_hold, g.p_throttle_rate, g.pid_throttle_accel, g.p_loiter_pos, g.pid_loiter_rate_lat, g.pid_loiter_rate_lon); static AC_WPNav wp_nav(&inertial_nav, &ahrs, pos_control); static AC_Circle circle_nav(inertial_nav, ahrs, pos_control); @@ -1421,11 +1421,7 @@ static void tuning(){ break; case CH6_THROTTLE_RATE_KP: - g.pid_throttle_rate.kP(tuning_value); - break; - - case CH6_THROTTLE_RATE_KD: - g.pid_throttle_rate.kD(tuning_value); + g.p_throttle_rate.kP(tuning_value); break; case CH6_THROTTLE_ACCEL_KP: diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ac9771a83c..e7ecb2f492 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -274,7 +274,7 @@ public: k_param_pid_nav_lat, // 233 - remove k_param_pid_nav_lon, // 234 - remove k_param_p_alt_hold, - k_param_pid_throttle_rate, + k_param_p_throttle_rate, k_param_pid_optflow_roll, k_param_pid_optflow_pitch, k_param_acro_balance_roll_old, // 239 - remove @@ -415,7 +415,7 @@ public: AC_PID pid_loiter_rate_lat; AC_PID pid_loiter_rate_lon; - AC_PID pid_throttle_rate; + AC_P p_throttle_rate; AC_PID pid_throttle_accel; AC_PID pid_optflow_roll; AC_PID pid_optflow_pitch; @@ -465,9 +465,8 @@ public: rc_12 (CH_12), #endif - // PID controller initial P initial I initial D - // initial imax - //----------------------------------------------------------------------------------------------------- + // PID controller initial P initial I initial D initial imax + //---------------------------------------------------------------------------------------------------------- pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX), pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX), pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX), @@ -475,21 +474,20 @@ public: pid_loiter_rate_lat (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX), pid_loiter_rate_lon (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX), - pid_throttle_rate (THROTTLE_RATE_P, THROTTLE_RATE_I, THROTTLE_RATE_D, THROTTLE_RATE_IMAX), + p_throttle_rate (THROTTLE_RATE_P), pid_throttle_accel (THROTTLE_ACCEL_P, THROTTLE_ACCEL_I, THROTTLE_ACCEL_D, THROTTLE_ACCEL_IMAX), pid_optflow_roll (OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX), pid_optflow_pitch (OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX), - // PI controller initial P initial I initial - // imax + // P controller initial P //---------------------------------------------------------------------- - p_loiter_pos (LOITER_POS_P), + p_loiter_pos (LOITER_POS_P), - p_stabilize_roll (STABILIZE_ROLL_P), - p_stabilize_pitch (STABILIZE_PITCH_P), - p_stabilize_yaw (STABILIZE_YAW_P), + p_stabilize_roll (STABILIZE_ROLL_P), + p_stabilize_pitch (STABILIZE_PITCH_P), + p_stabilize_yaw (STABILIZE_YAW_P), - p_alt_hold (ALT_HOLD_P) + p_alt_hold (ALT_HOLD_P) { } }; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 8f0cbe1d9c..b334490fac 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -360,7 +360,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @DisplayName: Channel 6 Tuning // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob // @User: Standard - // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,14:Altitude Hold kP,7:Throttle Rate kP,37:Throttle Rate kD,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,42:Loiter Speed,12:Loiter Pos kP,22:Loiter Rate kP,28:Loiter Rate kI,23:Loiter Rate kD,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,9:Relay On/Off,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,30:AHRS Yaw kP,31:AHRS kP,32:INAV_TC,38:Declination,39:Circle Rate,41:Sonar Gain + // @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,14:Altitude Hold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,42:Loiter Speed,12:Loiter Pos kP,22:Loiter Rate kP,28:Loiter Rate kI,23:Loiter Rate kD,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,9:Relay On/Off,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,30:AHRS Yaw kP,31:AHRS kP,32:INAV_TC,38:Declination,39:Circle Rate,41:Sonar Gain GSCALAR(radio_tuning, "TUNE", 0), // @Param: TUNE_LOW @@ -720,7 +720,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Description: Throttle rate controller D gain. Compensates for short-term change in desired vertical speed vs actual speed // @Range: 0.000 0.400 // @User: Standard - GGROUP(pid_throttle_rate, "THR_RATE_", AC_PID), + GGROUP(p_throttle_rate, "THR_RATE_", AC_P), // @Param: THR_ACCEL_P // @DisplayName: Throttle acceleration controller P gain diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 1f940985bd..4346ff9490 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -663,16 +663,6 @@ #ifndef THROTTLE_RATE_P # define THROTTLE_RATE_P 6.0f #endif -#ifndef THROTTLE_RATE_I - # define THROTTLE_RATE_I 0.0f -#endif -#ifndef THROTTLE_RATE_D - # define THROTTLE_RATE_D 0.0f -#endif - -#ifndef THROTTLE_RATE_IMAX - # define THROTTLE_RATE_IMAX 300 -#endif // default maximum vertical velocity the pilot may request #ifndef PILOT_VELZ_MAX diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 955872aff8..39b23a21d1 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -123,7 +123,6 @@ #define CH6_YAW_RATE_KD 26 // body frame yaw rate controller's D term #define CH6_ALTITUDE_HOLD_KP 14 // altitude hold controller's P term (alt error to desired rate) #define CH6_THROTTLE_RATE_KP 7 // throttle rate controller's P term (desired rate to acceleration or motor output) -#define CH6_THROTTLE_RATE_KD 37 // throttle rate controller's D term (desired rate to acceleration or motor output) #define CH6_THROTTLE_ACCEL_KP 34 // accel based throttle controller's P term #define CH6_THROTTLE_ACCEL_KI 35 // accel based throttle controller's I term #define CH6_THROTTLE_ACCEL_KD 36 // accel based throttle controller's D term