AM Mode, Green LED optimization

git-svn-id: https://arducopter.googlecode.com/svn/trunk@75 f9c3cf11-9bcb-44bc-f272-b75c42450872

Arducopter/Arducopter.pde

@@ -60,6 +60,16 @@ Red LED Off = No GPS Fix
 #define SW2_pin 40
 /* *** */
 
+/* AM PIN Definitions */
+/* Can be changed in future to AN extension ports */
+
+#define FR_LED 3  // Mega PE4 pin
+#define RE_LED 2  // Mega PE5 pin
+#define RI_LED 7  // Mega PH4 pin
+#define LE_LED 8  // Mega PH5 pin
+
+/* AM PIN Definitions - END */
+
 /* ***************************************************************************** */
 /*  CONFIGURATION PART                                                           */
 /* ***************************************************************************** */
@@ -86,10 +96,12 @@ Red LED Off = No GPS Fix
 #define OUTPUTMODE 1  //If value = 1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 Accel only data
 
 //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
-uint8_t sensors[6] = {1,2,0,4,5,6};  // For ArduPilot Mega Sensor Shield Hardware
+uint8_t sensors[6] = {
+  1,2,0,4,5,6};  // For ArduPilot Mega Sensor Shield Hardware
 
 //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
-int SENSOR_SIGN[]={1,-1,-1,-1,1,1,-1,-1,-1};  //{-1,1,-1,1,-1,1,-1,-1,-1};
+int SENSOR_SIGN[]={
+  1,-1,-1,-1,1,1,-1,-1,-1};  //{-1,1,-1,1,-1,1,-1,-1,-1};
 
 int AN[6]; //array that store the 6 ADC channels
 int AN_OFFSET[6]; //Array that store the Offset of the gyros and accelerometers
@@ -98,18 +110,27 @@ int gyro_temp;
 
 float G_Dt=0.02;    // Integration time for the gyros (DCM algorithm)
 
-float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+float Accel_Vector[3]= {
-float Accel_Vector_unfiltered[3]= {0,0,0}; //Store the acceleration in a vector
+  0,0,0}; //Store the acceleration in a vector
+float Accel_Vector_unfiltered[3]= {
+  0,0,0}; //Store the acceleration in a vector
 //float Accel_magnitude;
 //float Accel_weight;
-float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
+float Gyro_Vector[3]= {
-float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
+  0,0,0};//Store the gyros rutn rate in a vector
-float Omega_P[3]= {0,0,0};//Omega Proportional correction
+float Omega_Vector[3]= {
-float Omega_I[3]= {0,0,0};//Omega Integrator
+  0,0,0}; //Corrected Gyro_Vector data
-float Omega[3]= {0,0,0};
+float Omega_P[3]= {
+  0,0,0};//Omega Proportional correction
+float Omega_I[3]= {
+  0,0,0};//Omega Integrator
+float Omega[3]= {
+  0,0,0};
 
-float errorRollPitch[3]= {0,0,0};
+float errorRollPitch[3]= {
-float errorYaw[3]= {0,0,0};
+  0,0,0};
+float errorYaw[3]= {
+  0,0,0};
 float errorCourse=0;
 float COGX=0; //Course overground X axis
 float COGY=1; //Course overground Y axis
@@ -121,16 +142,29 @@ float yaw=0;
 unsigned int counter=0;
 
 float DCM_Matrix[3][3]= {
-  {1,0,0}
+  {
-  ,{0,1,0}
+    1,0,0  }
-  ,{0,0,1}
+  ,{
+    0,1,0  }
+  ,{
+    0,0,1  }
 }; 
-float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
+float Update_Matrix[3][3]={
+  {
+    0,1,2  }
+  ,{
+    3,4,5  }
+  ,{
+    6,7,8  }
+}; //Gyros here
 
 float Temporary_Matrix[3][3]={
-  {0,0,0}
+  {
-  ,{0,0,0}
+    0,0,0  }
-  ,{0,0,0}
+  ,{
+    0,0,0  }
+  ,{
+    0,0,0  }
 };
 
 // GPS variables
@@ -197,6 +231,11 @@ int Sonar_Counter=0;
 // AP_mode : 1=> Position hold  2=>Stabilization assist mode (normal mode)
 byte AP_mode = 2;  
 
+// Mode LED timers and variables, used to blink LED_Green
+byte gled_status = HIGH;
+long gled_timer;
+int gled_speed;
+
 long t0;
 int num_iter;
 float aux_debug;
@@ -313,7 +352,8 @@ void Attitude_control_v2()
 
   // PID control
   K_aux = KP_QUAD_ROLL; // Comment this out if you want to use transmitter to adjust gain
-  control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I + STABLE_MODE_KP_RATE*err_roll_rate; ; 
+  control_roll = K_aux*err_roll + KD_QUAD_ROLL*roll_D + KI_QUAD_ROLL*roll_I + STABLE_MODE_KP_RATE*err_roll_rate; 
+  ; 
 
   // PITCH CONTROL
   if (AP_mode==2)        // Normal mode => Stabilization mode
@@ -545,6 +585,11 @@ void setup()
   tlmTimer = millis();
   Read_adc_raw();        // Initialize ADC readings...
   delay(20);
+  
+    // Switch Left & Right lights on
+  digitalWrite(RI_LED, HIGH);
+  digitalWrite(LE_LED, HIGH); 
+  
   motorArmed = 0;
   digitalWrite(LED_Green,HIGH);     // Ready to go...
 }
@@ -719,10 +764,13 @@ void loop(){
     }
 
     // Control methodology selected using AUX2
-    if (ch_aux2 < 1200)
+    if (ch_aux2 < 1200) {
+      gled_speed = 1200;
       Attitude_control_v2();
+    }
     else
     {
+      gled_speed = 400;
       Rate_control();
       // Reset yaw, so if we change to stable mode we continue with the actual yaw direction
       command_rx_yaw = ToDeg(yaw);
@@ -764,6 +812,8 @@ void loop(){
     // Quadcopter mix
     // Ask Jose if we still need this IF statement, and if we want to do an ESC calibration
     if (motorArmed == 1) {   
+       digitalWrite(FR_LED, HIGH);    // AM-Mode
+     
 #ifdef FLIGHT_MODE_+
       rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
       leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
@@ -778,6 +828,9 @@ void loop(){
 #endif
     }
     if (motorArmed == 0) {
+      digitalWrite(FR_LED, LOW);    // AM-Mode
+      digitalWrite(LED_Green,HIGH); // Ready LED on
+      
       rightMotor = MIN_THROTTLE;
       leftMotor = MIN_THROTTLE;
       frontMotor = MIN_THROTTLE;
@@ -808,4 +861,23 @@ void loop(){
     tlmTimer = millis();
   }
 #endif
+
+  // AM and Mode lights
+  if(millis() - gled_timer > gled_speed) {
+    gled_timer = millis();
+    if(gled_status == HIGH) { 
+      digitalWrite(LED_Green, LOW);
+      digitalWrite(RE_LED, LOW);
+      gled_status = LOW;
     } 
+    else {
+      digitalWrite(LED_Green, HIGH);
+      if(motorArmed) digitalWrite(RE_LED, HIGH);
+      gled_status = HIGH;
+    } 
+  }
+
+
+
+}
+