diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 91559e372773f8b0cef60391245fc26ec9a2c11f..bae2e38655f7b613cf8bba9adee0417675957080 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1689,10 +1689,6 @@ void update_roll_pitch_mode(void) // apply SIMPLE mode transform update_simple_mode(); - // copy user input for reporting purposes - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; - if(failsafe.radio) { // don't allow loiter target to move during failsafe wp_nav.move_loiter_target(0.0f, 0.0f, 0.01f); @@ -1701,6 +1697,10 @@ void update_roll_pitch_mode(void) wp_nav.move_loiter_target(g.rc_1.control_in, g.rc_2.control_in, 0.01f); } + // copy latest output from nav controller to stabilize controller + control_roll = wp_nav.get_desired_roll(); + control_pitch = wp_nav.get_desired_pitch(); + get_stabilize_roll(control_roll); get_stabilize_pitch(control_pitch); break;