Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Change CH6_Tuning #defines into Enum

Browse Source
This commit is contained in:
Robert Lefebvre 2015-03-24 16:43:58 -04:00 committed by Randy Mackay
parent 167507e28b
commit 39d23519b1
2 changed files with 80 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
ArduCopter/defines.h
View File

@ -113,53 +113,49 @@ enum aux_sw_func {
#define NUM_MODES 18
// CH_6 Tuning
// -----------
#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_ACCEL_Z_KP 34 // accel based throttle controller's P term
#define CH6_ACCEL_Z_KI 35 // accel based throttle controller's I term
#define CH6_ACCEL_Z_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_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
#define CH6_RELAY 9 // deprecated -- remove
#define CH6_HELI_EXTERNAL_GYRO 13 // TradHeli specific external tail gyro gain
#define CH6_OPTFLOW_KP 17 // deprecated -- remove
#define CH6_OPTFLOW_KI 18 // deprecated -- remove
#define CH6_OPTFLOW_KD 19 // deprecated -- remove
#define CH6_AHRS_YAW_KP 30 // deprecated -- remove
#define CH6_AHRS_KP 31 // deprecated -- remove
#define CH6_INAV_TC 32 // deprecated -- remove
#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_SONAR_GAIN 41 // sonar gain
#define CH6_EKF_VERTICAL_POS 42 // EKF's baro vs accel (higher rely on accels more, baro impact is reduced). Range should be 0.2 ~ 4.0? 2.0 is default
#define CH6_EKF_HORIZONTAL_POS 43 // EKF's gps vs accel (higher rely on accels more, gps impact is reduced). Range should be 1.0 ~ 3.0? 1.5 is default
#define CH6_EKF_ACCEL_NOISE 44 // EKF's accel noise (lower means trust accels more, gps & baro less). Range should be 0.02 ~ 0.5 0.5 is default (but very robust at that level)
#define CH6_RC_FEEL_RP 45 // roll-pitch input smoothing
#define CH6_RATE_PITCH_KP 46 // body frame pitch rate controller's P term
#define CH6_RATE_PITCH_KI 47 // body frame pitch rate controller's I term
#define CH6_RATE_PITCH_KD 48 // body frame pitch rate controller's D term
#define CH6_RATE_ROLL_KP 49 // body frame roll rate controller's P term
#define CH6_RATE_ROLL_KI 50 // body frame roll rate controller's I term
#define CH6_RATE_ROLL_KD 51 // body frame roll rate controller's D term
#define CH6_RATE_PITCH_FF 52 // body frame pitch rate controller FF term
#define CH6_RATE_ROLL_FF 53 // body frame roll rate controller FF term
#define CH6_RATE_YAW_FF 54 // body frame yaw rate controller FF term
#define CH6_RATE_MOT_YAW_HEADROOM 55 // motors yaw headroom minimum
#define CH6_RATE_YAW_FILT 56 // yaw rate input filter
// Tuning enumeration
enum tuning_func {
TUNING_NONE = 0, // 0,
TUNING_STABILIZE_ROLL_PITCH_KP, // 1, stabilize roll/pitch angle controller's P term
TUNING_STABILIZE_YAW_KP = 3, // 3, stabilize yaw heading controller's P term
TUNING_RATE_ROLL_PITCH_KP, // 4, body frame roll/pitch rate controller's P term
TUNING_RATE_ROLL_PITCH_KI, // 5, body frame roll/pitch rate controller's I term
TUNING_YAW_RATE_KP, // 6, body frame yaw rate controller's P term
TUNING_THROTTLE_RATE_KP, // 7, throttle rate controller's P term (desired rate to acceleration or motor output)
TUNING_WP_SPEED = 10, // 10, maximum speed to next way point (0 to 10m/s)
TUNING_LOITER_POSITION_KP = 12, // 12, loiter distance controller's P term (position error to speed)
TUNING_HELI_EXTERNAL_GYRO, // 13, TradHeli specific external tail gyro gain
TUNING_ALTITUDE_HOLD_KP, // 14, altitude hold controller's P term (alt error to desired rate)
TUNING_RATE_ROLL_PITCH_KD = 21, // 21, body frame roll/pitch rate controller's D term
TUNING_VEL_XY_KP, // 22, loiter rate controller's P term (speed error to tilt angle)
TUNING_ACRO_RP_KP = 25, // 25, acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_YAW_RATE_KD, // 26, body frame yaw rate controller's D term
TUNING_VEL_XY_KI = 28, // 28, loiter rate controller's I term (speed error to tilt angle)
TUNING_AHRS_YAW_KP = 30, // 30, ahrs's compass effect on yaw angle (0 = very low, 1 = very high)
TUNING_AHRS_KP, // 31, accelerometer effect on roll/pitch angle (0=low)
TUNING_ACCEL_Z_KP = 34, // 34, accel based throttle controller's P term
TUNING_ACCEL_Z_KI, // 35, accel based throttle controller's I term
TUNING_ACCEL_Z_KD, // 36, accel based throttle controller's D term
TUNING_DECLINATION = 38, // 38, compass declination in radians
TUNING_CIRCLE_RATE, // 39, circle turn rate in degrees (hard coded to about 45 degrees in either direction)
TUNING_ACRO_YAW_KP, // 40, acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_SONAR_GAIN, // 41, sonar gain
TUNING_EKF_VERTICAL_POS, // 42, EKF's baro vs accel (higher rely on accels more, baro impact is reduced). Range should be 0.2 ~ 4.0? 2.0 is default
TUNING_EKF_HORIZONTAL_POS, // 43, EKF's gps vs accel (higher rely on accels more, gps impact is reduced). Range should be 1.0 ~ 3.0? 1.5 is default
TUNING_EKF_ACCEL_NOISE, // 44, EKF's accel noise (lower means trust accels more, gps & baro less). Range should be 0.02 ~ 0.5 0.5 is default (but very robust at that level)
TUNING_RC_FEEL_RP, // 45, roll-pitch input smoothing
TUNING_RATE_PITCH_KP, // 46, body frame pitch rate controller's P term
TUNING_RATE_PITCH_KI, // 47, body frame pitch rate controller's I term
TUNING_RATE_PITCH_KD, // 48, body frame pitch rate controller's D term
TUNING_RATE_ROLL_KP, // 49, body frame roll rate controller's P term
TUNING_RATE_ROLL_KI, // 50, body frame roll rate controller's I term
TUNING_RATE_ROLL_KD, // 51, body frame roll rate controller's D term
TUNING_RATE_PITCH_FF, // 52, body frame pitch rate controller FF term
TUNING_RATE_ROLL_FF, // 53, body frame roll rate controller FF term
TUNING_RATE_YAW_FF, // 54, body frame yaw rate controller FF term
TUNING_RATE_MOT_YAW_HEADROOM, // 55, motors yaw headroom minimum
TUNING_RATE_YAW_FILT // 56, yaw rate input filter
};
// Acro Trainer types
#define ACRO_TRAINER_DISABLED 0

74
ArduCopter/tuning.pde
View File

@ -22,173 +22,173 @@ static void tuning() {
switch(g.radio_tuning) {
// Roll, Pitch tuning
case CH6_STABILIZE_ROLL_PITCH_KP:
case TUNING_STABILIZE_ROLL_PITCH_KP:
g.p_stabilize_roll.kP(tuning_value);
g.p_stabilize_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KP:
case TUNING_RATE_ROLL_PITCH_KP:
g.pid_rate_roll.kP(tuning_value);
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KI:
case TUNING_RATE_ROLL_PITCH_KI:
g.pid_rate_roll.kI(tuning_value);
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_ROLL_PITCH_KD:
case TUNING_RATE_ROLL_PITCH_KD:
g.pid_rate_roll.kD(tuning_value);
g.pid_rate_pitch.kD(tuning_value);
break;
// Yaw tuning
case CH6_STABILIZE_YAW_KP:
case TUNING_STABILIZE_YAW_KP:
g.p_stabilize_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KP:
case TUNING_YAW_RATE_KP:
g.pid_rate_yaw.kP(tuning_value);
break;
case CH6_YAW_RATE_KD:
case TUNING_YAW_RATE_KD:
g.pid_rate_yaw.kD(tuning_value);
break;
// Altitude and throttle tuning
case CH6_ALTITUDE_HOLD_KP:
case TUNING_ALTITUDE_HOLD_KP:
g.p_alt_hold.kP(tuning_value);
break;
case CH6_THROTTLE_RATE_KP:
case TUNING_THROTTLE_RATE_KP:
g.p_vel_z.kP(tuning_value);
break;
case CH6_ACCEL_Z_KP:
case TUNING_ACCEL_Z_KP:
g.pid_accel_z.kP(tuning_value);
break;
case CH6_ACCEL_Z_KI:
case TUNING_ACCEL_Z_KI:
g.pid_accel_z.kI(tuning_value);
break;
case CH6_ACCEL_Z_KD:
case TUNING_ACCEL_Z_KD:
g.pid_accel_z.kD(tuning_value);
break;
// Loiter and navigation tuning
case CH6_LOITER_POSITION_KP:
case TUNING_LOITER_POSITION_KP:
g.p_pos_xy.kP(tuning_value);
break;
case CH6_VEL_XY_KP:
case TUNING_VEL_XY_KP:
g.pi_vel_xy.kP(tuning_value);
break;
case CH6_VEL_XY_KI:
case TUNING_VEL_XY_KI:
g.pi_vel_xy.kI(tuning_value);
break;
case CH6_WP_SPEED:
case TUNING_WP_SPEED:
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav.set_speed_xy(g.rc_6.control_in);
break;
// Acro roll pitch gain
case CH6_ACRO_RP_KP:
case TUNING_ACRO_RP_KP:
g.acro_rp_p = tuning_value;
break;
// Acro yaw gain
case CH6_ACRO_YAW_KP:
case TUNING_ACRO_YAW_KP:
g.acro_yaw_p = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO:
case TUNING_HELI_EXTERNAL_GYRO:
motors.ext_gyro_gain(g.rc_6.control_in);
break;
case CH6_RATE_PITCH_FF:
case TUNING_RATE_PITCH_FF:
g.pid_rate_pitch.ff(tuning_value);
break;
case CH6_RATE_ROLL_FF:
case TUNING_RATE_ROLL_FF:
g.pid_rate_roll.ff(tuning_value);
break;
case CH6_RATE_YAW_FF:
case TUNING_RATE_YAW_FF:
g.pid_rate_yaw.ff(tuning_value);
break;
#endif
case CH6_DECLINATION:
case TUNING_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
break;
case CH6_CIRCLE_RATE:
case TUNING_CIRCLE_RATE:
// set circle rate
circle_nav.set_rate(g.rc_6.control_in/25-20); // allow approximately 45 degree turn rate in either direction
break;
case CH6_SONAR_GAIN:
case TUNING_SONAR_GAIN:
// set sonar gain
g.sonar_gain.set(tuning_value);
break;
#if 0
// disabled for now - we need accessor functions
case CH6_EKF_VERTICAL_POS:
case TUNING_EKF_VERTICAL_POS:
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
ahrs.get_NavEKF()._gpsVertPosNoise = tuning_value;
break;
case CH6_EKF_HORIZONTAL_POS:
case TUNING_EKF_HORIZONTAL_POS:
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
ahrs.get_NavEKF()._gpsHorizPosNoise = tuning_value;
break;
case CH6_EKF_ACCEL_NOISE:
case TUNING_EKF_ACCEL_NOISE:
// EKF's accel noise (lower means trust accels more, gps & baro less)
ahrs.get_NavEKF()._accNoise = tuning_value;
break;
#endif
case CH6_RC_FEEL_RP:
case TUNING_RC_FEEL_RP:
// roll-pitch input smoothing
g.rc_feel_rp = g.rc_6.control_in / 10;
break;
case CH6_RATE_PITCH_KP:
case TUNING_RATE_PITCH_KP:
g.pid_rate_pitch.kP(tuning_value);
break;
case CH6_RATE_PITCH_KI:
case TUNING_RATE_PITCH_KI:
g.pid_rate_pitch.kI(tuning_value);
break;
case CH6_RATE_PITCH_KD:
case TUNING_RATE_PITCH_KD:
g.pid_rate_pitch.kD(tuning_value);
break;
case CH6_RATE_ROLL_KP:
case TUNING_RATE_ROLL_KP:
g.pid_rate_roll.kP(tuning_value);
break;
case CH6_RATE_ROLL_KI:
case TUNING_RATE_ROLL_KI:
g.pid_rate_roll.kI(tuning_value);
break;
case CH6_RATE_ROLL_KD:
case TUNING_RATE_ROLL_KD:
g.pid_rate_roll.kD(tuning_value);
break;
case CH6_RATE_MOT_YAW_HEADROOM:
case TUNING_RATE_MOT_YAW_HEADROOM:
motors.set_yaw_headroom(tuning_value*1000);
break;
case CH6_RATE_YAW_FILT:
case TUNING_RATE_YAW_FILT:
g.pid_rate_yaw.filt_hz(tuning_value);
break;
}