From 2c1ec9d0c079caf5ef6b2be0a958d4bd8b9b925c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 3 Feb 2014 22:04:35 +0900
Subject: [PATCH] Copter: remove control_roll, pitch, yaw from control files

---
 ArduCopter/control_althold.pde        |  6 -----
 ArduCopter/control_auto.pde           | 32 ++++++---------------------
 ArduCopter/control_autotune.pde       |  6 -----
 ArduCopter/control_circle.pde         |  6 -----
 ArduCopter/control_drift.pde          |  6 -----
 ArduCopter/control_guided.pde         |  6 -----
 ArduCopter/control_land.pde           | 12 ----------
 ArduCopter/control_loiter.pde         |  6 -----
 ArduCopter/control_ofloiter.pde       |  6 -----
 ArduCopter/control_rtl.pde            | 18 ---------------
 ArduCopter/control_sport.pde          |  6 -----
 ArduCopter/control_stabilize.pde      |  6 -----
 ArduCopter/heli_control_stabilize.pde |  6 -----
 13 files changed, 7 insertions(+), 115 deletions(-)

diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde
index b1b9beb084..265758c563 100644
--- a/ArduCopter/control_althold.pde
+++ b/ArduCopter/control_althold.pde
@@ -69,10 +69,4 @@ static void althold_run()
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
-
-    // refetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde
index dc6315d3e9..d54a1b8059 100644
--- a/ArduCopter/control_auto.pde
+++ b/ArduCopter/control_auto.pde
@@ -71,6 +71,9 @@ static void auto_takeoff_start(float final_alt)
 
     // initialise yaw
     set_auto_yaw_mode(AUTO_YAW_HOLD);
+
+    // tell motors to do a slow start
+    motors.slow_start(true);
 }
 
 // auto_takeoff_run - takeoff in auto mode
@@ -82,6 +85,8 @@ static void auto_takeoff_run()
         // reset attitude control targets
         attitude_control.init_targets();
         attitude_control.set_throttle_out(0, false);
+        // tell motors to do a slow start
+        motors.slow_start(true);
         // To-Do: re-initialise wpnav targets
         return;
     }
@@ -101,12 +106,6 @@ static void auto_takeoff_run()
 
     // roll & pitch from waypoint controller, yaw rate from pilot
     attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
-
-    // refetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
@@ -129,9 +128,10 @@ static void auto_wp_run()
     if(!ap.auto_armed) {
         // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
         //    (of course it would be better if people just used take-off)
-        // To-Do: set motors to slow start
         attitude_control.init_targets();
         attitude_control.set_throttle_out(0, false);
+        // tell motors to do a slow start
+        motors.slow_start(true);
         return;
     }
 
@@ -159,12 +159,6 @@ static void auto_wp_run()
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
         attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
     }
-
-    // refetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // auto_land_start - initialises controller to implement a landing
@@ -222,12 +216,6 @@ static void auto_land_run()
 
     // roll & pitch from waypoint controller, yaw rate from pilot
     attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
-
-    // refetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // auto_rtl_start - initialises RTL in AUTO flight mode
@@ -268,12 +256,6 @@ void auto_circle_run()
 
     // roll & pitch from waypoint controller, yaw rate from pilot
     attitude_control.angleef_rpy(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true);
-
-    // refetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde
index 56a4fa8c3a..530aff4560 100644
--- a/ArduCopter/control_autotune.pde
+++ b/ArduCopter/control_autotune.pde
@@ -271,12 +271,6 @@ static void autotune_run()
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
-
-    // refetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // autotune_attitude_controller - sets attitude control targets during tuning
diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde
index a296352075..3fb70ca99d 100644
--- a/ArduCopter/control_circle.pde
+++ b/ArduCopter/control_circle.pde
@@ -69,10 +69,4 @@ static void circle_run()
     // update altitude target and call position controller
     pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
     pos_control.update_z_controller();
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde
index e8b2bb87bc..ccf6c0d705 100644
--- a/ArduCopter/control_drift.pde
+++ b/ArduCopter/control_drift.pde
@@ -66,10 +66,4 @@ static void drift_run()
 
     // call attitude controller
     attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate);
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde
index 064565b827..56eefe8c97 100644
--- a/ArduCopter/control_guided.pde
+++ b/ArduCopter/control_guided.pde
@@ -88,10 +88,4 @@ static void guided_run()
         // roll, pitch from waypoint controller, yaw heading from auto_heading()
         attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true);
     }
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde
index c19687e822..d6de4a89ca 100644
--- a/ArduCopter/control_land.pde
+++ b/ArduCopter/control_land.pde
@@ -84,12 +84,6 @@ static void land_gps_run()
     // update altitude target and call position controller
     pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt);
     pos_control.update_z_controller();
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // land_nogps_run - runs the land controller
@@ -136,12 +130,6 @@ static void land_nogps_run()
     // call position controller
     pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt);
     pos_control.update_z_controller();
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // get_throttle_land - high level landing logic
diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde
index daf4c552b6..a1a977328e 100644
--- a/ArduCopter/control_loiter.pde
+++ b/ArduCopter/control_loiter.pde
@@ -81,10 +81,4 @@ static void loiter_run()
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
-
-    // refetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde
index 87dc103653..2b3c3076c1 100644
--- a/ArduCopter/control_ofloiter.pde
+++ b/ArduCopter/control_ofloiter.pde
@@ -80,12 +80,6 @@ static void ofloiter_run()
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 
diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde
index f68ea4664a..65b5248701 100644
--- a/ArduCopter/control_rtl.pde
+++ b/ArduCopter/control_rtl.pde
@@ -161,12 +161,6 @@ static void rtl_climb_return_descent_run()
 
     // check if we've completed this stage of RTL
     rtl_state_complete = wp_nav.reached_wp_destination();
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // rtl_return_start - initialise return to home
@@ -225,12 +219,6 @@ static void rtl_loiterathome_run()
     // check if we've completed this stage of RTL
     // To-Do: add extra check that we've reached the target yaw
     rtl_state_complete = ((millis() - rtl_loiter_start_time) > (uint32_t)g.rtl_loiter_time.get());
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 // rtl_loiterathome_start - initialise controllers to loiter over home
@@ -282,11 +270,5 @@ static void rtl_land_run()
 
     // check if we've completed this stage of RTL
     rtl_state_complete = ap.land_complete;
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde
index fafb5b813b..f30cdba823 100644
--- a/ArduCopter/control_sport.pde
+++ b/ArduCopter/control_sport.pde
@@ -64,10 +64,4 @@ static void sport_run()
         pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
         pos_control.update_z_controller();
     }
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde
index c3b359142e..cfa5ba1034 100644
--- a/ArduCopter/control_stabilize.pde
+++ b/ArduCopter/control_stabilize.pde
@@ -55,10 +55,4 @@ static void stabilize_run()
 
     // output pilot's throttle
     attitude_control.set_throttle_out(pilot_throttle_scaled, true);
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
diff --git a/ArduCopter/heli_control_stabilize.pde b/ArduCopter/heli_control_stabilize.pde
index c89d81fbf9..12e83d3082 100644
--- a/ArduCopter/heli_control_stabilize.pde
+++ b/ArduCopter/heli_control_stabilize.pde
@@ -41,12 +41,6 @@ static void heli_stabilize_run()
 
     // output pilot's throttle - note that TradHeli does not used angle-boost
     attitude_control.set_throttle_out(pilot_throttle_scaled, false);
-
-    // re-fetch angle targets for reporting
-    const Vector3f angle_target = attitude_control.angle_ef_targets();
-    control_roll = angle_target.x;
-    control_pitch = angle_target.y;
-    control_yaw = angle_target.z;
 }
 
 #endif  //HELI_FRAME