Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Acro_Balance update

This commit is contained in:
Leonard Hall 2019-07-27 13:00:18 +09:30 committed by Randy Mackay
parent 4754710e60
commit 8b7fe2ac12
2 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/mode_acro.cpp
View File

@ -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);
}
}

8
ArduCopter/mode_sport.cpp
View File

@ -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