diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 6c80d2bee5..58bf35265c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -118,6 +118,7 @@ #include // Mission command library #include // Rally point library #include // PID library +#include // PID library (2-axis) #include // Heli specific Rate PID library #include // P library #include // Attitude control library @@ -623,7 +624,7 @@ AC_AttitudeControl attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p #endif AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control, 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); + g.p_loiter_pos, g.pi_vel_xy); static AC_WPNav wp_nav(inertial_nav, ahrs, pos_control, attitude_control); static AC_Circle circle_nav(inertial_nav, ahrs, pos_control); diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 53ae962fed..8ef2bd88e7 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -301,8 +301,8 @@ public: k_param_p_stabilize_yaw, k_param_p_loiter_pos, k_param_p_loiter_lon, // remove - k_param_pid_loiter_rate_lat, - k_param_pid_loiter_rate_lon, + k_param_pid_loiter_rate_lat, // remove + k_param_pid_loiter_rate_lon, // remove k_param_pid_nav_lat, // 233 - remove k_param_pid_nav_lon, // 234 - remove k_param_p_alt_hold, @@ -317,6 +317,7 @@ public: k_param_acro_yaw_p, // 244 k_param_autotune_axis_bitmask, // 245 k_param_autotune_aggressiveness, // 246 + k_param_pi_vel_xy, // 247 // 254,255: reserved }; @@ -452,8 +453,7 @@ public: AC_PID pid_rate_pitch; AC_PID pid_rate_yaw; #endif - AC_PID pid_loiter_rate_lat; - AC_PID pid_loiter_rate_lon; + AC_PI_2D pi_vel_xy; AC_P p_throttle_rate; AC_PID pid_throttle_accel; @@ -515,8 +515,7 @@ public: pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS), pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS), - pid_loiter_rate_lat (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX, LOITER_RATE_FILT_HZ,WPNAV_LOITER_UPDATE_TIME), - pid_loiter_rate_lon (LOITER_RATE_P, LOITER_RATE_I, LOITER_RATE_D, LOITER_RATE_IMAX, LOITER_RATE_FILT_HZ,WPNAV_LOITER_UPDATE_TIME), + pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME), p_throttle_rate (THROTTLE_RATE_P), pid_throttle_accel (THROTTLE_ACCEL_P,THROTTLE_ACCEL_I, THROTTLE_ACCEL_D,THROTTLE_ACCEL_IMAX, THROTTLE_ACCEL_FILT_HZ, MAIN_LOOP_SECONDS), diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index ed6ded4a49..319bc448dc 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -334,7 +334,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,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,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,30:AHRS Yaw kP,31:AHRS kP,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF + // @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:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,13:Heli Ext Gyro,17:OF Loiter kP,18:OF Loiter kI,19:OF Loiter kD,30:AHRS Yaw kP,31:AHRS kP,38:Declination,39:Circle Rate,41:RangeFinder Gain,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF GSCALAR(radio_tuning, "TUNE", 0), // @Param: TUNE_LOW @@ -680,65 +680,28 @@ const AP_Param::Info var_info[] PROGMEM = { GGROUP(pid_rate_yaw, "RATE_YAW_", AC_PID), #endif - // @Param: LOITER_LAT_P - // @DisplayName: Loiter latitude rate controller P gain - // @Description: Loiter latitude rate controller P gain. Converts the difference between desired speed and actual speed into a lean angle in the latitude direction + // @Param: VEL_XY_P + // @DisplayName: Velocity (horizontal) P gain + // @Description: Velocity (horizontal) P gain. Converts the difference between desired velocity to a target acceleration // @Range: 0.1 6.0 // @Increment: 0.1 // @User: Advanced - // @Param: LOITER_LAT_I - // @DisplayName: Loiter latitude rate controller I gain - // @Description: Loiter latitude rate controller I gain. Corrects long-term difference in desired speed and actual speed in the latitude direction + // @Param: VEL_XY_I + // @DisplayName: Velocity (horizontal) I gain + // @Description: Velocity (horizontal) I gain. Corrects long-term difference in desired velocity to a target acceleration // @Range: 0.02 1.00 // @Increment: 0.01 // @User: Advanced - // @Param: LOITER_LAT_IMAX - // @DisplayName: Loiter rate controller I gain maximum - // @Description: Loiter rate controller I gain maximum. Constrains the lean angle that the I gain will output + // @Param: VEL_XY_IMAX + // @DisplayName: Velocity (horizontal) integrator maximum + // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output // @Range: 0 4500 // @Increment: 10 - // @Units: Centi-Degrees + // @Units: cm/s/s // @User: Advanced - - // @Param: LOITER_LAT_D - // @DisplayName: Loiter latitude rate controller D gain - // @Description: Loiter latitude rate controller D gain. Compensates for short-term change in desired speed vs actual speed - // @Range: 0.0 0.6 - // @Increment: 0.01 - // @User: Advanced - GGROUP(pid_loiter_rate_lat, "LOITER_LAT_", AC_PID), - - // @Param: LOITER_LON_P - // @DisplayName: Loiter longitude rate controller P gain - // @Description: Loiter longitude rate controller P gain. Converts the difference between desired speed and actual speed into a lean angle in the longitude direction - // @Range: 0.1 6.0 - // @Increment: 0.1 - // @User: Advanced - - // @Param: LOITER_LON_I - // @DisplayName: Loiter longitude rate controller I gain - // @Description: Loiter longitude rate controller I gain. Corrects long-term difference in desired speed and actual speed in the longitude direction - // @Range: 0.02 1.00 - // @Increment: 0.01 - // @User: Advanced - - // @Param: LOITER_LON_IMAX - // @DisplayName: Loiter longitude rate controller I gain maximum - // @Description: Loiter longitude rate controller I gain maximum. Constrains the lean angle that the I gain will output - // @Range: 0 4500 - // @Increment: 10 - // @Units: Centi-Degrees - // @User: Advanced - - // @Param: LOITER_LON_D - // @DisplayName: Loiter longituderate controller D gain - // @Description: Loiter longitude rate controller D gain. Compensates for short-term change in desired speed vs actual speed - // @Range: 0.0 0.6 - // @Increment: 0.01 - // @User: Advanced - GGROUP(pid_loiter_rate_lon, "LOITER_LON_", AC_PID), + GGROUP(pi_vel_xy, "VEL_XY_", AC_PI_2D), // @Param: THR_RATE_P // @DisplayName: Throttle rate controller P gain diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 0f7cbaef2f..ef291b1564 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -641,22 +641,19 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// Loiter rate control gains +// Velocity (horizontal) gains // -#ifndef LOITER_RATE_P - # define LOITER_RATE_P 1.0f +#ifndef VEL_XY_P + # define VEL_XY_P 1.0f #endif -#ifndef LOITER_RATE_I - # define LOITER_RATE_I 0.5f +#ifndef VEL_XY_I + # define VEL_XY_I 0.5f #endif -#ifndef LOITER_RATE_D - # define LOITER_RATE_D 0.0f +#ifndef VEL_XY_IMAX + # define VEL_XY_IMAX 1000 #endif -#ifndef LOITER_RATE_IMAX - # define LOITER_RATE_IMAX 1000 // maximum acceleration from I term build-up in cm/s/s -#endif -#ifndef LOITER_RATE_FILT_HZ - # define LOITER_RATE_FILT_HZ 5.0f +#ifndef VEL_XY_FILT_HZ + # define VEL_XY_FILT_HZ 5.0f #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 15a53c3313..8fd05aa02c 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -122,9 +122,8 @@ #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 #define CH6_LOITER_POSITION_KP 12 // loiter distance controller's P term (position error to speed) -#define CH6_LOITER_RATE_KP 22 // loiter rate controller's P term (speed error to tilt angle) -#define CH6_LOITER_RATE_KI 28 // loiter rate controller's I term (speed error to tilt angle) -#define CH6_LOITER_RATE_KD 23 // loiter rate controller's D term (speed error to tilt angle) +#define CH6_VEL_XY_KP 22 // loiter rate controller's P term (speed error to tilt angle) +#define CH6_VEL_XY_KI 28 // loiter rate controller's I term (speed error to tilt angle) #define CH6_WP_SPEED 10 // maximum speed to next way point (0 to 10m/s) #define CH6_ACRO_RP_KP 25 // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate #define CH6_ACRO_YAW_KP 40 // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate diff --git a/ArduCopter/tuning.pde b/ArduCopter/tuning.pde index 947cf5adb0..04aaec03cb 100644 --- a/ArduCopter/tuning.pde +++ b/ArduCopter/tuning.pde @@ -79,19 +79,12 @@ static void tuning() { g.p_loiter_pos.kP(tuning_value); break; - case CH6_LOITER_RATE_KP: - g.pid_loiter_rate_lon.kP(tuning_value); - g.pid_loiter_rate_lat.kP(tuning_value); + case CH6_VEL_XY_KP: + g.pi_vel_xy.kP(tuning_value); break; - case CH6_LOITER_RATE_KI: - g.pid_loiter_rate_lon.kI(tuning_value); - g.pid_loiter_rate_lat.kI(tuning_value); - break; - - case CH6_LOITER_RATE_KD: - g.pid_loiter_rate_lon.kD(tuning_value); - g.pid_loiter_rate_lat.kD(tuning_value); + case CH6_VEL_XY_KI: + g.pi_vel_xy.kI(tuning_value); break; case CH6_WP_SPEED: