Copter: bug fix for simple_mode when run at 400hz

Mark radio frame as consumed so that simple mode does not apply the yaw
correction more than once when run at very high rates (like 400hz on
Pixhawk)
This commit is contained in:
Randy Mackay 2014-03-28 17:03:31 +09:00
parent 176a450ad2
commit ac364c93dd

View File

@ -1312,6 +1312,9 @@ void update_simple_mode(void)
return;
}
// mark radio frame as consumed
ap.new_radio_frame = false;
if (ap.simple_mode == 1) {
// rotate roll, pitch input by -initial simple heading (i.e. north facing)
rollx = g.rc_1.control_in*simple_cos_yaw - g.rc_2.control_in*simple_sin_yaw;