From 49dbdce89ca8a89893ae21d0a2d64c3e05dc1cbf Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 4 Aug 2013 20:22:12 +0900 Subject: [PATCH] Copter: split ACRO_P into ACRO_RP_P and ACRO_YAW_P --- ArduCopter/ArduCopter.pde | 11 ++++++++--- ArduCopter/Attitude.pde | 20 ++++++++++---------- ArduCopter/Parameters.h | 8 +++++--- ArduCopter/Parameters.pde | 17 ++++++++++++----- ArduCopter/config.h | 8 ++++---- ArduCopter/defines.h | 3 ++- 6 files changed, 41 insertions(+), 26 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ec8820d82c..63bf112487 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -2212,9 +2212,14 @@ static void tuning(){ wp_nav.set_horizontal_velocity(g.rc_6.control_in); break; - // Acro and other tuning - case CH6_ACRO_KP: - g.acro_p = tuning_value; + // Acro roll pitch gain + case CH6_ACRO_RP_KP: + g.acro_rp_p = tuning_value; + break; + + // Acro yaw gain + case CH6_ACRO_YAW_KP: + g.acro_yaw_p = tuning_value; break; case CH6_RELAY: diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 5a583f8ac7..9343fbb63d 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -73,7 +73,7 @@ get_stabilize_yaw(int32_t target_angle) static void get_acro_roll(int32_t target_rate) { - target_rate = target_rate * g.acro_p; + target_rate = target_rate * g.acro_rp_p; // set targets for rate controller set_roll_rate_target(target_rate, BODY_FRAME); @@ -82,7 +82,7 @@ get_acro_roll(int32_t target_rate) static void get_acro_pitch(int32_t target_rate) { - target_rate = target_rate * g.acro_p; + target_rate = target_rate * g.acro_rp_p; // set targets for rate controller set_pitch_rate_target(target_rate, BODY_FRAME); @@ -91,7 +91,7 @@ get_acro_pitch(int32_t target_rate) static void get_acro_yaw(int32_t target_rate) { - target_rate = target_rate * g.acro_p; + target_rate = target_rate * g.acro_yaw_p; // set targets for rate controller set_yaw_rate_target(target_rate, BODY_FRAME); @@ -154,7 +154,7 @@ get_roll_rate_stabilized_bf(int32_t stick_angle) static float angle_error = 0; // convert the input to the desired body frame roll rate - int32_t rate_request = stick_angle * g.acro_p; + int32_t rate_request = stick_angle * g.acro_rp_p; if (g.acro_trainer == ACRO_TRAINER_LIMITED) { rate_request += acro_roll_rate; @@ -193,7 +193,7 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle) static float angle_error = 0; // convert the input to the desired body frame pitch rate - int32_t rate_request = stick_angle * g.acro_p; + int32_t rate_request = stick_angle * g.acro_rp_p; if (g.acro_trainer == ACRO_TRAINER_LIMITED) { rate_request += acro_pitch_rate; @@ -232,7 +232,7 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle) static float angle_error = 0; // convert the input to the desired body frame yaw rate - int32_t rate_request = stick_angle * g.acro_p; + int32_t rate_request = stick_angle * g.acro_yaw_p; if (g.acro_trainer == ACRO_TRAINER_LIMITED) { rate_request += acro_yaw_rate; @@ -271,7 +271,7 @@ get_roll_rate_stabilized_ef(int32_t stick_angle) int32_t angle_error = 0; // convert the input to the desired roll rate - int32_t target_rate = stick_angle * g.acro_p - (acro_roll * g.acro_balance_roll); + int32_t target_rate = stick_angle * g.acro_rp_p - (acro_roll * g.acro_balance_roll); // convert the input to the desired roll rate acro_roll += target_rate * G_Dt; @@ -312,7 +312,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle) int32_t angle_error = 0; // convert the input to the desired pitch rate - int32_t target_rate = stick_angle * g.acro_p - (acro_pitch * g.acro_balance_pitch); + int32_t target_rate = stick_angle * g.acro_rp_p - (acro_pitch * g.acro_balance_pitch); // convert the input to the desired pitch rate acro_pitch += target_rate * G_Dt; @@ -354,7 +354,7 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle) int32_t angle_error = 0; // convert the input to the desired yaw rate - int32_t target_rate = stick_angle * g.acro_p; + int32_t target_rate = stick_angle * g.acro_yaw_p; // convert the input to the desired yaw rate nav_yaw += target_rate * G_Dt; @@ -879,7 +879,7 @@ static void get_look_ahead_yaw(int16_t pilot_yaw) nav_yaw = get_yaw_slew(nav_yaw, g_gps->ground_course_cd, AUTO_YAW_SLEW_RATE); get_stabilize_yaw(wrap_360_cd(nav_yaw + pilot_yaw)); // Allow pilot to "skid" around corners up to 45 degrees }else{ - nav_yaw += pilot_yaw * g.acro_p * G_Dt; + nav_yaw += pilot_yaw * g.acro_yaw_p * G_Dt; nav_yaw = wrap_360_cd(nav_yaw); get_stabilize_yaw(nav_yaw); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4a8d942657..b54caf8fae 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -233,7 +233,7 @@ public: // // 220: PI/D Controllers // - k_param_acro_p = 221, + k_param_acro_rp_p = 221, k_param_axis_lock_p, // remove k_param_pid_rate_roll, k_param_pid_rate_pitch, @@ -255,7 +255,8 @@ public: k_param_acro_balance_pitch_old, // 240 - remove k_param_pid_throttle_accel, k_param_acro_balance_roll, - k_param_acro_balance_pitch, // 243 + k_param_acro_balance_pitch, + k_param_acro_yaw_p, // 244 // 254,255: reserved }; @@ -377,7 +378,8 @@ public: AP_Int16 rc_speed; // speed of fast RC Channels in Hz // Acro parameters - AP_Float acro_p; + AP_Float acro_rp_p; + AP_Float acro_yaw_p; AP_Float acro_balance_roll; AP_Float acro_balance_pitch; AP_Int8 acro_trainer; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index d0ff2635c4..8af262050a 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -378,7 +378,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,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 + // @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 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 GSCALAR(radio_tuning, "TUNE", 0), // @Param: TUNE_LOW @@ -514,12 +514,19 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(rc_speed, "RC_SPEED", RC_FAST_SPEED), - // @Param: ACRO_P - // @DisplayName: Acro P gain - // @Description: Used to convert pilot roll, pitch and yaw input into a dssired rate of rotation in ACRO mode. Higher values mean faster rate of rotation. + // @Param: ACRO_RP_P + // @DisplayName: Acro Roll and Pitch P gain + // @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode. Higher values mean faster rate of rotation. // @Range: 1 10 // @User: Standard - GSCALAR(acro_p, "ACRO_P", ACRO_P), + GSCALAR(acro_rp_p, "ACRO_RP_P", ACRO_RP_P), + + // @Param: ACRO_YAW_P + // @DisplayName: Acro Yaw P gain + // @Description: Converts pilot yaw input into a desired rate of rotation in ACRO, Stabilize and SPORT modes. Higher values mean faster rate of rotation. + // @Range: 1 10 + // @User: Standard + GSCALAR(acro_yaw_p, "ACRO_YAW_P", ACRO_YAW_P), // @Param: ACRO_BAL_ROLL // @DisplayName: Acro Balance Roll diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9d00272ee9..4a64eb83ed 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -758,12 +758,12 @@ // // Acro mode gains -#ifndef ACRO_P - # define ACRO_P 4.5f +#ifndef ACRO_RP_P + # define ACRO_RP_P 4.5f #endif -#ifndef AXIS_LOCK_ENABLED - # define AXIS_LOCK_ENABLED ENABLED +#ifndef ACRO_YAW_P + # define ACRO_YAW_P 4.5f #endif // Stabilize (angle controller) gains diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d0a98da97e..276dde24cc 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -164,7 +164,8 @@ #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_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 // 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)