From 3ba1c0c4aa19fc6d69d5a83826d9bcea781e22a2 Mon Sep 17 00:00:00 2001 From: lthall Date: Sat, 2 Aug 2014 18:27:42 +0930 Subject: [PATCH] Copter: ACRO add expo adds 30% expo to roll and pitch --- ArduCopter/control_acro.pde | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index f043d3884a..4e8f2b804b 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -44,14 +44,28 @@ static void acro_run() // returns desired angle rates in centi-degrees-per-second static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) { - - // Calculate trainer mode earth frame rate command for roll + // expo variables + static float _expo_factor = 0.3; + float rp_in, rp_in3, rp_out; + float rate_limit; Vector3f rate_ef_level, rate_bf_level, rate_bf_request; + // range check the input + roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); + pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX); + // calculate rate requests - rate_bf_request.x = roll_in * g.acro_rp_p; - rate_bf_request.y = pitch_in * g.acro_rp_p; + rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; + rp_in3 = rp_in*rp_in*rp_in; + rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in); + rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + + rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX; + rp_in3 = rp_in*rp_in*rp_in; + rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in); + rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + rate_bf_request.z = yaw_in * g.acro_yaw_p; // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode