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

ArduCopter: bug fix for get_yaw_rate_stabilized_ef 

Provided by Leonard Hall
This commit is contained in:
rmackay9 2012-10-17 10:18:24 +09:00
parent d52a8b146b
commit 7787bb4fd5
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Attitude.pde
View File

@ -157,22 +157,22 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
// convert the input to the desired yaw rate // convert the input to the desired yaw rate
int32_t target_rate = stick_angle * g.acro_p; int32_t target_rate = stick_angle * g.acro_p;
// convert the input to the desired yaw rate // update current target heading using pilot's desired rate of turn
nav_yaw += target_rate * G_Dt; nav_yaw += target_rate * G_Dt;
nav_yaw = wrap_360(nav_yaw); nav_yaw = wrap_360(nav_yaw);
// angle error with maximum of +- max_angle_overshoot // calculate difference between desired heading and current heading
angle_error = wrap_180(nav_yaw - ahrs.yaw_sensor); angle_error = wrap_180(nav_yaw - ahrs.yaw_sensor);
// this limits the maximum overshoot // limit the maximum overshoot
angle_error = constrain(angle_error, -MAX_ANGLE_OVERSHOOT, MAX_ANGLE_OVERSHOOT); angle_error = constrain(angle_error, -MAX_ANGLE_OVERSHOOT, MAX_ANGLE_OVERSHOOT);
if (motors.armed() == false || ((g.rc_3.control_in == 0) && !failsafe)) { if (motors.armed() == false || ((g.rc_3.control_in == 0) && !failsafe)) {
angle_error = 0; angle_error = 0;
} }
// update nav_yaw to be within max_angle_overshoot of our current heading // set nav_yaw to our desired heading
nav_yaw = angle_error + ahrs.yaw_sensor; nav_yaw = wrap_360(angle_error + ahrs.yaw_sensor);
// set earth frame targets for rate controller // set earth frame targets for rate controller
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME); set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);