From 4b5167e529138be41091799e456ab3ded71e86a0 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Sun, 11 May 2014 12:21:27 -0400
Subject: [PATCH] TradHeli: Change behavior of Acro and Stabilize modes when
 disarmed.  Servos should move realistically for testing when disarmed.

---
 ArduCopter/heli_control_acro.pde      | 21 ++++++++++++++++-----
 ArduCopter/heli_control_stabilize.pde | 19 +++++++++++++++++--
 2 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde
index 77a2457d0f..5c658ff576 100644
--- a/ArduCopter/heli_control_acro.pde
+++ b/ArduCopter/heli_control_acro.pde
@@ -16,14 +16,25 @@ static bool heli_acro_init(bool ignore_checks)
 static void heli_acro_run()
 {
     float target_roll, target_pitch, target_yaw;
-
-    // if not armed or main rotor not up to full speed clear stabilized rate errors
-    // unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
-    if(!motors.armed() || !motors.motor_runup_complete()) {
-        attitude_control.relax_bf_rate_controller();
+    static bool init_targets_on_arming;
+    
+    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
+    // we may be in autorotation flight.  These should be reset only when transitioning from disarmed
+    // to armed, because the pilot will have placed the helicopter down on the landing pad.  This is so
+    // that the servos move in a realistic fashion while disarmed for operational checks.
+    // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
+    
+    if(!motors.armed()) {
+        init_targets_on_arming=true;
         attitude_control.set_yaw_target_to_current_heading();
     }
 
+    if(motors.armed() && init_targets_on_arming) {
+        init_targets_on_arming=false;
+        attitude_control.relax_bf_rate_controller();
+        
+    }   
+
     // To-Do: add support for flybarred helis
 
     // convert the input to the desired body frame rate
diff --git a/ArduCopter/heli_control_stabilize.pde b/ArduCopter/heli_control_stabilize.pde
index edb4c73b82..71db2e22f2 100644
--- a/ArduCopter/heli_control_stabilize.pde
+++ b/ArduCopter/heli_control_stabilize.pde
@@ -20,9 +20,24 @@ static void heli_stabilize_run()
     int16_t target_roll, target_pitch;
     float target_yaw_rate;
     int16_t pilot_throttle_scaled;
+    static bool init_targets_on_arming;
 
-    // To-Do: should tradheli reset roll, pitch, yaw targets when motors are not runup?
-
+    // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because
+    // we may be in autorotation flight.  These should be reset only when transitioning from disarmed
+    // to armed, because the pilot will have placed the helicopter down on the landing pad.  This is so
+    // that the servos move in a realistic fashion while disarmed for operational checks.
+    // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
+    
+    if(!motors.armed()) {
+        init_targets_on_arming=true;
+        attitude_control.set_yaw_target_to_current_heading();
+    }
+    
+    if(motors.armed() && init_targets_on_arming) {
+        init_targets_on_arming=false;
+        attitude_control.relax_bf_rate_controller();
+    }
+    
     // apply SIMPLE mode transform to pilot inputs
     update_simple_mode();