arduimu at higher integration rate; returns corrected gyro values

conf/modules/ins_arduimu_basic.xml

@@ -6,7 +6,7 @@
   </header>
   <init fun="ArduIMU_init()"/>
   <periodic fun="ArduIMU_periodic()" freq="60"/>
-  <periodic fun="ArduIMU_periodicGPS()" freq="8"/>
+  <periodic fun="ArduIMU_periodicGPS()" freq="4"/>
   <event fun="ArduIMU_event()"/>
   <makefile target="ap">
     <file name="ins_arduimu_basic.c"/>

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/ADC.pde

@@ -31,29 +31,9 @@ float read_adc(int select)
   float temp;
   if (SENSOR_SIGN[select]<0){
     temp = (AN_OFFSET[select]-AN[select]);
-    if (abs(temp)>900) {
-#if PRINT_DEBUG != 0
-    Serial.print("!!!ADC:1,VAL:");
-    Serial.print (temp);
-    Serial.println("***");
-#endif
-#if PERFORMANCE_REPORTING == 1
-    adc_constraints++;   
-#endif 
-    }
     return constrain(temp,-900,900);             //Throw out nonsensical values
   } else {
     temp = (AN[select]-AN_OFFSET[select]); 
-    if (abs(temp)>900) {
-#if PRINT_DEBUG != 0
-    Serial.print("!!!ADC:2,VAL:");
-    Serial.print (temp);
-    Serial.println("***");
-#endif    
-#if PERFORMANCE_REPORTING == 1
-    adc_constraints++; 
-#endif
-    } 
     return constrain(temp,-900,900);
   }
 }
@@ -61,8 +41,8 @@ float read_adc(int select)
 //Activating the ADC interrupts. 
 void Analog_Init(void)
 {
- ADCSRA|=(1<<ADIE)|(1<<ADEN);
- ADCSRA|= (1<<ADSC);
+  ADCSRA|=(1<<ADIE)|(1<<ADEN);
+  ADCSRA|= (1<<ADSC);
 }
 
 //
@@ -80,8 +60,8 @@ ISR(ADC_vect)
   high = ADCH;
 
   if(analog_count[MuxSel]<63) {
-        analog_buffer[MuxSel] += (high << 8) | low;   // cumulate analog values
-        analog_count[MuxSel]++;
+    analog_buffer[MuxSel] += (high << 8) | low;   // cumulate analog values
+    analog_count[MuxSel]++;
   }
   MuxSel++;
   MuxSel &= 0x07;   //if(MuxSel >=8) MuxSel=0;

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/DCM.pde

@@ -21,27 +21,13 @@ void Normalize(void)
     renorm= .5 * (3-renorm);                                                 //eq.21
   } else if (renorm < 100.0f && renorm > 0.01f) {
     renorm= 1. / sqrt(renorm);
-#if PERFORMANCE_REPORTING == 1  
+#if PERFORMANCE_REPORTING == 1
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:1,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   } else {
     problem = TRUE;
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:1,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   }
   Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
@@ -53,25 +39,11 @@ void Normalize(void)
     renorm= 1. / sqrt(renorm);  
 #if PERFORMANCE_REPORTING == 1    
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:2,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   } else {
     problem = TRUE;
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:2,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   }
   Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
@@ -83,23 +55,11 @@ void Normalize(void)
     renorm= 1. / sqrt(renorm);   
 #if PERFORMANCE_REPORTING == 1 
     renorm_sqrt_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???SQT:3,RNM:");
-    Serial.print (renorm);
-    Serial.print (",ERR:");
-    Serial.print (error);
-    Serial.println("***");    
 #endif
   } else {
     problem = TRUE;  
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
-#endif
-#if PRINT_DEBUG != 0
-    Serial.print("???PRB:3,RNM:");
-    Serial.print (renorm);
-    Serial.println("***");    
 #endif
   }
   Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
@@ -170,23 +130,18 @@ void Drift_correction(void)
   Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
   if (Integrator_magnitude > ToRad(300)) {
     Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
-#if PRINT_DEBUG != 0
-    Serial.print("!!!INT:1,MAG:");
-    Serial.print (ToDeg(Integrator_magnitude));
-
-    Serial.println("***");    
-#endif
   }
 
 }
+
 /**************************************************/
 void Accel_adjust(void)
 {
   Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
   Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY 
 }
-/**************************************************/
 
+/**************************************************/
 void Matrix_update(void)
 {
   Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
@@ -202,7 +157,6 @@ void Matrix_update(void)
 
   Accel_adjust();    //Remove centrifugal acceleration.
 
-#if OUTPUTMODE==1         
   Update_Matrix[0][0]=0;
   Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
   Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
@@ -212,17 +166,6 @@ void Matrix_update(void)
   Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
   Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
   Update_Matrix[2][2]=0;
-#else                    // Uncorrected data (no drift correction)
-  Update_Matrix[0][0]=0;
-  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
-  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
-  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
-  Update_Matrix[1][1]=0;
-  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
-  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
-  Update_Matrix[2][2]=0;
-#endif
 
   Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
 
@@ -235,16 +178,11 @@ void Matrix_update(void)
   }
 }
 
+/**************************************************/
 void Euler_angles(void)
 {
-#if (OUTPUTMODE==2)         // Only accelerometer info (debugging purposes)
-  roll = atan2(Accel_Vector[1],Accel_Vector[2]);    // atan2(acc_y,acc_z)
-  pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
-  yaw = 0;
-#else
   pitch = -asin(DCM_Matrix[2][0]);
   roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
   yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
-#endif
 }
 

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/Output.pde

@@ -1,24 +1,28 @@
 
 //*****I2C Output************************************************************
-
-void requestEvent(){
-#if PRINT_DEBUG != 0
-  Serial.println("Sending IMU Data");
-#endif
-
+void fill_I2C_message() {
   // Message Array : Roll; Pitch; YaW; GyroX; GyroY; GyroZ; ACCX; ACCY; ACCZ
   // Float Number is multipited with 10000 and converted to an Integer, for sending via I2C.
   // Resolution for angles: 12, rates: 12, accel:10 (from pprza BFP math lib)
   I2C_Message_ar[0] = int(roll*(1<<12));
   I2C_Message_ar[1] = int(pitch*(1<<12));
   I2C_Message_ar[2] = int(yaw*(1<<12));
-  I2C_Message_ar[3] = int(Gyro_Vector[0]*(1<<12));
-  I2C_Message_ar[4] = int(Gyro_Vector[1]*(1<<12));
-  I2C_Message_ar[5] = int(Gyro_Vector[2]*(1<<12));
+  I2C_Message_ar[3] = int(Omega_Vector[0]*(1<<12));
+  I2C_Message_ar[4] = int(Omega_Vector[1]*(1<<12));
+  I2C_Message_ar[5] = int(Omega_Vector[2]*(1<<12));
   I2C_Message_ar[6] = int((9.81*Accel_Vector[0]/GRAVITY)*(1<<10));
   I2C_Message_ar[7] = int((9.81*Accel_Vector[1]/GRAVITY)*(1<<10));
   I2C_Message_ar[8] = int((9.81*Accel_Vector[2]/GRAVITY)*(1<<10));
 
+}
+
+void requestEvent(){
+#if PRINT_DEBUG != 0
+  Serial.println("Sending IMU Data");
+#endif
+
+  fill_I2C_message();
+
   byte* pointer;
   pointer = (byte*) &I2C_Message_ar;
   Wire.send(pointer, 18);
@@ -51,6 +55,7 @@ void receiveEvent(int howMany){
 void printdata(void){    
 
 #if PRINT_I2C_MSG == 1
+  fill_I2C_message();
   Serial.print("Time ");
   Serial.print(millis());
   Serial.print(";  Roll ");
@@ -89,8 +94,7 @@ void printdata(void){
   Serial.print (",AN4:");
   Serial.print(read_adc(4));
   Serial.print (",AN5:");
-  Serial.print(read_adc(5));
-  Serial.print (",");
+  Serial.println(read_adc(5));
 #endif
 
 #if PRINT_DCM == 1
@@ -111,8 +115,7 @@ void printdata(void){
   Serial.print (",EX7:");
   Serial.print(convert_to_dec(DCM_Matrix[2][1]));
   Serial.print (",EX8:");
-  Serial.print(convert_to_dec(DCM_Matrix[2][2]));
-  Serial.print (",");
+  Serial.println(convert_to_dec(DCM_Matrix[2][2]));
 #endif
 
 #if PRINT_EULER == 1
@@ -123,8 +126,7 @@ void printdata(void){
   Serial.print(",YAW:");
   Serial.print(ToDeg(yaw));
   Serial.print(",IMUH:");
-  Serial.print((imu_health>>8)&0xff);
-  Serial.print (",");
+  Serial.println((imu_health>>8)&0xff);
 #endif
 
 #if PRINT_GPS == 1
@@ -141,6 +143,7 @@ void printdata(void){
     gps_messages_sent++;
 #endif
   }
+  Serial.println("");
 #endif
 
 }
@@ -174,7 +177,7 @@ void printPerfData(long time)
   Serial.print(gps_messages_sent,DEC);
   Serial.print(",imu:");
   Serial.print((imu_health>>8),DEC);
-  Serial.print(",***");
+  Serial.println(",***");
 
   // Reset counters
   mainLoop_count = 0;

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_basic/arduimu_basic.pde

@@ -30,9 +30,6 @@
 /*For debugging propurses*/
 #define PRINT_DEBUG 0   //Will print Debug messages
 
-//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
-#define OUTPUTMODE 1
-
 #define PRINT_I2C_MSG 0 //Will print the I2C output buffer
 #define PRINT_DCM 0     //Will print the whole direction cosine matrix
 #define PRINT_ANALOGS 0 //Will print the analog raw data
@@ -49,7 +46,7 @@ int I2C_Message_ar[9];
 
 
 // *** NOTE!   To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here)
-#define PRINT_BINARY 0   //Will print binary message and suppress ASCII messages (above)
+#define PRINT_BINARY 1   //Will print binary message and suppress ASCII messages (above)
 
 // *** NOTE!   Performance reporting is only supported for Ublox.  Set to 0 for others
 #define PERFORMANCE_REPORTING 1  //Will include performance reports in the binary output ~ 1/2 min
@@ -70,10 +67,9 @@ int I2C_Message_ar[9];
 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 
 // LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
-// Tested values : 0.96,0.96,0.94
-#define Gyro_Gain_X 0.92 //X axis Gyro gain
-#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
-#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
+#define Gyro_Gain_X 1.0 //X axis Gyro gain
+#define Gyro_Gain_Y 1.0 //Y axis Gyro gain
+#define Gyro_Gain_Z 1.0 //Z axis Gyro gain
 #define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
 #define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
 #define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second
@@ -84,7 +80,7 @@ int I2C_Message_ar[9];
 //#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
 #define Ki_YAW 0.00005
 
-#define ADC_WARM_CYCLES 75
+#define ADC_WARM_CYCLES 100
 
 #define FALSE 0
 #define TRUE 1
@@ -268,7 +264,10 @@ void loop() //Main Loop
 {
   timeNow = millis();
 
-  if((timeNow-timer)>=20)  // Main loop runs at 50Hz
+  // 20 -> Main loop runs at 50Hz
+  // 5 -> Main loop runs at 200Hz
+  // Max measured duty time around 3ms
+  if((timeNow-timer)>=5)
   {
     timer_old = timer;
     timer = timeNow;

sw/airborne/modules/ins/ins_arduimu_basic.c

@@ -132,7 +132,7 @@ void ArduIMU_event( void ) {
     estimator_p = arduimu_rates.p;
     ardu_ins_trans.status = I2CTransDone;
 
-    RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
+    //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
     RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
     RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
   }