From c60ff46cc5be9bb33c6d31bdd97f5a6842c44ac2 Mon Sep 17 00:00:00 2001
From: Robert Lefebvre <robert.lefebvre@gmail.com>
Date: Wed, 17 Jul 2013 21:47:39 -0400
Subject: [PATCH] TradHeli: leaky-I-term based on dynamic_flight_detector

---
 ArduCopter/ArduCopter.pde | 34 +++++++++++++++++++++++++++++++++-
 ArduCopter/Attitude.pde   | 12 ++++++++++--
 2 files changed, 43 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 455930dcf7..02829f6517 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -670,6 +670,11 @@ static uint8_t throttle_mode;
 static int16_t angle_boost;
 // counter to verify landings
 static uint16_t land_detector;
+// counter to control dynamic flight profile
+#if FRAME_CONFIG == HELI_FRAME
+static uint8_t dynamic_flight_counter;
+static bool dynamic_flight;
+#endif // HELI_FRAME
 
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -1873,7 +1878,7 @@ void update_throttle_mode(void)
 	} else {
 		motors.stab_throttle = false;
 	}
-
+    check_dynamic_flight();
     // allow swash collective to move if we are in manual throttle modes, even if disarmed
     if( !motors.armed() ) {
         if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){
@@ -2014,6 +2019,33 @@ void update_throttle_mode(void)
     }
 }
 
+#if FRAME_CONFIG == HELI_FRAME
+static void check_dynamic_flight(void){
+    
+    if (!motors.armed() || throttle_mode == THROTTLE_LAND || motors.motor_runup_complete == false){
+        dynamic_flight_counter=0;
+        dynamic_flight=false;
+        return;
+    }
+    if (dynamic_flight_counter < 255){                                                      // check if we're in dynamic flight mode
+        if (g.rc_3.servo_out > 800 || (labs(ahrs.pitch_sensor) > 2000)) {
+            dynamic_flight_counter++;
+        }
+        if (dynamic_flight_counter > 254){                                              // we must be in the air by now
+            dynamic_flight = true;
+        }
+    }
+    if (dynamic_flight_counter > 0){
+        if ((labs(ahrs.roll_sensor) < 1500) && (labs(ahrs.pitch_sensor) < 1500)) {
+            dynamic_flight_counter--;
+        }
+        if (dynamic_flight_counter < 1){
+            dynamic_flight = false;
+        }
+    }
+}
+#endif //  HELI_FRAME
+
 // set_target_alt_for_reporting - set target altitude in cm for reporting purposes (logs and gcs)
 static void set_target_alt_for_reporting(float alt_cm)
 {
diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde
index 0a9f0de6c8..465e961ebc 100644
--- a/ArduCopter/Attitude.pde
+++ b/ArduCopter/Attitude.pde
@@ -498,7 +498,11 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
                 roll_i = g.pid_rate_roll.get_integrator();
             }
         } else {
-            roll_i = g.pid_rate_roll.get_leaky_i(roll_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);	    // Flybarless Helis get huge I-terms. I-term controls much of the rate
+            if (dynamic_flight){
+                roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
+            } else {
+                roll_i = g.pid_rate_roll.get_leaky_i(roll_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
+            }
         }
     }
     
@@ -512,7 +516,11 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
                 pitch_i = g.pid_rate_pitch.get_integrator();
             }
         } else {
-            pitch_i = g.pid_rate_pitch.get_leaky_i(pitch_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);	// Flybarless Helis get huge I-terms. I-term controls much of the rate
+            if (dynamic_flight){
+                pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);	
+            } else {
+                pitch_i = g.pid_rate_pitch.get_leaky_i(pitch_rate_error, G_Dt, RATE_INTEGRATOR_LEAK_RATE);
+            }
         }
     }