From b3da8a462f2aeb34a35796228822486d890f6fef Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 29 Jun 2013 23:51:43 -1000 Subject: [PATCH] Copter: CH6 tuning definition clean-up Renamed and reorganised the CH6 #defines and parameter definitions --- ArduCopter/ArduCopter.pde | 98 +++++++++++++++++---------------------- ArduCopter/Attitude.pde | 28 +++++------ ArduCopter/Parameters.pde | 2 +- ArduCopter/defines.h | 65 ++++++++++++-------------- 4 files changed, 87 insertions(+), 106 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index feff539ec5..afc1badc2f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2042,41 +2042,30 @@ static void tuning(){ switch(g.radio_tuning) { - case CH6_RATE_KD: - g.pid_rate_roll.kD(tuning_value); - g.pid_rate_pitch.kD(tuning_value); - break; - - case CH6_STABILIZE_KP: + // Roll, Pitch tuning + case CH6_STABILIZE_ROLL_PITCH_KP: g.pi_stabilize_roll.kP(tuning_value); g.pi_stabilize_pitch.kP(tuning_value); break; - case CH6_STABILIZE_KI: - g.pi_stabilize_roll.kI(tuning_value); - g.pi_stabilize_pitch.kI(tuning_value); - break; - - case CH6_ACRO_KP: - g.acro_p = tuning_value; - break; - - case CH6_RATE_KP: + case CH6_RATE_ROLL_PITCH_KP: g.pid_rate_roll.kP(tuning_value); g.pid_rate_pitch.kP(tuning_value); break; - case CH6_RATE_KI: + case CH6_RATE_ROLL_PITCH_KI: g.pid_rate_roll.kI(tuning_value); g.pid_rate_pitch.kI(tuning_value); break; - case CH6_YAW_KP: - g.pi_stabilize_yaw.kP(tuning_value); + case CH6_RATE_ROLL_PITCH_KD: + g.pid_rate_roll.kD(tuning_value); + g.pid_rate_pitch.kD(tuning_value); break; - case CH6_YAW_KI: - g.pi_stabilize_yaw.kI(tuning_value); + // Yaw tuning + case CH6_STABILIZE_YAW_KP: + g.pi_stabilize_yaw.kP(tuning_value); break; case CH6_YAW_RATE_KP: @@ -2087,38 +2076,37 @@ static void tuning(){ g.pid_rate_yaw.kD(tuning_value); break; - case CH6_THROTTLE_KP: + // Altitude and throttle tuning + case CH6_ALTITUDE_HOLD_KP: + g.pi_alt_hold.kP(tuning_value); + break; + + case CH6_THROTTLE_RATE_KP: g.pid_throttle.kP(tuning_value); break; - case CH6_THROTTLE_KI: - g.pid_throttle.kI(tuning_value); - break; - - case CH6_THROTTLE_KD: + case CH6_THROTTLE_RATE_KD: g.pid_throttle.kD(tuning_value); break; - case CH6_RELAY: - if (g.rc_6.control_in > 525) relay.on(); - if (g.rc_6.control_in < 475) relay.off(); + case CH6_THROTTLE_ACCEL_KP: + g.pid_throttle_accel.kP(tuning_value); break; - case CH6_WP_SPEED: - // set waypoint navigation horizontal speed to 0 ~ 1000 cm/s - wp_nav.set_horizontal_velocity(g.rc_6.control_in); + case CH6_THROTTLE_ACCEL_KI: + g.pid_throttle_accel.kI(tuning_value); break; - case CH6_LOITER_KP: + case CH6_THROTTLE_ACCEL_KD: + g.pid_throttle_accel.kD(tuning_value); + break; + + // Loiter and navigation tuning + case CH6_LOITER_POSITION_KP: g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value); break; - case CH6_LOITER_KI: - g.pi_loiter_lat.kI(tuning_value); - g.pi_loiter_lon.kI(tuning_value); - break; - case CH6_LOITER_RATE_KP: g.pid_loiter_rate_lon.kP(tuning_value); g.pid_loiter_rate_lat.kP(tuning_value); @@ -2134,16 +2122,27 @@ static void tuning(){ g.pid_loiter_rate_lat.kD(tuning_value); break; + case CH6_WP_SPEED: + // set waypoint navigation horizontal speed to 0 ~ 1000 cm/s + wp_nav.set_horizontal_velocity(g.rc_6.control_in); + break; + + // Acro and other tuning + case CH6_ACRO_KP: + g.acro_p = tuning_value; + break; + + case CH6_RELAY: + if (g.rc_6.control_in > 525) relay.on(); + if (g.rc_6.control_in < 475) relay.off(); + break; + #if FRAME_CONFIG == HELI_FRAME case CH6_HELI_EXTERNAL_GYRO: motors.ext_gyro_gain = tuning_value; break; #endif - case CH6_THR_HOLD_KP: - g.pi_alt_hold.kP(tuning_value); - break; - case CH6_OPTFLOW_KP: g.pid_optflow_roll.kP(tuning_value); g.pid_optflow_pitch.kP(tuning_value); @@ -2175,18 +2174,6 @@ static void tuning(){ inertial_nav.set_time_constant_z(tuning_value); break; - case CH6_THR_ACCEL_KP: - g.pid_throttle_accel.kP(tuning_value); - break; - - case CH6_THR_ACCEL_KI: - g.pid_throttle_accel.kI(tuning_value); - break; - - case CH6_THR_ACCEL_KD: - g.pid_throttle_accel.kD(tuning_value); - break; - case CH6_DECLINATION: // set declination to +-20degrees compass.set_declination(ToRad((2.0f * g.rc_6.control_in - g.radio_tuning_high)/100.0f), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact @@ -2195,7 +2182,6 @@ static void tuning(){ case CH6_CIRCLE_RATE: // set circle rate g.circle_rate.set(g.rc_6.control_in/25-20); // allow approximately 45 degree turn rate in either direction - //cliSerial->printf_P(PSTR("\nRate:%4.2f"),(float)g.circle_rate); break; } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index df00720b06..2615d47331 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -59,11 +59,11 @@ get_stabilize_yaw(int32_t target_angle) #if LOGGING_ENABLED == ENABLED // log output if PID logging is on and we are tuning the yaw - if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_YAW_KP ) { + if( g.log_bitmask & MASK_LOG_PID && g.radio_tuning == CH6_STABILIZE_YAW_KP ) { pid_log_counter++; if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 pid_log_counter = 0; - Log_Write_PID(CH6_YAW_KP, angle_error, target_rate, i_term, 0, output, tuning_value); + Log_Write_PID(CH6_STABILIZE_YAW_KP, angle_error, target_rate, i_term, 0, output, tuning_value); } } #endif @@ -318,11 +318,11 @@ get_heli_rate_roll(int32_t target_rate) #if LOGGING_ENABLED == ENABLED // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_ROLL_PITCH_KP || g.radio_tuning == CH6_RATE_ROLL_PITCH_KI || g.radio_tuning == CH6_RATE_ROLL_PITCH_KD) ) { pid_log_counter++; if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 pid_log_counter = 0; - Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d, output, tuning_value); + Log_Write_PID(CH6_RATE_ROLL_PITCH_KP, rate_error, p, i, d, output, tuning_value); } } #endif @@ -370,9 +370,9 @@ get_heli_rate_pitch(int32_t target_rate) #if LOGGING_ENABLED == ENABLED // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_ROLL_PITCH_KP || g.radio_tuning == CH6_RATE_ROLL_PITCH_KI || g.radio_tuning == CH6_RATE_ROLL_PITCH_KD) ) { if( pid_log_counter == 0 ) { // relies on get_heli_rate_roll to update pid_log_counter - Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, 0, output, tuning_value); + Log_Write_PID(CH6_RATE_ROLL_PITCH_KP+100, rate_error, p, i, 0, output, tuning_value); } } #endif @@ -458,11 +458,11 @@ get_rate_roll(int32_t target_rate) #if LOGGING_ENABLED == ENABLED // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_ROLL_PITCH_KP || g.radio_tuning == CH6_RATE_ROLL_PITCH_KI || g.radio_tuning == CH6_RATE_ROLL_PITCH_KD) ) { pid_log_counter++; // Note: get_rate_pitch pid logging relies on this function to update pid_log_counter so if you change the line above you must change the equivalent line in get_rate_pitch if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10 pid_log_counter = 0; - Log_Write_PID(CH6_RATE_KP, rate_error, p, i, d, output, tuning_value); + Log_Write_PID(CH6_RATE_ROLL_PITCH_KP, rate_error, p, i, d, output, tuning_value); } } #endif @@ -499,9 +499,9 @@ get_rate_pitch(int32_t target_rate) #if LOGGING_ENABLED == ENABLED // log output if PID logging is on and we are tuning the rate P, I or D gains - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_KP || g.radio_tuning == CH6_RATE_KI || g.radio_tuning == CH6_RATE_KD) ) { + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_RATE_ROLL_PITCH_KP || g.radio_tuning == CH6_RATE_ROLL_PITCH_KI || g.radio_tuning == CH6_RATE_ROLL_PITCH_KD) ) { if( pid_log_counter == 0 ) { // relies on get_rate_roll having updated pid_log_counter - Log_Write_PID(CH6_RATE_KP+100, rate_error, p, i, d, output, tuning_value); + Log_Write_PID(CH6_RATE_ROLL_PITCH_KP+100, rate_error, p, i, d, output, tuning_value); } } #endif @@ -854,11 +854,11 @@ get_throttle_accel(int16_t z_target_accel) #if LOGGING_ENABLED == ENABLED // log output if PID loggins is on and we are tuning the yaw - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THR_ACCEL_KP || g.radio_tuning == CH6_THR_ACCEL_KI || g.radio_tuning == CH6_THR_ACCEL_KD) ) { + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_ACCEL_KP || g.radio_tuning == CH6_THROTTLE_ACCEL_KI || g.radio_tuning == CH6_THROTTLE_ACCEL_KD) ) { pid_log_counter++; if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz pid_log_counter = 0; - Log_Write_PID(CH6_THR_ACCEL_KP, z_accel_error, p, i, d, output, tuning_value); + Log_Write_PID(CH6_THROTTLE_ACCEL_KP, z_accel_error, p, i, d, output, tuning_value); } } #endif @@ -1003,11 +1003,11 @@ get_throttle_rate(float z_target_speed) #if LOGGING_ENABLED == ENABLED // log output if PID loggins is on and we are tuning the yaw - if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_KP || g.radio_tuning == CH6_THROTTLE_KI || g.radio_tuning == CH6_THROTTLE_KD) ) { + if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_THROTTLE_RATE_KP || g.radio_tuning == CH6_THROTTLE_RATE_KD) ) { pid_log_counter++; if( pid_log_counter >= 10 ) { // (update rate / desired output rate) = (50hz / 10hz) = 5hz pid_log_counter = 0; - Log_Write_PID(CH6_THROTTLE_KP, z_rate_error, p, i, d, output, tuning_value); + Log_Write_PID(CH6_THROTTLE_RATE_KP, z_rate_error, p, i, d, output, tuning_value); } } #endif diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index adab7947ac..3d99cad630 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,9:CH6_RELAY,10:CH6_WP_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,38:CH6_DECLINATION,39:CH6_CIRCLE_RATE + // @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,12:Loiter Pos kP,22:Loiter Rate kP,28:Loiter Rate kI,23:Loiter Rate kD,10:WP Speed,25:Acro 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 GSCALAR(radio_tuning, "TUNE", 0), // @Param: TUNE_LOW diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index e3ed2eda1e..011823521c 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -137,41 +137,36 @@ // CH_6 Tuning // ----------- -#define CH6_NONE 0 // no tuning performed -#define CH6_STABILIZE_KP 1 // stabilize roll/pitch angle controller's P term -#define CH6_STABILIZE_KI 2 // stabilize roll/pitch angle controller's I term -#define CH6_STABILIZE_KD 29 // stabilize roll/pitch angle controller's D term -#define CH6_YAW_KP 3 // stabilize yaw heading controller's P term -#define CH6_YAW_KI 24 // stabilize yaw heading controller's P term -#define CH6_ACRO_KP 25 // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate -#define CH6_RATE_KP 4 // body frame roll/pitch rate controller's P term -#define CH6_RATE_KI 5 // body frame roll/pitch rate controller's I term -#define CH6_RATE_KD 21 // body frame roll/pitch rate controller's D term -#define CH6_YAW_RATE_KP 6 // body frame yaw rate controller's P term -#define CH6_YAW_RATE_KD 26 // body frame yaw rate controller's D term -#define CH6_THR_HOLD_KP 14 // altitude hold controller's P term (alt error to desired rate) -#define CH6_THROTTLE_KP 7 // throttle rate controller's P term (desired rate to acceleration or motor output) -#define CH6_THROTTLE_KI 33 // throttle rate controller's I term (desired rate to acceleration or motor output) -#define CH6_THROTTLE_KD 37 // throttle rate controller's D term (desired rate to acceleration or motor output) -#define CH6_THR_ACCEL_KP 34 // accel based throttle controller's P term -#define CH6_THR_ACCEL_KI 35 // accel based throttle controller's I term -#define CH6_THR_ACCEL_KD 36 // accel based throttle controller's D term -#define CH6_RELAY 9 // switch relay on if ch6 high, off if low -#define CH6_WP_SPEED 10 // maximum speed to next way point (0 to 10m/s) -#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 -#define CH6_OPTFLOW_KP 17 // optical flow loiter controller's P term (position error to tilt angle) -#define CH6_OPTFLOW_KI 18 // optical flow loiter controller's I term (position error to tilt angle) -#define CH6_OPTFLOW_KD 19 // optical flow loiter controller's D term (position error to tilt angle) -#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_AHRS_YAW_KP 30 // ahrs's compass effect on yaw angle (0 = very low, 1 = very high) -#define CH6_AHRS_KP 31 // accelerometer effect on roll/pitch angle (0=low) -#define CH6_INAV_TC 32 // inertial navigation baro/accel and gps/accel time constant (1.5 = strong baro/gps correction on accel estimatehas very strong does not correct accel estimate, 7 = very weak correction) -#define CH6_DECLINATION 38 // compass declination in radians -#define CH6_CIRCLE_RATE 39 // circle turn rate in degrees (hard coded to about 45 degrees in either direction) +#define CH6_NONE 0 // no tuning performed +#define CH6_STABILIZE_ROLL_PITCH_KP 1 // stabilize roll/pitch angle controller's P term +#define CH6_RATE_ROLL_PITCH_KP 4 // body frame roll/pitch rate controller's P term +#define CH6_RATE_ROLL_PITCH_KI 5 // body frame roll/pitch rate controller's I term +#define CH6_RATE_ROLL_PITCH_KD 21 // body frame roll/pitch rate controller's D term +#define CH6_STABILIZE_YAW_KP 3 // stabilize yaw heading controller's P term +#define CH6_YAW_RATE_KP 6 // body frame yaw rate controller's P term +#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 +#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_WP_SPEED 10 // maximum speed to next way point (0 to 10m/s) +#define CH6_ACRO_KP 25 // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate +#define CH6_RELAY 9 // switch relay on if ch6 high, off if low +#define CH6_HELI_EXTERNAL_GYRO 13 // TradHeli specific external tail gyro gain +#define CH6_OPTFLOW_KP 17 // optical flow loiter controller's P term (position error to tilt angle) +#define CH6_OPTFLOW_KI 18 // optical flow loiter controller's I term (position error to tilt angle) +#define CH6_OPTFLOW_KD 19 // optical flow loiter controller's D term (position error to tilt angle) +#define CH6_AHRS_YAW_KP 30 // ahrs's compass effect on yaw angle (0 = very low, 1 = very high) +#define CH6_AHRS_KP 31 // accelerometer effect on roll/pitch angle (0=low) +#define CH6_INAV_TC 32 // inertial navigation baro/accel and gps/accel time constant (1.5 = strong baro/gps correction on accel estimatehas very strong does not correct accel estimate, 7 = very weak correction) +#define CH6_DECLINATION 38 // compass declination in radians +#define CH6_CIRCLE_RATE 39 // circle turn rate in degrees (hard coded to about 45 degrees in either direction) // Commands - Note that APM now uses a subset of the MAVLink protocol