From bffe4fa4122e5d53fbf38bd4f22b4c5c6bcb3165 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 14 Apr 2013 12:09:45 +0900 Subject: [PATCH] Copter: remove NAV_LAT and NAV_LON Loiter and navigation controllers now combined --- ArduCopter/ArduCopter.pde | 10 -------- ArduCopter/Parameters.pde | 54 +-------------------------------------- ArduCopter/defines.h | 2 -- 3 files changed, 1 insertion(+), 65 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 297dcb128b..99e2b8b6ac 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2150,11 +2150,6 @@ static void tuning(){ g.pi_loiter_lon.kI(tuning_value); break; - case CH6_NAV_KP: - g.pid_nav_lat.kP(tuning_value); - g.pid_nav_lon.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); @@ -2170,11 +2165,6 @@ static void tuning(){ g.pid_loiter_rate_lat.kD(tuning_value); break; - case CH6_NAV_KI: - g.pid_nav_lat.kI(tuning_value); - g.pid_nav_lon.kI(tuning_value); - break; - #if FRAME_CONFIG == HELI_FRAME case CH6_HELI_EXTERNAL_GYRO: motors.ext_gyro_gain = tuning_value; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index bc95c8f520..0e4b7456ec 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -385,7 +385,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:CH6_NONE,1:CH6_STABILIZE_KP,2:CH6_STABILIZE_KI,3:CH6_YAW_KP,4:CH6_RATE_KP,5:CH6_RATE_KI,6:CH6_YAW_RATE_KP,7:CH6_THROTTLE_KP,8:CH6_TOP_BOTTOM_RATIO,9:CH6_RELAY,10:CH6_TRAVERSE_SPEED,11:CH6_NAV_KP,12:CH6_LOITER_KP,13:CH6_HELI_EXTERNAL_GYRO,14:CH6_THR_HOLD_KP,17:CH6_OPTFLOW_KP,18:CH6_OPTFLOW_KI,19:CH6_OPTFLOW_KD,20:CH6_NAV_KI,21:CH6_RATE_KD,22:CH6_LOITER_RATE_KP,23:CH6_LOITER_RATE_KD,24:CH6_YAW_KI,25:CH6_ACRO_KP,26:CH6_YAW_RATE_KD,27:CH6_LOITER_KI,28:CH6_LOITER_RATE_KI,29:CH6_STABILIZE_KD,30:CH6_AHRS_YAW_KP,31:CH6_AHRS_KP,32:CH6_INAV_TC,33:CH6_THROTTLE_KI,34:CH6_THR_ACCEL_KP,35:CH6_THR_ACCEL_KI,36:CH6_THR_ACCEL_KD + // @Values: 0:CH6_NONE,1:CH6_STABILIZE_KP,2:CH6_STABILIZE_KI,3:CH6_YAW_KP,4:CH6_RATE_KP,5:CH6_RATE_KI,6:CH6_YAW_RATE_KP,7:CH6_THROTTLE_KP,8:CH6_TOP_BOTTOM_RATIO,9:CH6_RELAY,10:CH6_TRAVERSE_SPEED,12:CH6_LOITER_KP,13:CH6_HELI_EXTERNAL_GYRO,14:CH6_THR_HOLD_KP,17:CH6_OPTFLOW_KP,18:CH6_OPTFLOW_KI,19:CH6_OPTFLOW_KD,21:CH6_RATE_KD,22:CH6_LOITER_RATE_KP,23:CH6_LOITER_RATE_KD,24:CH6_YAW_KI,25:CH6_ACRO_KP,26:CH6_YAW_RATE_KD,27:CH6_LOITER_KI,28:CH6_LOITER_RATE_KI,29:CH6_STABILIZE_KD,30:CH6_AHRS_YAW_KP,31:CH6_AHRS_KP,32:CH6_INAV_TC,33:CH6_THROTTLE_KI,34:CH6_THR_ACCEL_KP,35:CH6_THR_ACCEL_KI,36:CH6_THR_ACCEL_KD GSCALAR(radio_tuning, "TUNE", 0), // @Param: TUNE_LOW @@ -657,58 +657,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GGROUP(pid_loiter_rate_lon, "LOITER_LON_", AC_PID), - // @Param: NAV_LAT_P - // @DisplayName: Navigation latitude rate controller P gain - // @Description: Navigation latitude rate controller P gain. Converts the difference between desired speed and actual speed into a lean angle in the latitude direction - // @Range: 2.000 2.800 - // @User: Standard - - // @Param: NAV_LAT_I - // @DisplayName: Navigation latitude rate controller I gain - // @Description: Navigation latitude rate controller I gain. Corrects long-term difference in desired speed and actual speed in the latitude direction - // @Range: 0.140 0.200 - // @User: Standard - - // @Param: NAV_LAT_IMAX - // @DisplayName: Navigation rate controller I gain maximum - // @Description: Navigation rate controller I gain maximum. Constrains the lean angle that the I gain will output - // @Range: 0 4500 - // @Unit: Centi-Degrees - // @User: Standard - - // @Param: NAV_LAT_D - // @DisplayName: Navigation latitude rate controller D gain - // @Description: Navigation latitude rate controller D gain. Compensates for short-term change in desired speed vs actual speed - // @Range: 0.000 0.100 - // @User: Standard - GGROUP(pid_nav_lat, "NAV_LAT_", AC_PID), - - // @Param: NAV_LON_P - // @DisplayName: Navigation longitude rate controller P gain - // @Description: Navigation longitude rate controller P gain. Converts the difference between desired speed and actual speed into a lean angle in the longitude direction - // @Range: 2.000 2.800 - // @User: Standard - - // @Param: NAV_LON_I - // @DisplayName: Navigation longitude rate controller I gain - // @Description: Navigation longitude rate controller I gain. Corrects long-term difference in desired speed and actual speed in the longitude direction - // @Range: 0.140 0.200 - // @User: Standard - - // @Param: NAV_LON_IMAX - // @DisplayName: Navigation longitude rate controller I gain maximum - // @Description: Navigation longitude rate controller I gain maximum. Constrains the lean angle that the I gain will generate - // @Range: 0 4500 - // @Unit: Centi-Degrees - // @User: Standard - - // @Param: NAV_LON_D - // @DisplayName: Navigation longituderate controller D gain - // @Description: Navigation longitude rate controller D gain. Compensates for short-term change in desired speed vs actual speed - // @Range: 0.000 0.100 - // @User: Standard - GGROUP(pid_nav_lon, "NAV_LON_", AC_PID), - // @Param: THR_RATE_P // @DisplayName: Throttle rate controller P gain // @Description: Throttle rate controller P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index c48c094eca..c4113d9878 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -162,8 +162,6 @@ #define CH6_TOP_BOTTOM_RATIO 8 // upper/lower motor ratio (not used) #define CH6_RELAY 9 // switch relay on if ch6 high, off if low #define CH6_TRAVERSE_SPEED 10 // maximum speed to next way point (0 to 10m/s) -#define CH6_NAV_KP 11 // navigation rate controller's P term (speed error to tilt angle) -#define CH6_NAV_KI 20 // navigation rate controller's I term (speed error to tilt angle) #define CH6_LOITER_KP 12 // loiter distance controller's P term (position error to speed) #define CH6_LOITER_KI 27 // loiter distance controller's I term (position error to speed) #define CH6_HELI_EXTERNAL_GYRO 13 // TradHeli specific external tail gyro gain