aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: bug fix to sport roll pitch control

Browse Source
This commit is contained in:
Randy Mackay 2014-05-07 17:25:46 +09:00
parent 0af9d502d9
commit 197d83e32a
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/control_sport.pde
View File

@ -39,15 +39,14 @@ static void sport_run()
// 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;
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;
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) {
@ -80,6 +79,7 @@ static void sport_run()
// move throttle to minimum to keep us on the ground
attitude_control.set_throttle_out(0, false);
}else{
// call attitude controller
attitude_control.rate_ef_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);