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

Copter: reset yaw angle only when motors not spinning

This commit is contained in:
Randy Mackay 2013-06-03 17:50:45 +09:00
parent 537d0032a8
commit 40f5226518
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/Attitude.pde
View File

@ -122,15 +122,14 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
angle_error = constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT); angle_error = constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
} }
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) { // reset target angle to current angle if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0; angle_error = 0;
} }
// update roll_axis to be within max_angle_overshoot of our current heading // update roll_axis to be within max_angle_overshoot of our current heading
roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor); roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor);
// set earth frame targets for rate controller
// set earth frame targets for rate controller // set earth frame targets for rate controller
set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME); set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME);
} }
@ -158,7 +157,8 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
angle_error = constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT); angle_error = constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
} }
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) { // reset target angle to current angle if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0; angle_error = 0;
} }
@ -189,7 +189,8 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
// limit the maximum overshoot // limit the maximum overshoot
angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT); angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) { // reset target angle to current heading if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0; angle_error = 0;
} }