From 27ffb9e7e8bf9679a5058705cde336d74799ac86 Mon Sep 17 00:00:00 2001
From: Jairo <jairoscz@gmail.com>
Date: Sat, 7 Apr 2012 21:17:46 -0500
Subject: [PATCH] Better aircraft enable. Tuning PID

---
 main.cpp | 24 ++++++++++++++----------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/main.cpp b/main.cpp
index e64cee4..cf2453a 100644
--- a/main.cpp
+++ b/main.cpp
@@ -53,8 +53,10 @@ int main(void) {
     uint32 t_prev, dt;
     uint32 cpu_util, cpu_util2;
     uint32 tick50hz = 0;
+
+    bool throttle_enable;
+
     float uRoll, uPitch, uYaw, uThrottle;
-    float KpRoll, KiRoll, KdRoll;
     float m1, m2, m3;
     float pidRoll, pidPitch, pidYaw;
     float servoAngle;
@@ -81,15 +83,12 @@ int main(void) {
     t_prev = 0;
     uRoll = 0;
 
-
-    KpRoll = 0.00005; //rc.get_channel(CH_AUX1)*0.001; //
-    KiRoll = 0; //rc.get_channel(CH_AUX2)*0.001;
-    KdRoll = -0.000004; //rc.get_channel(CH_AUX3)*-0.00005;
-
-    rollCtrl.set_gains(KpRoll, KiRoll, KdRoll);
-    pitchCtrl.set_gains(KpRoll, KiRoll, KdRoll);
+    rollCtrl.set_gains(  0.0004, 0.00, 0.00000);
+    pitchCtrl.set_gains( 0.0005, 0.00, 0.00000);
     yawCtrl.set_gains(1.0, 0, 0);
 
+    throttle_enable = 0;
+
     while (1) {
 
         t = micros();
@@ -102,6 +101,8 @@ int main(void) {
             rc.update(); // update RC commands
             ahrs.update();
 
+            throttle_enable = (rc.get_channel(CH_AUX4) > 0.5 && rc.status() == SUCCESS);
+
             if (rc.status() == SUCCESS) {
                 uThrottle = rc.get_channel(CH_THROTTLE);
                 uRoll = 100 * (rc.get_channel(CH_ROLL) - 0.5);
@@ -133,7 +134,7 @@ int main(void) {
             m3 = uThrottle + ( 0) * (pidRoll) + (+4.0/3.0) * pidPitch;
 
             yawServo.set_offset(servoAngle);
-            if (rc.get_channel(CH_AUX4) > 0.5 && rc.status() == SUCCESS) {
+            if (throttle_enable) {
                 set_rotor_throttle(1, m1);
                 set_rotor_throttle(2, m2);
                 set_rotor_throttle(3, m3);
@@ -146,7 +147,10 @@ int main(void) {
 
             // 10Hz loop
             if (tick50hz % 5 == 0) {
-                toggleLED();
+                if(throttle_enable)
+                    toggleLED();
+                else
+                    digitalWrite(BOARD_LED_PIN, 0);
             }
 
             // 2Hz loop