From 32b8f80510014909e33781682c26ccfface2e6ed Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 25 Jan 2014 12:00:07 +0900 Subject: [PATCH] Copter: Acro flight mode from Leonard --- ArduCopter/control_acro.pde | 140 +++++++++++++++++++++++++++++++ ArduCopter/control_stabilize.pde | 42 +--------- 2 files changed, 141 insertions(+), 41 deletions(-) create mode 100644 ArduCopter/control_acro.pde diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde new file mode 100644 index 0000000000..3af24beb33 --- /dev/null +++ b/ArduCopter/control_acro.pde @@ -0,0 +1,140 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + * control_stabilize.pde - init and run calls for acro flight mode + */ + +// acro_init - initialise acro controller +static bool acro_init(bool ignore_checks) +{ + return true; +} + +// acro_run - runs the acro controller +// should be called at 100hz or more +static void acro_run() +{ + int16_t target_roll, target_pitch, target_yaw; + Vector3f rate_target; // for roll, pitch, yaw body-frame rate targets + + // convert the input to the desired body frame rate + get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw); + + attitude_control.ratebf_rpy(target_roll, target_pitch, target_yaw); + + // To-Do: handle helicopter + + // pilot controlled yaw using rate controller + //get_yaw_rate_stabilized_bf(pilot_yaw); + + // call get_acro_level_rates() here? + + // To-Do: convert body-frame stabilized angles to earth frame angles and update controll_roll, pitch and yaw? + + // body-frame rate controller is run directly from 100hz loop +} + + +// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates +// 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, int16_t &roll_out, int16_t &pitch_out, int16_t &yaw_out) +{ + + // Calculate trainer mode earth frame rate command for roll + int32_t target_rate; + Vector3f rate_ef_level, rate_bf_level, rate_bf_request; + + // calculate rate requests + rate_bf_request.x = roll_in * g.acro_rp_p; + rate_bf_request.y = pitch_in * g.acro_rp_p; + rate_bf_request.z = yaw_in * g.acro_yaw_p; + // todo: add acceleration slew + + // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode + + acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*cos_pitch_x; + + // Calculate trainer mode earth frame rate command for roll + int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); + target_rate = 0; + + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + if (roll_angle > aparm.angle_max){ + target_rate = g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle); + }else if (roll_angle < -aparm.angle_max) { + target_rate = g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle); + } + } + roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); + rate_ef_level.x -= roll_angle * g.acro_balance_roll; + + // Calculate trainer mode earth frame rate command for pitch + int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); + target_rate = 0; + + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + if (pitch_angle > aparm.angle_max){ + target_rate = g.pi_stabilize_pitch.get_p(aparm.angle_max-pitch_angle); + }else if (pitch_angle < -aparm.angle_max) { + target_rate = g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle); + } + } + pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); + rate_ef_level.y -= pitch_angle * g.acro_balance_pitch; + + // Calculate trainer mode earth frame rate command for yaw + rate_ef_level.z = 0; + + // convert earth-frame level rates to body-frame level rates + attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level); + + + // combine earth frame rate corrections with rate requests + + // combine bf roll leveling with requested bf roll rate + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_bf_request.x += rate_bf_level.x; + }else{ + // Scale pitch leveling by stick input + rate_bf_level.x = (float)rate_bf_level.x*acro_level_mix; + + // Calculate rate limit to prevent change of rate through inverted + int32_t rate_limit = labs(labs(rate_bf_request.x)-labs(rate_bf_level.x)); + + rate_bf_request.x += acro_roll_rate; + rate_bf_request.x = constrain_int32(rate_bf_request.x, -rate_limit, rate_limit); + } + + // combine bf pitch leveling with requested bf pitch rate + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_bf_request.y += rate_bf_level.y; + }else{ + // Scale pitch leveling by stick input + rate_bf_level.y = (float)rate_bf_level.y*acro_level_mix; + + // Calculate rate limit to prevent change of rate through inverted + int32_t rate_limit = labs(labs(rate_bf_request.y)-labs(rate_bf_level.y)); + + rate_bf_request.y += acro_roll_rate; + rate_bf_request.y = constrain_int32(rate_bf_request.y, -rate_limit, rate_limit); + } + + // combine bf yaw leveling with requested bf yaw rate + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_bf_request.z += rate_bf_level.z; + }else{ + // Scale pitch leveling by stick input + rate_bf_level.z = (float)rate_bf_level.z*acro_level_mix; + + // Calculate rate limit to prevent change of rate through inverted + int32_t rate_limit = labs(labs(rate_bf_request.z)-labs(rate_bf_level.z)); + + rate_bf_request.z += acro_roll_rate; + rate_bf_request.z = constrain_int32(rate_bf_request.z, -rate_limit, rate_limit); + } + + // hand back rate request + roll_out = rate_bf_request.x; + pitch_out = rate_bf_request.y; + yaw_out = rate_bf_request.z; +} diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index dd8e8c05ac..2ff825e385 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -1,49 +1,9 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* - * control_stabilize.pde - init and run calls for acro, stabilize, althold, drift and sport flight modes + * control_stabilize.pde - init and run calls for stabilize, althold, drift and sport flight modes */ -// acro_init - initialise acro controller -static bool acro_init(bool ignore_checks) -{ - return true; -} - -// acro_run - runs the acro controller -// should be called at 100hz or more -static void acro_run() -{ - Vector3f rate_target; // for roll, pitch, yaw body-frame rate targets - - // convert the input to the desired body frame rate - rate_target.x = g.rc_1.control_in * g.acro_rp_p; - rate_target.y = g.rc_2.control_in * g.acro_rp_p; - rate_target.z = g.rc_4.control_in * g.acro_yaw_p; - - // To-Do: handle acro trainer here? - - // To-Do: handle helicopter - - acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x; - - // set targets for body frame rate controller - attitude_control.rate_stab_bf_targets(rate_target); - - // convert stabilize rates to regular rates - attitude_control.rate_stab_bf_to_rate_bf_roll(); - attitude_control.rate_stab_bf_to_rate_bf_pitch(); - attitude_control.rate_stab_bf_to_rate_bf_yaw(); - // pilot controlled yaw using rate controller - //get_yaw_rate_stabilized_bf(pilot_yaw); - - // call get_acro_level_rates() here? - - // To-Do: convert body-frame stabilized angles to earth frame angles and update controll_roll, pitch and yaw? - - // body-frame rate controller is run directly from 100hz loop -} - // stabilize_init - initialise stabilize controller static bool stabilize_init(bool ignore_checks) {