Arducopter: Yaw stop fix

This commit is contained in:
Jason Short 2012-07-14 12:23:10 -07:00
parent a469682b7b
commit e3af0c7920

View File

@ -1511,13 +1511,12 @@ void update_yaw_mode(void)
g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in);
yaw_stopped = false; yaw_stopped = false;
yaw_timer = 150; yaw_timer = 150;
}else if (!yaw_stopped){ }else if (!yaw_stopped){
g.rc_4.servo_out = get_acro_yaw(0); g.rc_4.servo_out = get_acro_yaw(0);
yaw_timer--; yaw_timer--;
if ( abs(omega.z * DEGX100) < 1000 ){
yaw_stopped = true; if((yaw_timer == 0) || (fabs(omega.z) < .17)){
}
if(yaw_timer == 0){
yaw_stopped = true; yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor; nav_yaw = ahrs.yaw_sensor;
} }