starting migration of functions

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

ArducopterNG/ArducopterNG.pde

@@ -6,7 +6,7 @@
  File     : Arducopter.pde
  Version  : v1.0, Aug 27, 2010
  Author(s): ArduCopter Team
-             Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
+             Ted Carancho (AeroQuad), Jose Julio, Jordi Muñoz,
              Jani Hirvinen, Ken McEwans, Roberto Navoni,          
              Sandro Benigno, Chris Anderson
  
@@ -145,7 +145,159 @@ void setup() {
   //APM_Init_Serial();
   //APM_Init_xx
 
+// Just add this in now and edit later
+  int i, j;
+  float aux_float[3];
 
+  pinMode(LED_Yellow,OUTPUT); //Yellow LED A  (PC1)
+  pinMode(LED_Red,OUTPUT);    //Red LED B     (PC2)
+  pinMode(LED_Green,OUTPUT);  //Green LED C   (PC0)
+
+  pinMode(SW1_pin,INPUT);     //Switch SW1 (pin PG0)
+
+  pinMode(RELE_pin,OUTPUT);   // Rele output
+  digitalWrite(RELE_pin,LOW);
+  
+  APM_RC.Init();             // APM Radio initialization
+  // RC channels Initialization (Quad motors)  
+  APM_RC.OutputCh(0,MIN_THROTTLE);  // Motors stoped
+  APM_RC.OutputCh(1,MIN_THROTTLE);
+  APM_RC.OutputCh(2,MIN_THROTTLE);
+  APM_RC.OutputCh(3,MIN_THROTTLE);
+
+  //  delay(1000); // Wait until frame is not moving after initial power cord has connected
+  for(i = 0; i <= 50; i++) {
+    digitalWrite(LED_Green, HIGH);
+    digitalWrite(LED_Yellow, HIGH);
+    digitalWrite(LED_Red, HIGH);
+    delay(20);
+    digitalWrite(LED_Green, LOW);
+    digitalWrite(LED_Yellow, LOW);
+    digitalWrite(LED_Red, LOW);
+    delay(20);
+  }
+
+  APM_ADC.Init();            // APM ADC library initialization
+  DataFlash.Init();          // DataFlash log initialization
+
+#ifdef IsGPS  
+  GPS.Init();                // GPS Initialization
+#ifdef IsNEWMTEK  
+  delay(250);
+  // DIY Drones MTEK GPS needs binary sentences activated if you upgraded to latest firmware.
+  // If your GPS shows solid blue but LED C (Red) does not go on, your GPS is on NMEA mode
+  Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); 
+#endif
+#endif
+
+  readUserConfig(); // Load user configurable items from EEPROM
+
+  // Safety measure for Channel mids
+  if(roll_mid < 1400 || roll_mid > 1600) roll_mid = 1500;
+  if(pitch_mid < 1400 || pitch_mid > 1600) pitch_mid = 1500;
+  if(yaw_mid < 1400 || yaw_mid > 1600) yaw_mid = 1500;
+
+  if (MAGNETOMETER == 1)
+    APM_Compass.Init();  // I2C initialization
+
+  DataFlash.StartWrite(1);   // Start a write session on page 1
+
+  SerBeg(SerBau);                      // Initialize SerialXX.port, IsXBEE define declares which port
+#ifndef CONFIGURATOR  
+  SerPri("ArduCopter Quadcopter v");
+  SerPriln(VER)
+  SerPri("Serial ready on port: ");    // Printout greeting to selecter serial port
+  SerPriln(SerPor);                    // Printout serial port name
+#endif
+
+  // 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)
+  {
+    SerPriln("Entering Log Read Mode...");
+    Log_Read(1,2000);
+    delay(30000);
+  }
+
+  Read_adc_raw();
+  delay(10);
+
+  // Offset values for accels and gyros...
+  AN_OFFSET[3] = acc_offset_x;
+  AN_OFFSET[4] = acc_offset_y;
+  AN_OFFSET[5] = acc_offset_z;
+  aux_float[0] = gyro_offset_roll;
+  aux_float[1] = gyro_offset_pitch;
+  aux_float[2] = gyro_offset_yaw;
+
+  j = 0;
+  // 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;
+      //SerPri(AN[y]);
+      //SerPri(",");
+    }
+    //SerPriln();
+    Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
+    delay(10);
+    
+    // Runnings lights effect to let user know that we are taking mesurements
+    if(j == 0) {
+      digitalWrite(LED_Green, HIGH);
+      digitalWrite(LED_Yellow, LOW);
+      digitalWrite(LED_Red, LOW);
+    } 
+    else if (j == 1) {
+      digitalWrite(LED_Green, LOW);
+      digitalWrite(LED_Yellow, HIGH);
+      digitalWrite(LED_Red, LOW);
+    } 
+    else {
+      digitalWrite(LED_Green, LOW);
+      digitalWrite(LED_Yellow, LOW);
+      digitalWrite(LED_Red, HIGH);
+    }
+    if((i % 5) == 0) j++;
+    if(j >= 3) j = 0;
+  }
+  digitalWrite(LED_Green, LOW);
+  digitalWrite(LED_Yellow, LOW);
+  digitalWrite(LED_Red, LOW);
+
+  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
+  for(i=0;i<6;i++)
+  {
+    SerPri("AN[]:");
+    SerPriln(AN_OFFSET[i]);
+  }
+  SerPri("Yaw neutral value:");
+  //  SerPriln(Neutro_yaw);
+  SerPri(yaw_mid);
+#endif
+  delay(1000);
+
+  DataFlash.StartWrite(1);   // Start a write session on page 1
+  timer = millis();
+  tlmTimer = millis();
+  Read_adc_raw();        // Initialize ADC readings...
+  delay(20);
+
+#ifdef IsAM
+  // Switch Left & Right lights on
+  digitalWrite(RI_LED, HIGH);
+  digitalWrite(LE_LED, HIGH); 
+#endif
+
+  motorArmed = 0;
+  digitalWrite(LED_Green,HIGH);     // Ready to go...
   
 }
 
@@ -155,27 +307,38 @@ void setup() {
 /* ************** MAIN PROGRAM - MAIN LOOP ******************** */
 /* ************************************************************ */
 void loop() {
-	// We want this to execute at 500Hz if possible
+  // We want this to execute at 500Hz if possible
-	// -------------------------------------------
+  // -------------------------------------------
-	if (millis()-fast_loopTimer > 5) {
+  if (millis()-fast_loopTimer > 5) {
-		deltaMiliSeconds 	= millis() - fast_loopTimer;
+    deltaMiliSeconds = millis() - fast_loopTimer;
-		G_Dt 			= (float)deltaMiliSeconds / 1000.f;
+    G_Dt = (float)deltaMiliSeconds / 1000.0f;
-		fast_loopTimer		= millis();
+    fast_loopTimer = millis();
-		mainLoop_count++;
+    mainLoop_count++;
+    // Execute the fast loop
+    // ---------------------
+    // fast_loop();
+    // - PWM Updates
+    // - Stabilization
+    // - Altitude correction
+    if(FL_mode == 0) {        // Changed for variable
+      gled_speed = 1200;
+      Attitude_control_v3();
+    }
+    else {
+      gled_speed = 400;
+      Rate_control_v2();
+      // Reset yaw, so if we change to stable mode we continue with the actual yaw direction
+      command_rx_yaw = ToDeg(yaw);
+    }
+		
+  // Execute the medium loop 
+  // -----------------------
+  // medium_loop();
+  // - Radio read
+  // - GPS read
+  // - Drift correction
   
-		// Execute the fast loop
-		// ---------------------
-		// fast_loop();
-                // - PWM Updates
-                // - Stabilization
-                // - Altitude correction
 
-		// Execute the medium loop 
-		// -----------------------
-		// medium_loop();
-                // - Radio read
-                // - GPS read
-                // - Drift correction
 
 		// Execute the slow loop 
 		// -----------------------

ArducopterNG/Attitude.pde

@@ -36,7 +36,7 @@ TODO:
 /* ************************************************************ */
 
 ////////////////////////////////////////////////// 
-// Function  : Attitude_control_v2()
+// Function  : Attitude_control_v3()
 //
 //             Stable flight mode main algoritms
 //
@@ -52,77 +52,70 @@ TODO:
 //  Radio input, Gyro
 //
 
+/* ************************************************************ */
 // STABLE MODE
-// ROLL, PITCH and YAW PID controls... 
+// PI absolute angle control driving a P rate control
 // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
-void Attitude_control_v2()
+void Attitude_control_v3()
 {
-  float err_roll_rate;
+  float stable_roll,stable_pitch,stable_yaw;
-  float err_pitch_rate;
-  float roll_rate;
-  float pitch_rate;
   
   // ROLL CONTROL    
-  if (AP_mode == 2)        // Normal Mode => Stabilization mode
+  if (AP_mode==2)        // Normal Mode => Stabilization mode
     err_roll = command_rx_roll - ToDeg(roll);
   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...
+  roll_I += err_roll*G_Dt;
+  roll_I = constrain(roll_I,-20,20);
 
-  // New control term...
+  // PID absolute angle control
-  roll_rate = ToDeg(Omega[0]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
-  err_roll_rate = ((ch_roll - roll_mid) >> 1) - roll_rate;
-
-  roll_I += err_roll * G_Dt;
-  roll_I = constrain(roll_I, -20, 20);
-  // D term implementation => two parts: gyro part and command part
-  // To have a better (faster) response we can use the Gyro reading directly for the Derivative term...
-  // We also add a part that takes into account the command from user (stick) to make the system more responsive to user inputs
-  roll_D = - roll_rate;  // Take into account Angular velocity of the stick (command)
-
-  // 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;  
+  stable_roll = K_aux*err_roll + KI_QUAD_ROLL*roll_I;
+  
+  // PD rate control (we use also the bias corrected gyro rates)
+  err_roll = stable_roll - ToDeg(Omega[0]); // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
+  control_roll = STABLE_MODE_KP_RATE_ROLL*err_roll;
   
   // PITCH CONTROL
   if (AP_mode==2)        // Normal mode => Stabilization mode
     err_pitch = command_rx_pitch - ToDeg(pitch);
-  else
+  else                   // GPS Position hold
     err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch);  // Position Control
+  err_pitch = constrain(err_pitch,-25,25);  // to limit max pitch command...
   
-  err_pitch = constrain(err_pitch, -25, 25);  // to limit max pitch command...
+  pitch_I += err_pitch*G_Dt;
+  pitch_I = constrain(pitch_I,-20,20);
  
-  // New control term...
+  // PID absolute angle control
-  pitch_rate = ToDeg(Omega[1]);
-  err_pitch_rate = ((ch_pitch - pitch_mid) >> 1) - pitch_rate;
-
-  pitch_I += err_pitch * G_Dt;
-  pitch_I = constrain(pitch_I, -20, 20);
-  // D term
-  pitch_D = - pitch_rate;
-
-  // PID control
   K_aux = KP_QUAD_PITCH; // Comment this out if you want to use transmitter to adjust gain
-  control_pitch = K_aux * err_pitch + KD_QUAD_PITCH * pitch_D + KI_QUAD_PITCH * pitch_I + STABLE_MODE_KP_RATE * err_pitch_rate; 
+  stable_pitch = K_aux*err_pitch + KI_QUAD_PITCH*pitch_I;
+  
+  // P rate control (we use also the bias corrected gyro rates)
+  err_pitch = stable_pitch - ToDeg(Omega[1]);
+  control_pitch = STABLE_MODE_KP_RATE_PITCH*err_pitch;
   
   // 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;
+  err_yaw = constrain(err_yaw,-60,60);  // to limit max yaw command...
   
-  err_yaw = constrain(err_yaw, -60, 60);  // to limit max yaw command...
+  yaw_I += err_yaw*G_Dt;
+  yaw_I = constrain(yaw_I,-20,20);
  
-  yaw_I += err_yaw * G_Dt;
+  // PID absoulte angle control
-  yaw_I = constrain(yaw_I, -20, 20);
+  stable_yaw = KP_QUAD_YAW*err_yaw + KI_QUAD_YAW*yaw_I;
-  yaw_D = - ToDeg(Omega[2]);
+  // PD rate control (we use also the bias corrected gyro rates)
-
+  err_yaw = stable_yaw - ToDeg(Omega[2]);
-  // PID control
+  control_yaw = STABLE_MODE_KP_RATE_YAW*err_yaw;
-  control_yaw = KP_QUAD_YAW * err_yaw + KD_QUAD_YAW * yaw_D + KI_QUAD_YAW * yaw_I;
 }
 
+// ACRO MODE
+
 
 ////////////////////////////////////////////////// 
 // Function  : Rate_control()
@@ -143,46 +136,42 @@ void Attitude_control_v2()
 
 
 // ACRO MODE
-void Rate_control()
+void Rate_control_v2()
 {
   static float previousRollRate, previousPitchRate, previousYawRate;
   float currentRollRate, currentPitchRate, currentYawRate;
   
   // ROLL CONTROL
-  currentRollRate = read_adc(0);      // I need a positive sign here
+  currentRollRate = ToDeg(Omega[0]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
-
+  err_roll = ((ch_roll- roll_mid) * xmitFactor) - currentRollRate;  
-  err_roll = ((ch_roll - roll_mid) * xmitFactor) - currentRollRate;
+  roll_I += err_roll*G_Dt;
-
+  roll_I = constrain(roll_I,-20,20);
-  roll_I += err_roll * G_Dt;
+  roll_D = (currentRollRate - previousRollRate)/G_Dt;
-  roll_I = constrain(roll_I, -20, 20);
-
-  roll_D = currentRollRate - previousRollRate;
   previousRollRate = currentRollRate;  
-
   // PID control
-  control_roll = Kp_RateRoll * err_roll + Kd_RateRoll * roll_D + Ki_RateRoll * roll_I; 
+  control_roll = Kp_RateRoll*err_roll + Kd_RateRoll*roll_D + Ki_RateRoll*roll_I; 
   
   // PITCH CONTROL
-  currentPitchRate = read_adc(1);
+  currentPitchRate = ToDeg(Omega[1]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
   err_pitch = ((ch_pitch - pitch_mid) * xmitFactor) - currentPitchRate;
   
   pitch_I += err_pitch*G_Dt;
   pitch_I = constrain(pitch_I,-20,20);
 
-  pitch_D = currentPitchRate - previousPitchRate;
+  pitch_D = (currentPitchRate - previousPitchRate)/G_Dt;
   previousPitchRate = currentPitchRate;
  
   // PID control
   control_pitch = Kp_RatePitch*err_pitch + Kd_RatePitch*pitch_D + Ki_RatePitch*pitch_I; 
   
   // YAW CONTROL
-  currentYawRate = read_adc(2);
+  currentYawRate = ToDeg(Omega[2]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected;
-  err_yaw = ((ch_yaw - yaw_mid) * xmitFactor) - currentYawRate;
+  err_yaw = ((ch_yaw - yaw_mid)* xmitFactor) - currentYawRate;
   
   yaw_I += err_yaw*G_Dt;
-  yaw_I = constrain(yaw_I, -20, 20);
+  yaw_I = constrain(yaw_I,-20,20);
 
-  yaw_D = currentYawRate - previousYawRate;
+  yaw_D = (currentYawRate - previousYawRate)/G_Dt;
   previousYawRate = currentYawRate;
  
   // PID control
@@ -200,13 +189,13 @@ int channel_filter(int ch, int ch_old)
   diff_ch_old = ch - ch_old;      // Difference with old reading
   if (diff_ch_old < 0)
   {
-    if (diff_ch_old <- 40)
+    if (diff_ch_old <- 60)
-      return(ch_old - 40);        // We limit the max difference between readings
+      return(ch_old - 60);        // We limit the max difference between readings
   }
   else
   {
-    if (diff_ch_old > 40)    
+    if (diff_ch_old > 60)    
-      return(ch_old + 40);
+      return(ch_old + 60);
   }
   return((ch + ch_old) >> 1);   // Small filtering
   //return(ch);