/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include "Sub.h" /* * control_acro.pde - init and run calls for acro flight mode */ // acro_init - initialise acro controller bool Sub::acro_init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && (g.rc_3.control_in > get_non_takeoff_throttle())) { return false; } // set target altitude to zero for reporting pos_control.set_alt_target(0); return true; } // acro_run - runs the acro controller // should be called at 100hz or more void Sub::acro_run() { float target_roll, target_pitch, target_yaw; int16_t pilot_throttle_scaled; // if motors not running reset angle targets if(!motors.armed()) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); // slow start if landed if (ap.land_complete) { motors.slow_start(true); } return; } // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); // run attitude controller attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); // output pilot's throttle without angle boost attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); //control_in is range 0-1000 //radio_in is raw pwm value motors.set_forward(channel_forward->radio_in); motors.set_strafe(channel_strafe->radio_in); } // 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 void Sub::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) { float rate_limit; Vector3f rate_ef_level, rate_bf_level, rate_bf_request; // apply circular limit to pitch and roll inputs float total_in = pythagorous2((float)pitch_in, (float)roll_in); if (total_in > ROLL_PITCH_INPUT_MAX) { float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; roll_in *= ratio; pitch_in *= ratio; } // calculate roll, pitch rate requests if (g.acro_expo <= 0) { rate_bf_request.x = roll_in * g.acro_rp_p; rate_bf_request.y = pitch_in * g.acro_rp_p; } else { // expo variables float rp_in, rp_in3, rp_out; // range check expo if (g.acro_expo > 1.0f) { g.acro_expo = 1.0f; } // roll expo rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; rp_in3 = rp_in*rp_in*rp_in; rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; // pitch expo rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX; rp_in3 = rp_in*rp_in*rp_in; rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in); rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; } // calculate yaw rate request 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 if (g.acro_trainer != ACRO_TRAINER_DISABLED) { // Calculate trainer mode earth frame rate command for roll int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; // Calculate trainer mode earth frame rate command for pitch int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; // Calculate trainer mode earth frame rate command for yaw rate_ef_level.z = 0; // Calculate angle limiting earth frame rate commands if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (roll_angle > aparm.angle_max){ rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max); }else if (roll_angle < -aparm.angle_max) { rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max); } if (pitch_angle > aparm.angle_max){ rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max); }else if (pitch_angle < -aparm.angle_max) { rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max); } } // convert earth-frame level rates to body-frame level rates attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); // combine earth frame rate corrections with rate requests if (g.acro_trainer == ACRO_TRAINER_LIMITED) { rate_bf_request.x += rate_bf_level.x; rate_bf_request.y += rate_bf_level.y; rate_bf_request.z += rate_bf_level.z; }else{ float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); // Scale leveling rates by stick input rate_bf_level = rate_bf_level*acro_level_mix; // Calculate rate limit to prevent change of rate through inverted rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x)); rate_bf_request.x += rate_bf_level.x; rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); // Calculate rate limit to prevent change of rate through inverted rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y)); rate_bf_request.y += rate_bf_level.y; rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); // Calculate rate limit to prevent change of rate through inverted rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z)); rate_bf_request.z += rate_bf_level.z; rate_bf_request.z = constrain_float(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; }