AM Mode, Green LED optimization

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

Arducopter/Arducopter.pde

@@ -24,18 +24,18 @@
 
 /*
 **** Switch Functions *****
-AUX1 ON = Stable Mode
-AUX1 OFF = Acro Mode
-GEAR ON = GPS Hold
-GEAR OFF = Flight Assist (Stable Mode)
-
-**** LED Feedback ****
-Green LED On = APM Initialization Finished
-Yellow LED On = GPS Hold Mode
-Yellow LED Off = Flight Assist Mode (No GPS)
-Red LED On = GPS Fix
-Red LED Off = No GPS Fix
-*/
+ AUX1 ON = Stable Mode
+ AUX1 OFF = Acro Mode
+ GEAR ON = GPS Hold
+ GEAR OFF = Flight Assist (Stable Mode)
+ 
+ **** LED Feedback ****
+ Green LED On = APM Initialization Finished
+ Yellow LED On = GPS Hold Mode
+ Yellow LED Off = Flight Assist Mode (No GPS)
+ Red LED On = GPS Fix
+ Red LED Off = No GPS Fix
+ */
 
 #include <Wire.h>
 #include <APM_ADC.h>
@@ -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_unfiltered[3]= {0,0,0}; //Store the acceleration in a vector
+float Accel_Vector[3]= {
+  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 Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
-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 errorYaw[3]= {0,0,0};
+float Gyro_Vector[3]= {
+  0,0,0};//Store the gyros rutn rate in a vector
+float Omega_Vector[3]= {
+  0,0,0}; //Corrected Gyro_Vector data
+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 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}
-  ,{0,0,1}
+  {
+    1,0,0  }
+  ,{
+    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;
@@ -298,7 +337,7 @@ void Attitude_control_v2()
   else
     err_roll = (command_rx_roll + command_gps_roll) - ToDeg(roll);  // Position control
 
-  err_roll = constrain(err_roll,-25,25);  // to limit max roll command...
+    err_roll = constrain(err_roll,-25,25);  // to limit max roll command...
 
   // New control term...
   roll_rate = ToDeg(Omega[0]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
@@ -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
@@ -339,7 +379,7 @@ void Attitude_control_v2()
   // YAW CONTROL
   err_yaw = command_rx_yaw - ToDeg(yaw);
   if (err_yaw > 180)    // Normalize to -180,180
-    err_yaw -= 360;
+      err_yaw -= 360;
   else if(err_yaw < -180)
     err_yaw += 360;
 
@@ -410,15 +450,15 @@ int channel_filter(int ch, int ch_old)
     return(ch);
   diff_ch_old = ch - ch_old;      // Difference with old reading
   if (diff_ch_old<0)
-     {
-     if (diff_ch_old<-40)
-       return(ch_old-40);        // We limit the max difference between readings
-     }
+  {
+    if (diff_ch_old<-40)
+      return(ch_old-40);        // We limit the max difference between readings
+  }
   else
-     {
-     if (diff_ch_old>40)    
-       return(ch_old+40);
-     }
+  {
+    if (diff_ch_old>40)    
+      return(ch_old+40);
+  }
   return((ch+ch_old)>>1);   // Small filtering
   //return(ch);
 }
@@ -459,7 +499,7 @@ void setup()
 
   DataFlash.StartWrite(1);   // Start a write session on page 1
 
-  //Serial.begin(57600);
+    //Serial.begin(57600);
   Serial.begin(115200);
   //Serial.println();
   //Serial.println("ArduCopter Quadcopter v1.0");
@@ -467,11 +507,11 @@ void setup()
   // Check if we enable the DataFlash log Read Mode (switch)
   // If we press switch 1 at startup we read the Dataflash eeprom
   while (digitalRead(SW1_pin)==0)
-    {
+  {
     Serial.println("Entering Log Read Mode...");
     Log_Read(1,1000);
     delay(30000);
-    }
+  }
 
   //delay(3000);
 
@@ -488,55 +528,55 @@ void setup()
 
   // Take the gyro offset values
   for(i=0;i<300;i++)
-    {
+  {
     Read_adc_raw();
     for(int y=0; y<=2; y++)   // Read initial ADC values for gyro offset.
-      {
+    {
       aux_float[y]=aux_float[y]*0.8 + AN[y]*0.2;
       //Serial.print(AN[y]);
       //Serial.print(",");
-      }
+    }
     //Serial.println();
     Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
     delay(10);
-    }
+  }
   for(int y=0; y<=2; y++)   
     AN_OFFSET[y]=aux_float[y];
 
   Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value
-  #ifndef CONFIGURATOR
+#ifndef CONFIGURATOR
   for(i=0;i<6;i++)
-    {
+  {
     Serial.print("AN[]:");
     Serial.println(AN_OFFSET[i]);
-    }
+  }
 
   Serial.print("Yaw neutral value:");
   Serial.println(Neutro_yaw);
-  #endif
+#endif
 
-  #if (RADIO_TEST_MODE)    // RADIO TEST MODE TO TEST RADIO CHANNELS
+#if (RADIO_TEST_MODE)    // RADIO TEST MODE TO TEST RADIO CHANNELS
   while(1)
-   {
-   if (APM_RC.GetState()==1)
-     {
-     Serial.print("AIL:");
-     Serial.print(APM_RC.InputCh(0));
-     Serial.print("ELE:");
-     Serial.print(APM_RC.InputCh(1));
-     Serial.print("THR:");
-     Serial.print(APM_RC.InputCh(2));
-     Serial.print("YAW:");
-     Serial.print(APM_RC.InputCh(3));
-     Serial.print("AUX(mode):");
-     Serial.print(APM_RC.InputCh(4));
-     Serial.print("AUX2:");
-     Serial.print(APM_RC.InputCh(5));
-     Serial.println();
-     delay(200);
-     }
-   } 
-  #endif  
+  {
+    if (APM_RC.GetState()==1)
+    {
+      Serial.print("AIL:");
+      Serial.print(APM_RC.InputCh(0));
+      Serial.print("ELE:");
+      Serial.print(APM_RC.InputCh(1));
+      Serial.print("THR:");
+      Serial.print(APM_RC.InputCh(2));
+      Serial.print("YAW:");
+      Serial.print(APM_RC.InputCh(3));
+      Serial.print("AUX(mode):");
+      Serial.print(APM_RC.InputCh(4));
+      Serial.print("AUX2:");
+      Serial.print(APM_RC.InputCh(5));
+      Serial.println();
+      delay(200);
+    }
+  } 
+#endif  
 
   delay(1000);
 
@@ -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...
 }
@@ -572,11 +617,11 @@ void loop(){
     Read_adc_raw();
     if (MAGNETOMETER == 1) {
       if (counter > 10)  // Read compass data at 10Hz... (10 loop runs)
-        {
+      {
         counter=0;
         APM_Compass.Read();     // Read magnetometer
         APM_Compass.Calculate(roll,pitch);  // Calculate heading
-        }
+      }
     }
     Matrix_update(); 
     Normalize();
@@ -589,7 +634,7 @@ void loop(){
     log_pitch = ToDeg(pitch)*10;
     log_yaw = ToDeg(yaw)*10;
 
-  #ifndef CONFIGURATOR    
+#ifndef CONFIGURATOR    
     Serial.print(log_roll);
     Serial.print(",");
     Serial.print(log_pitch);
@@ -597,11 +642,11 @@ void loop(){
     Serial.print(log_yaw);
 
     for (int i=0;i<6;i++)
-      {
+    {
       Serial.print(AN[i]);
       Serial.print(",");
-      }
-  #endif
+    }
+#endif
 
     // Write Sensor raw data to DataFlash log
     Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],gyro_temp);
@@ -609,7 +654,7 @@ void loop(){
     Log_Write_Attitude(log_roll,log_pitch,log_yaw);
 
     if (APM_RC.GetState()==1)   // New radio frame?
-      {
+    {
       // Commands from radio Rx... 
       // Stick position defines the desired angle in roll, pitch and yaw
       ch_roll = channel_filter(APM_RC.InputCh(0),ch_roll);
@@ -645,32 +690,32 @@ void loop(){
 
       // We read the Quad Mode from Channel 5
       if (ch_aux < 1200)
-        {
+      {
         AP_mode = 1;           // Position hold mode (GPS position control)
         digitalWrite(LED_Yellow,HIGH); // Yellow LED On
-        }
+      }
       else
-        {
+      {
         AP_mode = 2;          // Normal mode (Stabilization assist mode)
         digitalWrite(LED_Yellow,LOW); // Yellow LED off
-        }
+      }
       // Write Radio data to DataFlash log
       Log_Write_Radio(ch_roll,ch_pitch,ch_throttle,ch_yaw,int(K_aux*100),(int)AP_mode);
-      }  // END new radio data
+    }  // END new radio data
 
     if (AP_mode==1)  // Position Control
-      {
+    {
       if (target_position==0)   // If this is the first time we switch to Position control, actual position is our target position
-        {
+      {
         target_lattitude = GPS.Lattitude;
         target_longitude = GPS.Longitude;
-        #ifndef CONFIGURATOR
+#ifndef CONFIGURATOR
         Serial.println();
         Serial.print("* Target:");
         Serial.print(target_longitude);
         Serial.print(",");
         Serial.println(target_lattitude);
-        #endif
+#endif
         target_position=1;
         //target_sonar_altitude = sonar_value;
         //Initial_Throttle = ch3;
@@ -678,15 +723,15 @@ void loop(){
         altitude_I = 0;
         gps_roll_I = 0;
         gps_pitch_I = 0;
-        }        
       }        
+    }
     else
       target_position=0;
 
     //Read GPS
     GPS.Read();
     if (GPS.NewData)  // New GPS data?
-      {
+    {
       GPS.NewData=0;  // We Reset the flag...
 
       //Output GPS data
@@ -704,30 +749,33 @@ void loop(){
         digitalWrite(LED_Red,LOW);
 
       if (AP_mode==1)
-        {
+      {
         if ((target_position==1)&&(GPS.Fix))
-          {
+        {
           Position_control(target_lattitude,target_longitude);  // Call position hold routine
-          }
+        }
         else
-          {
+        {
           //Serial.print("NOFIX");
           command_gps_roll=0;
           command_gps_pitch=0;
-          }
         }
       }
+    }
 
     // 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);
       command_rx_yaw_diff = 0;
-      }
+    }
 
     // Arm motor output : Throttle down and full yaw right for more than 2 seconds
     if (ch_throttle < 1200) {
@@ -764,20 +812,25 @@ 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) {   
-      #ifdef FLIGHT_MODE_+
-        rightMotor = constrain(ch_throttle - control_roll - control_yaw, minThrottle, 2000);
-        leftMotor = constrain(ch_throttle + control_roll - control_yaw, minThrottle, 2000);
-        frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000);
-        backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000);
-      #endif
-      #ifdef FLIGHT_MODE_X
-        frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor
-        rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor
-        leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000);  // rear left motor
-        backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000);  // rear right motor
-      #endif
+       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);
+      frontMotor = constrain(ch_throttle + control_pitch + control_yaw, minThrottle, 2000);
+      backMotor = constrain(ch_throttle - control_pitch + control_yaw, minThrottle, 2000);
+#endif
+#ifdef FLIGHT_MODE_X
+      frontMotor = constrain(ch_throttle + control_roll + control_pitch - control_yaw, minThrottle, 2000); // front left motor
+      rightMotor = constrain(ch_throttle - control_roll + control_pitch + control_yaw, minThrottle, 2000); // front right motor
+      leftMotor = constrain(ch_throttle + control_roll - control_pitch + control_yaw, minThrottle, 2000);  // rear left motor
+      backMotor = constrain(ch_throttle - control_roll - control_pitch - control_yaw, minThrottle, 2000);  // rear right motor
+#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;
@@ -797,15 +850,34 @@ void loop(){
     APM_RC.Force_Out0_Out1();
     APM_RC.Force_Out2_Out3();
 
-    #ifndef CONFIGURATOR
+#ifndef CONFIGURATOR
     Serial.println();  // Line END 
-    #endif
- }
-  #ifdef CONFIGURATOR
+#endif
+  }
+#ifdef CONFIGURATOR
   if((millis()-tlmTimer)>=100) {
     readSerialCommand();
     sendSerialTelemetry();
     tlmTimer = millis();
   }
-  #endif
+#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;
+    } 
+  }
+
+
+
 }
+