From c2ca5c46b8e4f672c7c4e7d52e5e244a8624e255 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 6 Dec 2013 21:54:50 +0900 Subject: [PATCH] Copter: correct auto_run --- ArduCopter/control_stabilize.pde | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index d373abe7cd..26e8986064 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -61,20 +61,16 @@ static void auto_run() { Vector3f angle_target; - // user input, although ignored is put into control_roll and pitch for reporting purposes - control_roll = g.rc_1.control_in; - control_pitch = g.rc_2.control_in; - // copy latest output from nav controller to stabilize controller - angle_target.x = wp_nav.get_desired_roll(); - angle_target.y = wp_nav.get_desired_pitch(); - - // To-Do: handle yaw - angle_target.z = control_yaw; + control_roll = wp_nav.get_desired_roll(); + control_pitch = wp_nav.get_desired_pitch(); // copy angle targets for reporting purposes - control_roll = angle_target.x; - control_pitch = angle_target.y; + angle_target.x = control_roll; + angle_target.y = control_pitch; + + // To-Do: handle pilot input for yaw and different methods to update yaw (ROI, face next wp) + angle_target.z = control_yaw; // To-Do: shorten below by moving these often used steps into a single function in the AC_AttitudeControl lib