From 8b7fe2ac12ca6ceb49c8b6ea30ffd8474189c8e8 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 27 Jul 2019 13:00:18 +0930 Subject: [PATCH] Copter: Acro_Balance update --- ArduCopter/mode_acro.cpp | 8 ++++---- ArduCopter/mode_sport.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index 1620c78833..dcfa838839 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -124,15 +124,15 @@ void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, if (g.acro_trainer == ACRO_TRAINER_LIMITED) { const float angle_max = copter.aparm.angle_max; if (roll_angle > angle_max){ - rate_ef_level.x -= g.acro_balance_roll*(roll_angle-angle_max); + rate_ef_level.x += AC_AttitudeControl::sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); }else if (roll_angle < -angle_max) { - rate_ef_level.x -= g.acro_balance_roll*(roll_angle+angle_max); + rate_ef_level.x += AC_AttitudeControl::sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); } if (pitch_angle > angle_max){ - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-angle_max); + rate_ef_level.y += AC_AttitudeControl::sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); }else if (pitch_angle < -angle_max) { - rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+angle_max); + rate_ef_level.y += AC_AttitudeControl::sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); } } diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 462402f004..588f8c50ad 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -54,15 +54,15 @@ void ModeSport::run() const float angle_max = copter.aparm.angle_max; if (roll_angle > angle_max){ - target_roll_rate -= g.acro_rp_p*(roll_angle-angle_max); + target_roll_rate += AC_AttitudeControl::sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); }else if (roll_angle < -angle_max) { - target_roll_rate -= g.acro_rp_p*(roll_angle+angle_max); + target_roll_rate += AC_AttitudeControl::sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max(), G_Dt); } if (pitch_angle > angle_max){ - target_pitch_rate -= g.acro_rp_p*(pitch_angle-angle_max); + target_pitch_rate += AC_AttitudeControl::sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); }else if (pitch_angle < -angle_max) { - target_pitch_rate -= g.acro_rp_p*(pitch_angle+angle_max); + target_pitch_rate += AC_AttitudeControl::sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max(), G_Dt); } // get pilot's desired yaw rate