diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index f54d753d22..5a583f8ac7 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -101,11 +101,19 @@ get_acro_yaw(int32_t target_rate) static void get_acro_level_rates() { + // zero earth frame leveling if trainer is disabled + if (g.acro_trainer == ACRO_TRAINER_DISABLED) { + set_roll_rate_target(0, BODY_EARTH_FRAME); + set_pitch_rate_target(0, BODY_EARTH_FRAME); + set_yaw_rate_target(0, BODY_EARTH_FRAME); + return; + } + // Calculate trainer mode earth frame rate command for roll int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); int32_t target_rate = 0; - if (g.acro_trainer_enabled) { + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (roll_angle > 4500){ target_rate = g.pi_stabilize_roll.get_p(4500-roll_angle); }else if (roll_angle < -4500) { @@ -113,7 +121,7 @@ get_acro_level_rates() } } roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); - target_rate -= (roll_angle * g.acro_balance_roll)/100; + target_rate -= roll_angle * g.acro_balance_roll; // add earth frame targets for roll rate controller set_roll_rate_target(target_rate, BODY_EARTH_FRAME); @@ -122,7 +130,7 @@ get_acro_level_rates() int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); target_rate = 0; - if (g.acro_trainer_enabled) { + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (pitch_angle > 4500){ target_rate = g.pi_stabilize_pitch.get_p(4500-pitch_angle); }else if (pitch_angle < -4500) { @@ -130,7 +138,7 @@ get_acro_level_rates() } } pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); - target_rate -= (pitch_angle * g.acro_balance_pitch)/100; + target_rate -= pitch_angle * g.acro_balance_pitch; // add earth frame targets for pitch rate controller set_pitch_rate_target(target_rate, BODY_EARTH_FRAME); @@ -148,7 +156,9 @@ get_roll_rate_stabilized_bf(int32_t stick_angle) // convert the input to the desired body frame roll rate int32_t rate_request = stick_angle * g.acro_p; - if (!g.acro_trainer_enabled) { + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_request += acro_roll_rate; + }else{ // Scale pitch leveling by stick input acro_roll_rate = (float)acro_roll_rate*acro_level_mix; @@ -157,8 +167,6 @@ get_roll_rate_stabilized_bf(int32_t stick_angle) rate_request += acro_roll_rate; rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); - } else { - rate_request += acro_roll_rate; } // add automatic correction @@ -187,7 +195,9 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle) // convert the input to the desired body frame pitch rate int32_t rate_request = stick_angle * g.acro_p; - if (!g.acro_trainer_enabled) { + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_request += acro_pitch_rate; + }else{ // Scale pitch leveling by stick input acro_pitch_rate = (float)acro_pitch_rate*acro_level_mix; @@ -196,8 +206,6 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle) rate_request += acro_pitch_rate; rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); - } else { - rate_request += acro_pitch_rate; } // add automatic correction @@ -226,7 +234,9 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle) // convert the input to the desired body frame yaw rate int32_t rate_request = stick_angle * g.acro_p; - if (!g.acro_trainer_enabled) { + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_request += acro_yaw_rate; + }else{ // Scale pitch leveling by stick input acro_yaw_rate = (float)acro_yaw_rate*acro_level_mix; @@ -235,8 +245,6 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle) rate_request += acro_yaw_rate; rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); - } else { - rate_request += acro_yaw_rate; } // add automatic correction @@ -263,14 +271,14 @@ 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)/100; + int32_t target_rate = stick_angle * g.acro_p - (acro_roll * g.acro_balance_roll); // convert the input to the desired roll rate acro_roll += target_rate * G_Dt; acro_roll = wrap_180_cd(acro_roll); // ensure that we don't reach gimbal lock - if (labs(acro_roll) > 4500 && g.acro_trainer_enabled) { + if (labs(acro_roll) > 4500 && g.acro_trainer == ACRO_TRAINER_LIMITED) { acro_roll = constrain_int32(acro_roll, -4500, 4500); angle_error = wrap_180_cd(acro_roll - ahrs.roll_sensor); } else { @@ -304,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)/100; + int32_t target_rate = stick_angle * g.acro_p - (acro_pitch * g.acro_balance_pitch); // convert the input to the desired pitch rate acro_pitch += target_rate * G_Dt; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 7bc1929bb8..ab01345823 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -82,7 +82,7 @@ public: k_param_rssi_pin, k_param_throttle_accel_enabled, // deprecated - remove k_param_wp_yaw_behavior, - k_param_acro_trainer_enabled, + k_param_acro_trainer, k_param_pilot_velocity_z_max, k_param_circle_rate, k_param_sonar_gain, @@ -251,9 +251,11 @@ public: k_param_pid_throttle_rate, k_param_pid_optflow_roll, k_param_pid_optflow_pitch, - k_param_acro_balance_roll, // scalar (not PID) - k_param_acro_balance_pitch, // scalar (not PID) - k_param_pid_throttle_accel, // 241 + k_param_acro_balance_roll_old, // 239 - remove + k_param_acro_balance_pitch_old, // 240 - remove + k_param_pid_throttle_accel, + k_param_acro_balance_roll, + k_param_acro_balance_pitch, // 243 // 254,255: reserved }; @@ -377,9 +379,9 @@ public: // Acro parameters AP_Float acro_p; - AP_Int16 acro_balance_roll; - AP_Int16 acro_balance_pitch; - AP_Int8 acro_trainer_enabled; + AP_Float acro_balance_roll; + AP_Float acro_balance_pitch; + AP_Int8 acro_trainer; // PI/D controllers AC_PID pid_rate_roll; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index b4e76d7af1..60eecf6b12 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -531,25 +531,25 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: ACRO_BAL_ROLL // @DisplayName: Acro Balance Roll // @Description: rate at which roll angle returns to level in acro mode - // @Range: 0 300 - // @Increment: 1 + // @Range: 0 3 + // @Increment: 0.1 // @User: Advanced GSCALAR(acro_balance_roll, "ACRO_BAL_ROLL", ACRO_BALANCE_ROLL), // @Param: ACRO_BAL_PITCH // @DisplayName: Acro Balance Pitch // @Description: rate at which pitch angle returns to level in acro mode - // @Range: 0 300 - // @Increment: 1 + // @Range: 0 3 + // @Increment: 0.1 // @User: Advanced GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH), // @Param: ACRO_TRAINER - // @DisplayName: Acro Trainer Enabled - // @Description: Set to 1 (Enabled) to make roll return to within 45 degrees of level automatically - // @Values: 0:Disabled,1:Enabled + // @DisplayName: Acro Trainer + // @Description: Type of trainer used in acro mode + // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited // @User: Advanced - GSCALAR(acro_trainer_enabled, "ACRO_TRAINER", ACRO_TRAINER_ENABLED), + GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED), // @Param: LED_MODE // @DisplayName: Copter LED Mode diff --git a/ArduCopter/config.h b/ArduCopter/config.h index bb790a70e0..9d00272ee9 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -869,15 +869,11 @@ #endif #ifndef ACRO_BALANCE_ROLL - #define ACRO_BALANCE_ROLL 200 + #define ACRO_BALANCE_ROLL 1.0f #endif #ifndef ACRO_BALANCE_PITCH - #define ACRO_BALANCE_PITCH 200 -#endif - -#ifndef ACRO_TRAINER_ENABLED - #define ACRO_TRAINER_ENABLED ENABLED + #define ACRO_BALANCE_PITCH 1.0f #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index ebe834fdbf..a4f9411310 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -175,6 +175,10 @@ #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) +// Acro Trainer types +#define ACRO_TRAINER_DISABLED 0 +#define ACRO_TRAINER_LEVELING 1 +#define ACRO_TRAINER_LIMITED 2 // Commands - Note that APM now uses a subset of the MAVLink protocol // commands. See enum MAV_CMD in the GCS_Mavlink library