Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC Acro and Sport: Angle limit code update

Browse Source
This commit is contained in:
lthall 2014-02-12 23:25:13 +10:30 committed by Andrew Tridgell
parent 65c2fc0cc6
commit 41dcfae7c0
2 changed files with 28 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/control_acro.pde
View File

@ -72,15 +72,15 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
// 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.pi_stabilize_roll.get_p(roll_angle-aparm.angle_max);
rate_ef_level.x -= g.acro_rp_p*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x -= g.pi_stabilize_roll.get_p(roll_angle+aparm.angle_max);
rate_ef_level.x -= g.acro_rp_p*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
rate_ef_level.y -= g.pi_stabilize_pitch.get_p(pitch_angle-aparm.angle_max);
rate_ef_level.y -= g.acro_rp_p*(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y -= g.pi_stabilize_pitch.get_p(pitch_angle+aparm.angle_max);
rate_ef_level.y -= g.acro_rp_p*(pitch_angle+aparm.angle_max);
}
}

26
ArduCopter/control_sport.pde
View File

@ -28,8 +28,30 @@ static void sport_run()
update_simple_mode();
// get pilot's desired roll and pitch rates
target_roll_rate = g.rc_1.control_in;
target_pitch_rate = g.rc_2.control_in;
// calculate rate requests
target_roll_rate = g.rc_1.control_in * g.acro_rp_p;
target_pitch_rate = g.rc_2.control_in * g.acro_rp_p;
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
target_roll_rate = -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);
target_pitch_rate = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
if (roll_angle > aparm.angle_max){
target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max);
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);