IMU performance enhancements and stable mode update

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

Arducopter/Arducopter.pde

@@ -9,7 +9,7 @@
 /*           Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,            */
 /*           Jani Hirvinen, Ken McEwans, Roberto Navoni,                  */
 /*           Sandro Benigno, Chris Anderson                               */
-/* Date : 04-07-2010                                                      */
+/* Date : 08-08-2010                                                      */
 /* Version : 1.3 beta                                                     */
 /* Hardware : ArduPilot Mega + Sensor Shield (Production versions)        */
 /* Mounting position : RC connectors pointing backwards                   */
@@ -19,7 +19,7 @@
 /*   DataFlash : DataFlash log library                                    */
 /*   APM_BMP085 : BMP085 barometer library                                */
 /*   APM_Compass : HMC5843 compass library [optional]                     */
-/*   GPS_UBLOX or GPS_NMEA: GPS library    [optional]                     */
+/*   GPS_UBLOX or GPS_NMEA or GPS_MTK : GPS library    [optional]         */
 /* ********************************************************************** */
 
 /*
@@ -43,7 +43,8 @@ Red LED Off = No GPS Fix
 #include <DataFlash.h>
 #include <APM_Compass.h>
 // Put your GPS library here:
-#include <GPS_NMEA.h>  // MTK GPS
+//#include <GPS_NMEA.h>  // MTK GPS
+#include <GPS_MTK.h>
 //#include <GPS_UBLOX.h>
 
 // EEPROM storage for user configurable values
@@ -92,6 +93,8 @@ 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
+int gyro_temp;
+
 
 float G_Dt=0.02;    // Integration time for the gyros (DCM algorithm)
 
@@ -275,10 +278,9 @@ void Position_control(long lat_dest, long lon_dest)
 }
 
 /* ************************************************************ */
+// STABLE MODE
 // ROLL, PITCH and YAW PID controls... 
 // Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
-
-// Stable Mode
 void Attitude_control()
 {
   // ROLL CONTROL    
@@ -336,7 +338,83 @@ void Attitude_control()
   control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
 }
 
-// Acro Mode
+/* ************************************************************ */
+// STABLE MODE
+// ROLL, PITCH and YAW PID controls... 
+// Input : desired Roll, Pitch and Yaw absolute angles. Output : Motor commands
+void Attitude_control_v2()
+{
+  
+  #define STABLE_MODE_KP_RATE_ROLL 0.2
+  #define STABLE_MODE_KP_RATE_PITCH 0.2
+  
+  float err_roll_rate;
+  float err_pitch_rate;
+  float roll_rate;
+  float pitch_rate;
+  
+  // ROLL CONTROL    
+  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...
+  
+  // New control term...
+  roll_rate = ToDeg(Omega[0]);  // Omega[] is the raw gyro reading plus Omega_I, so it´s bias corrected
+  err_roll_rate = ((ch_roll-1500)>>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_ROLL*err_roll_rate; ; 
+  
+  // PITCH CONTROL
+  if (AP_mode==2)        // Normal mode => Stabilization mode
+    err_pitch = command_rx_pitch - ToDeg(pitch);
+  else
+    err_pitch = (command_rx_pitch + command_gps_pitch) - ToDeg(pitch);  // Position Control
+    
+  err_pitch = constrain(err_pitch,-25,25);  // to limit max pitch command...
+  
+  // New control term...
+  pitch_rate = ToDeg(Omega[1]);
+  err_pitch_rate = ((ch_pitch-1500)>>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_PITCH*err_pitch_rate; 
+  
+  // YAW CONTROL
+  err_yaw = command_rx_yaw - ToDeg(yaw);
+  if (err_yaw > 180)    // Normalize to -180,180
+    err_yaw -= 360;
+  else if(err_yaw < -180)
+    err_yaw += 360;
+  
+  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_D = command_rx_yaw_diff*KD_QUAD_COMMAND_PART - ToDeg(Omega[2]);
+ 
+  // PID control
+  control_yaw = KP_QUAD_YAW*err_yaw + KD_QUAD_YAW*yaw_D + KI_QUAD_YAW*yaw_I;
+}
+
+// ACRO MODE
 void Rate_control()
 {
   static float previousRollRate, previousPitchRate, previousYawRate;
@@ -358,8 +436,7 @@ void Rate_control()
   
   // PITCH CONTROL
   currentPitchRate = read_adc(1);
-  // err_pitch = ((1500-ch_pitch) * xmitFactor) - currentPitchRate; // was incorrect, inverted ELE between Arco / Stable
-  err_pitch = ((ch_pitch - 1500) * xmitFactor) - currentPitchRate;  // correct one, now ELE is ok on both modes
+  err_pitch = ((ch_pitch-1500) * xmitFactor) - currentPitchRate;
   
   pitch_I += err_pitch*G_Dt;
   pitch_I = constrain(pitch_I,-20,20);
@@ -403,8 +480,8 @@ int channel_filter(int ch, int ch_old)
      if (diff_ch_old>40)    
        return(ch_old+40);
      }
-  //return((ch+ch_old)>>1);   // Small filtering
-  return(ch);
+  return((ch+ch_old)>>1);   // Small filtering
+  //return(ch);
 }
 
 
@@ -588,7 +665,7 @@ void loop(){
   #endif
     
     // Write Sensor raw data to DataFlash log
-    Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],ch_throttle);
+    Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],gyro_temp);
     // Write attitude to DataFlash log
     Log_Write_Attitude(log_roll,log_pitch,log_yaw);
     
@@ -675,9 +752,9 @@ void loop(){
       
       //Output GPS data
       //Serial.print(",");
-      Serial.print(GPS.Lattitude);
-      Serial.print(",");
-      Serial.print(GPS.Longitude);
+      //Serial.print(GPS.Lattitude);
+      //Serial.print(",");
+      //Serial.print(GPS.Longitude);
      
       // Write GPS data to DataFlash log
       Log_Write_GPS(GPS.Time, GPS.Lattitude,GPS.Longitude,GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats);
@@ -704,7 +781,7 @@ void loop(){
     
     // Control methodology selected using AUX2
     if (ch_aux2 < 1200)
-      Attitude_control();
+      Attitude_control_v2();
     else
       {
       Rate_control();
@@ -775,4 +852,4 @@ void loop(){
     tlmTimer = millis();
   }
   #endif
-}
+}

Arducopter/DCM.pde

@@ -8,7 +8,7 @@ void Read_adc_raw(void)
     AN[i] = APM_ADC.Ch(sensors[i]);
   
   // Correction for non ratiometric sensor (test code)
-  //temp = APM_ADC.Ch(3);
+  gyro_temp = APM_ADC.Ch(3);
   //AN[0] += 1500-temp;
   //AN[1] += 1500-temp;
   //AN[2] += 1500-temp;
@@ -59,24 +59,18 @@ void Drift_correction(void)
   float errorCourse;
   static float Scaled_Omega_P[3];
   static float Scaled_Omega_I[3];
-  float Accel_magnitude;
-  float Accel_weight;
   
   //*****Roll and Pitch***************
 
-  // Calculate the magnitude of the accelerometer vector
-  //Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
-  //Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
-  // Weight for accelerometer info (<0.75G = 0.0, 1G = 1.0 , >1.25G = 0.0)
-  // Accel_weight = constrain(1 - 4*abs(1 - Accel_magnitude),0,1);
-  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
-  //Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);
-  Accel_weight = 1.0;
-
   Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
-  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
-  
-  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+  // errorRollPitch are in Accel ADC units
+  // Limit max errorRollPitch to limit max Omega_P and Omega_I
+  errorRollPitch[0] = constrain(errorRollPitch[0],-50,50);
+  errorRollPitch[1] = constrain(errorRollPitch[1],-50,50);
+  errorRollPitch[2] = constrain(errorRollPitch[2],-50,50);
+  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH);
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH);
   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
   
   //*****YAW***************
@@ -107,14 +101,14 @@ void Matrix_update(void)
   Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
   Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
   
-  Accel_Vector[0]=read_adc(3); // acc x
-  Accel_Vector[1]=read_adc(4); // acc y
-  Accel_Vector[2]=read_adc(5); // acc z
+  //Accel_Vector[0]=read_adc(3); // acc x
+  //Accel_Vector[1]=read_adc(4); // acc y
+  //Accel_Vector[2]=read_adc(5); // acc z
   
   // Low pass filter on accelerometer data (to filter vibrations)
-  //Accel_Vector[0]=Accel_Vector[0]*0.5 + (float)read_adc(3)*0.5; // acc x
-  //Accel_Vector[1]=Accel_Vector[1]*0.5 + (float)read_adc(4)*0.5; // acc y
-  //Accel_Vector[2]=Accel_Vector[2]*0.5 + (float)read_adc(5)*0.5; // acc z
+  Accel_Vector[0]=Accel_Vector[0]*0.6 + (float)read_adc(3)*0.4; // acc x
+  Accel_Vector[1]=Accel_Vector[1]*0.6 + (float)read_adc(4)*0.4; // acc y
+  Accel_Vector[2]=Accel_Vector[2]*0.6 + (float)read_adc(5)*0.4; // acc z
   
   Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);//adding integrator
   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]);//adding proportional
@@ -167,4 +161,66 @@ void Euler_angles(void)
     yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
   #endif
 }
+
+
+// VECTOR FUNCTIONS
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+  float op=0;
+  
+  for(int c=0; c<3; c++)
+  {
+  op+=vector1[c]*vector2[c];
+  }
+  
+  return op; 
+}
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar. 
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+  for(int c=0; c<3; c++)
+  {
+   vectorOut[c]=vectorIn[c]*scale2; 
+  }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+  for(int c=0; c<3; c++)
+  {
+     vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+  }
+}
+
+/********* MATRIX FUNCTIONS *****************************************/
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+  float op[3]; 
+  for(int x=0; x<3; x++)
+  {
+    for(int y=0; y<3; y++)
+    {
+      for(int w=0; w<3; w++)
+      {
+       op[w]=a[x][w]*b[w][y];
+      } 
+      mat[x][y]=0;
+      mat[x][y]=op[0]+op[1]+op[2];
+      
+      float test=mat[x][y];
+    }
+  }
+}
+
 

Arducopter/SerialCom.pde

@@ -132,15 +132,15 @@ void readSerialCommand() {
       break;
     case 'Y': // Initialize EEPROM with default values
       KP_QUAD_ROLL = 1.8;
-      KD_QUAD_ROLL = 0.48;
-      KI_QUAD_ROLL = 0.40;
+      KD_QUAD_ROLL = 0.4; //0.48
+      KI_QUAD_ROLL = 0.30; //0.4
       KP_QUAD_PITCH = 1.8;
-      KD_QUAD_PITCH = 0.48;
-      KI_QUAD_PITCH = 0.40;
+      KD_QUAD_PITCH = 0.4; //0.48
+      KI_QUAD_PITCH = 0.30; //0.4
       KP_QUAD_YAW = 3.6;
       KD_QUAD_YAW = 1.2;
       KI_QUAD_YAW = 0.15;
-      KD_QUAD_COMMAND_PART = 4.0;
+      KD_QUAD_COMMAND_PART = 0.0;
       KP_GPS_ROLL = 0.012;
       KD_GPS_ROLL = 0.005;
       KI_GPS_ROLL = 0.004;
@@ -157,9 +157,9 @@ void readSerialCommand() {
       gyro_offset_roll = 1659;
       gyro_offset_pitch = 1618;
       gyro_offset_yaw = 1673;
-      Kp_ROLLPITCH = 0.002;
-      Ki_ROLLPITCH = 0.0000005;
-      Kp_YAW = 1.5;
+      Kp_ROLLPITCH = 0.0014;
+      Ki_ROLLPITCH = 0.00000015;
+      Kp_YAW = 1.2;
       Ki_YAW = 0.00005;
       GEOG_CORRECTION_FACTOR = 0.87;
       MAGNETOMETER = 0;

Arducopter/Vector.pde

@@ -1,60 +0,0 @@
-//Computes the dot product of two vectors
-float Vector_Dot_Product(float vector1[3],float vector2[3])
-{
-  float op=0;
-  
-  for(int c=0; c<3; c++)
-  {
-  op+=vector1[c]*vector2[c];
-  }
-  
-  return op; 
-}
-
-//Computes the cross product of two vectors
-void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
-{
-  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
-  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
-  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
-}
-
-//Multiply the vector by a scalar. 
-void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
-{
-  for(int c=0; c<3; c++)
-  {
-   vectorOut[c]=vectorIn[c]*scale2; 
-  }
-}
-
-void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
-{
-  for(int c=0; c<3; c++)
-  {
-     vectorOut[c]=vectorIn1[c]+vectorIn2[c];
-  }
-}
-
-/********* MATRIX FUNCTIONS *****************************************/
-//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
-void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
-{
-  float op[3]; 
-  for(int x=0; x<3; x++)
-  {
-    for(int y=0; y<3; y++)
-    {
-      for(int w=0; w<3; w++)
-      {
-       op[w]=a[x][w]*b[w][y];
-      } 
-      mat[x][y]=0;
-      mat[x][y]=op[0]+op[1]+op[2];
-      
-      float test=mat[x][y];
-    }
-  }
-}
-
-