basic version of the arduimu firmware

returns angles rates and accels
gps velocities and status are send
no baro or magneto support

conf/modules/ins_arduimu.xml

@@ -2,14 +2,14 @@
 
 <module name="ins_ArduIMU" dir="ins">
   <header>
-    <file name="ins_arduimu_modified.h"/>
+    <file name="ins_arduimu_basic.h"/>
   </header>
   <init fun="ArduIMU_init()"/>
   <periodic fun="ArduIMU_periodic()" freq="60"/>		<!-- 15 ist soll -->
   <periodic fun="ArduIMU_periodicGPS()" freq="8"/>       <!--  8 ist soll -->
   <event fun="ArduIMU_event()"/>
   <makefile target="ap">
-    <file name="ins_arduimu_modified.c"/>
+    <file name="ins_arduimu_basic.c"/>
   </makefile>
   <makefile target="sim">
     <file_arch name="ins_arduimu.c"/>

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

@@ -5,17 +5,17 @@ void Normalize(void)
   float temporary[3][3];
   float renorm=0;
   boolean problem=FALSE;
-  
+
   error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
 
   Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
   Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
-  
+
   Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
   Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
-  
+
   Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
-  
+
   renorm= Vector_Dot_Product(&temporary[0][0],&temporary[0][0]); 
   if (renorm < 1.5625f && renorm > 0.64f) {
     renorm= .5 * (3-renorm);                                                 //eq.21
@@ -36,7 +36,7 @@ void Normalize(void)
 #if PERFORMANCE_REPORTING == 1
     renorm_blowup_count++;
 #endif
-	#if PRINT_DEBUG != 0
+#if PRINT_DEBUG != 0
     Serial.print("???PRB:1,RNM:");
     Serial.print (renorm);
     Serial.print (",ERR:");
@@ -44,8 +44,8 @@ void Normalize(void)
     Serial.println("***");    
 #endif
   }
-      Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
-  
+
   renorm= Vector_Dot_Product(&temporary[1][0],&temporary[1][0]); 
   if (renorm < 1.5625f && renorm > 0.64f) {
     renorm= .5 * (3-renorm);                                                 //eq.21
@@ -75,7 +75,7 @@ void Normalize(void)
 #endif
   }
   Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
-  
+
   renorm= Vector_Dot_Product(&temporary[2][0],&temporary[2][0]); 
   if (renorm < 1.5625f && renorm > 0.64f) {
     renorm= .5 * (3-renorm);                                                 //eq.21
@@ -103,18 +103,18 @@ void Normalize(void)
 #endif
   }
   Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
-  
+
   if (problem) {                // Our solution is blowing up and we will force back to initial condition.  Hope we are not upside down!
-      DCM_Matrix[0][0]= 1.0f;
+    DCM_Matrix[0][0]= 1.0f;
-      DCM_Matrix[0][1]= 0.0f;
+    DCM_Matrix[0][1]= 0.0f;
-      DCM_Matrix[0][2]= 0.0f;
+    DCM_Matrix[0][2]= 0.0f;
-      DCM_Matrix[1][0]= 0.0f;
+    DCM_Matrix[1][0]= 0.0f;
-      DCM_Matrix[1][1]= 1.0f;
+    DCM_Matrix[1][1]= 1.0f;
-      DCM_Matrix[1][2]= 0.0f;
+    DCM_Matrix[1][2]= 0.0f;
-      DCM_Matrix[2][0]= 0.0f;
+    DCM_Matrix[2][0]= 0.0f;
-      DCM_Matrix[2][1]= 0.0f;
+    DCM_Matrix[2][1]= 0.0f;
-      DCM_Matrix[2][2]= 1.0f;
+    DCM_Matrix[2][2]= 1.0f;
-      problem = FALSE;  
+    problem = FALSE;  
   }
 }
 
@@ -122,15 +122,13 @@ void Normalize(void)
 void Drift_correction(void)
 {
   //Compensation the Roll, Pitch and Yaw drift. 
-  float mag_heading_x;
-  float mag_heading_y;
   static float Scaled_Omega_P[3];
   static float Scaled_Omega_I[3];
   float Accel_magnitude;
   float Accel_weight;
   float Integrator_magnitude;
   float tempfloat;
-  
+
   //*****Roll and Pitch***************
 
   // Calculate the magnitude of the accelerometer vector
@@ -139,48 +137,35 @@ void Drift_correction(void)
   // Dynamic weighting of accelerometer info (reliability filter)
   // 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);  //  
-  
+
-  #if PERFORMANCE_REPORTING == 1
+#if PERFORMANCE_REPORTING == 1
-    tempfloat = ((Accel_weight - 0.5) * 256.0f);    //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
+  tempfloat = ((Accel_weight - 0.5) * 256.0f);    //amount added was determined to give imu_health a time constant about twice the time constant of the roll/pitch drift correction
-    imu_health += tempfloat;
+  imu_health += tempfloat;
-    imu_health = constrain(imu_health,129,65405);
+  imu_health = constrain(imu_health,129,65405);
-  #endif
+#endif
-  
+
   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);
   Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
-  
+
   //*****YAW***************
-  
+
-  #if USE_MAGNETOMETER==1 
+  // Use GPS Ground course to correct yaw gyro drift
-    // We make the gyro YAW drift correction based on compass magnetic heading
-    mag_heading_x = cos(MAG_Heading);
-    mag_heading_y = sin(MAG_Heading);
-    errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
-    Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
-    
-    Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
-    Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
-    
-    Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
-    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I   
-  #else  // Use GPS Ground course to correct yaw gyro drift
   if(ground_speed>=SPEEDFILT)
   {
     COGX = cos(ToRad(ground_course));
     COGY = sin(ToRad(ground_course));
     errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX);  //Calculating YAW error
     Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
-  
+
     Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
     Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
-  
+
     Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
     Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I   
   }
-  #endif
   //  Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
   Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
   if (Integrator_magnitude > ToRad(300)) {
@@ -192,14 +177,13 @@ void Drift_correction(void)
     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[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 
+  Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY 
 }
 /**************************************************/
 
@@ -208,17 +192,17 @@ void Matrix_update(void)
   Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
   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
-  
+
   Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
   Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
 
   Accel_adjust();    //Remove centrifugal acceleration.
-  
+
- #if OUTPUTMODE==1         
+#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
@@ -228,7 +212,7 @@ 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)
+#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
@@ -238,7 +222,7 @@ void Matrix_update(void)
   Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
   Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
   Update_Matrix[2][2]=0;
- #endif
+#endif
 
   Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
 
@@ -253,14 +237,14 @@ void Matrix_update(void)
 
 void Euler_angles(void)
 {
-  #if (OUTPUTMODE==2)         // Only accelerometer info (debugging purposes)
+#if (OUTPUTMODE==2)         // Only accelerometer info (debugging purposes)
-    roll = atan2(Accel_Vector[1],Accel_Vector[2]);    // atan2(acc_y,acc_z)
+  roll = atan2(Accel_Vector[1],Accel_Vector[2]);    // atan2(acc_y,acc_z)
-    pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
+  pitch = -asin((Accel_Vector[0])/(double)GRAVITY); // asin(acc_x)
-    yaw = 0;
+  yaw = 0;
-  #else
+#else
-    pitch = -asin(DCM_Matrix[2][0]);
+  pitch = -asin(DCM_Matrix[2][0]);
-    roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
-    yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
-  #endif
+#endif
 }
 

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

@@ -0,0 +1,64 @@
+/****************************************************************
+ * Parse GPS data from a Paparazzi autopilot
+ ****************************************************************/
+ // Code from Jordi, modified by Jose, modified by Gautier
+
+
+void init_gps(void)
+{
+  //Serial.begin(38400); 
+  pinMode(2,OUTPUT); //Serial Mux
+  digitalWrite(2,HIGH); //Serial Mux
+} 
+
+/****************************************************************
+ * 
+ ****************************************************************/
+void parse_pprz_gps() {
+
+#if PERFORMANCE_REPORTING == 1
+  gps_pos_fix_count++;
+#endif
+
+  speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0;    // m/s  0,1,2,3
+  ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D  4,5,6,7
+  ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D  8,9,10,11
+  stGpsFix = Paparazzi_GPS_buffer[12];
+  stFlags = Paparazzi_GPS_buffer[13];
+
+  if((stGpsFix >= 0x03) && (stFlags&0x01)) {
+    gpsFix = 0; //valid position
+    digitalWrite(6,HIGH);  //Turn LED when gps is fixed. 
+    GPS_timer = DIYmillis(); //Restarting timer...
+  }
+  else {
+    gpsFix = 1; //invalid position
+    digitalWrite(6,LOW);
+  }
+
+  if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
+
+}
+
+
+/****************************************************************
+ * Join 4 bytes into a long
+ ****************************************************************/
+int32_t join_4_bytes(byte Buffer[])
+{
+  longUnion.byte[0] = *Buffer;
+  longUnion.byte[1] = *(Buffer+1);
+  longUnion.byte[2] = *(Buffer+2);
+  longUnion.byte[3] = *(Buffer+3);
+  return(longUnion.dword);
+}
+
+/****************************************************************
+ * 
+void checksum(byte ubx_data)
+{
+  ck_a+=ubx_data;
+  ck_b+=ck_a; 
+}
+
+ ****************************************************************/

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

@@ -0,0 +1,200 @@
+
+//*****I2C Output************************************************************
+
+void requestEvent(){
+#if PRINT_DEBUG != 0
+  Serial.println("Sending IMU Data");
+#endif
+
+  // 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[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));
+
+  byte* pointer;
+  pointer = (byte*) &I2C_Message_ar;
+  Wire.send(pointer, 18);
+
+}
+
+//******** GPS Data from Paparazzi AP ******************************************
+
+void receiveEvent(int howMany){
+#if PRINT_DEBUG != 0
+  Serial.print("Receiving GPS Bytes : "); 
+  Serial.println(howMany);
+#endif
+
+  for(int i=0; i < howMany; i++){
+    Paparazzi_GPS_buffer[i]=Wire.receive();
+  }
+
+  parse_pprz_gps();               // Parse new GPS packet...
+  GPS_timer=DIYmillis();         //Restarting timer...
+
+  gpsDataReady=1;
+
+}
+
+
+//*************************************************************************************************************
+
+
+void printdata(void){    
+
+#if PRINT_I2C_MSG == 1
+  Serial.print("Time ");
+  Serial.print(millis());
+  Serial.print(";  Roll ");
+  Serial.print(I2C_Message_ar[0]);
+  Serial.print(";  Pitch ");
+  Serial.print(I2C_Message_ar[1]);
+  Serial.print(";  YaW ");
+  Serial.print(I2C_Message_ar[2]);
+  Serial.print(";  GyroX ");
+  Serial.print(I2C_Message_ar[3]);
+  Serial.print(";  GyroY ");
+  Serial.print(I2C_Message_ar[4]);
+  Serial.print(";  GyroZ ");
+  Serial.print(I2C_Message_ar[5]);
+  Serial.print(";  ACCX ");
+  Serial.print(I2C_Message_ar[6]);
+  Serial.print(";  ACCY ");
+  Serial.print(I2C_Message_ar[7]);
+  Serial.print(";  ACCZ ");
+  Serial.println(I2C_Message_ar[8]);
+#endif
+
+  //Serial.print("!!!VER:");
+  //Serial.print(SOFTWARE_VER);  //output the software version
+  //Serial.print(",");
+
+#if PRINT_ANALOGS == 1
+  Serial.print("AN0:");
+  Serial.print(read_adc(0)); //Reversing the sign. 
+  Serial.print(",AN1:");
+  Serial.print(read_adc(1));
+  Serial.print(",AN2:");
+  Serial.print(read_adc(2));  
+  Serial.print(",AN3:");
+  Serial.print(read_adc(3));
+  Serial.print (",AN4:");
+  Serial.print(read_adc(4));
+  Serial.print (",AN5:");
+  Serial.print(read_adc(5));
+  Serial.print (",");
+#endif
+
+#if PRINT_DCM == 1
+  Serial.print ("EX0:");
+  Serial.print(convert_to_dec(DCM_Matrix[0][0]));
+  Serial.print (",EX1:");
+  Serial.print(convert_to_dec(DCM_Matrix[0][1]));
+  Serial.print (",EX2:");
+  Serial.print(convert_to_dec(DCM_Matrix[0][2]));
+  Serial.print (",EX3:");
+  Serial.print(convert_to_dec(DCM_Matrix[1][0]));
+  Serial.print (",EX4:");
+  Serial.print(convert_to_dec(DCM_Matrix[1][1]));
+  Serial.print (",EX5:");
+  Serial.print(convert_to_dec(DCM_Matrix[1][2]));
+  Serial.print (",EX6:");
+  Serial.print(convert_to_dec(DCM_Matrix[2][0]));
+  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 (",");
+#endif
+
+#if PRINT_EULER == 1
+  Serial.print("RLL:");
+  Serial.print(ToDeg(roll));
+  Serial.print(",PCH:");
+  Serial.print(ToDeg(pitch));
+  Serial.print(",YAW:");
+  Serial.print(ToDeg(yaw));
+  Serial.print(",IMUH:");
+  Serial.print((imu_health>>8)&0xff);
+  Serial.print (",");
+#endif
+
+#if PRINT_GPS == 1
+  if(gpsFixnew==1) {
+    gpsFixnew=0;
+    Serial.print("COG:");
+    Serial.print((ground_course));
+    Serial.print(",SOG:");
+    Serial.print(ground_speed);
+    Serial.print(",FIX:");
+    Serial.print((int)gpsFix);
+    Serial.print (",");
+#if PERFORMANCE_REPORTING == 1
+    gps_messages_sent++;
+#endif
+  }
+#endif
+
+}
+
+void printPerfData(long time)
+{
+  Serial.print("PPP");
+  Serial.print("pTm:");
+  Serial.print(time,DEC);
+  Serial.print(",mLc:");
+  Serial.print(mainLoop_count,DEC);
+  Serial.print(",DtM:");
+  Serial.print(G_Dt_max,DEC);
+  Serial.print(",gsc:");
+  Serial.print(gyro_sat_count,DEC);
+  Serial.print(",adc:");
+  Serial.print(adc_constraints,DEC);
+  Serial.print(",rsc:");
+  Serial.print(renorm_sqrt_count,DEC);
+  Serial.print(",rbc:");
+  Serial.print(renorm_blowup_count,DEC);
+  Serial.print(",gpe:");
+  Serial.print(gps_payload_error_count,DEC);
+  Serial.print(",gce:");
+  Serial.print(gps_checksum_error_count,DEC);
+  Serial.print(",gpf:");
+  Serial.print(gps_pos_fix_count,DEC);
+  Serial.print(",gnf:");
+  Serial.print(gps_nav_fix_count,DEC);
+  Serial.print(",gms:");
+  Serial.print(gps_messages_sent,DEC);
+  Serial.print(",imu:");
+  Serial.print((imu_health>>8),DEC);
+  Serial.print(",***");
+
+  // Reset counters
+  mainLoop_count = 0;
+  G_Dt_max  = 0;
+  gyro_sat_count = 0;
+  adc_constraints = 0;
+  renorm_sqrt_count = 0;
+  renorm_blowup_count = 0;
+  gps_payload_error_count = 0;
+  gps_checksum_error_count = 0;
+  gps_pos_fix_count = 0;
+  gps_nav_fix_count = 0;
+  gps_messages_sent = 0;
+
+
+}
+
+
+long convert_to_dec(float x)
+{
+  return x*10000000;
+}
+

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/Compass.pde

@@ -1,74 +0,0 @@
-/* ******************************************************* */
-/* I2C HMC5843 magnetometer                                */
-/* ******************************************************* */
-
-// Local magnetic declination
-// I use this web : http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
-#define MAGNETIC_DECLINATION -6.0    // not used now -> magnetic bearing
-
-int CompassAddress = 0x1E;  //0x3C //0x3D;  //(0x42>>1);
-
-void I2C_Init()
-{
-  Wire.begin();
-}
-
-
-void Compass_Init()
-{
-  Wire.beginTransmission(CompassAddress);
-  Wire.send(0x02); 
-  Wire.send(0x00);   // Set continouos mode (default to 10Hz)
-  Wire.endTransmission(); //end transmission
-}
-
-void Read_Compass()
-{
-  int i = 0;
-  byte buff[6];
- 
-  Wire.beginTransmission(CompassAddress); 
-  Wire.send(0x03);        //sends address to read from
-  Wire.endTransmission(); //end transmission
-  
-  //Wire.beginTransmission(CompassAddress); 
-  Wire.requestFrom(CompassAddress, 6);    // request 6 bytes from device
-  while(Wire.available())   // ((Wire.available())&&(i<6))
-  { 
-    buff[i] = Wire.receive();  // receive one byte
-    i++;
-  }
-  Wire.endTransmission(); //end transmission
-  
-  if (i==6)  // All bytes received?
-    {
-    // MSB byte first, then LSB, X,Y,Z
-    magnetom_x = SENSOR_SIGN[6]*((((int)buff[2]) << 8) | buff[3]);    // X axis (internal sensor y axis)
-    magnetom_y = SENSOR_SIGN[7]*((((int)buff[0]) << 8) | buff[1]);    // Y axis (internal sensor x axis)
-    magnetom_z = SENSOR_SIGN[8]*((((int)buff[4]) << 8) | buff[5]);    // Z axis
-    }
-  else
-    Serial.println("!ERR: Mag data");
-}
-
-
-void Compass_Heading()
-{
-  float MAG_X;
-  float MAG_Y;
-  float cos_roll;
-  float sin_roll;
-  float cos_pitch;
-  float sin_pitch;
-  
-  cos_roll = cos(roll);
-  sin_roll = sin(roll);
-  cos_pitch = cos(pitch);
-  sin_pitch = sin(pitch);
-  // Tilt compensated Magnetic filed X:
-  MAG_X = magnetom_x*cos_pitch+magnetom_y*sin_roll*sin_pitch+magnetom_z*cos_roll*sin_pitch;
-  // Tilt compensated Magnetic filed Y:
-  MAG_Y = magnetom_y*cos_roll-magnetom_z*sin_roll;
-  // Magnetic Heading
-  MAG_Heading = atan2(-MAG_Y,MAG_X);
-}

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/GPS_UBLOX.pde

@@ -1,120 +0,0 @@
-#if GPS_PROTOCOL == 3
-
-/****************************************************************
- * Here you have all the parsing stuff for uBlox
- ****************************************************************/
- // Code from Jordi, modified by Jose
- 
- //You have to disable all the other string, only leave this ones:
- 
- //NAV-POSLLH Geodetic Position Solution, PAGE 66 of datasheet
- //NAV-VELNED Velocity Solution in NED, PAGE 71 of datasheet
- //NAV-STATUS Receiver Navigation Status, PAGE 67 of datasheet
- //NAV-SOL Navigation Solution Information, PAGE 72 of datasheet
- 
- 
- // Baud Rate:38400
-
-/* 
- GPSfix Type 
- - 0x00 = no fix
- - 0x01 = dead reckonin
- - 0x02 = 2D-fix
- - 0x03 = 3D-fix
- - 0x04 = GPS + dead re
- - 0x05 = Time only fix
- - 0x06..0xff = reserved*/
- 
- //Luckly uBlox has internal EEPROM so all the settings you change will remain forever.  Not like the SIRF modules!
- 
-void init_gps(void)
-{
-  //Serial.begin(38400); 
-  pinMode(2,OUTPUT); //Serial Mux
-  digitalWrite(2,HIGH); //Serial Mux
-} 
-
-/****************************************************************
- * 
- ****************************************************************/
-
-void decode_gps(void){ 
-
-  if(DIYmillis() - GPS_timer > 2000){
-    digitalWrite(6, LOW);  //If we don't receive any byte in two seconds turn off gps fix LED...
-    debug_print("Yeah, your GPS is disconnected"); 
-    gpsFix=1;
-  } 
-}
-
-/****************************************************************
- * 
- ****************************************************************/
-void parse_ubx_gps(){
-
-#if PERFORMANCE_REPORTING == 1
-  gps_pos_fix_count++;
-#endif
-
-  speed_3d = (float)join_4_bytes(&Paparazzi_GPS_buffer[0])/100.0;    // m/s  0,1,2,3
-  ground_speed = (float)join_4_bytes(&Paparazzi_GPS_buffer[4])/100.0; // Ground speed 2D  4,5,6,7
-  ground_course = (float)join_4_bytes(&Paparazzi_GPS_buffer[8])/100000.0; // Heading 2D  8,9,10,11
-  alt = join_4_bytes(&Paparazzi_GPS_buffer[12]);                      // 12,13,14,15
-  alt_MSL = join_4_bytes(&Paparazzi_GPS_buffer[16]);                  // 16,17,18,19
-  stGpsFix = Paparazzi_GPS_buffer[20];
-  stFlags = Paparazzi_GPS_buffer[21];
-
-  if((stGpsFix >= 0x03)&&(stFlags&0x01)){
-    gpsFix=0; //valid position
-    digitalWrite(6,HIGH);  //Turn LED when gps is fixed. 
-    GPS_timer=DIYmillis(); //Restarting timer...
-  }
-  else{
-    gpsFix=1; //invalid position
-    digitalWrite(6,LOW);
-  }
-
-  if (ground_speed > SPEEDFILT && gpsFix==0) gc_offset = ground_course - ToDeg(yaw);
-
-#if 0           
-  // Serial.print("Time von Arduino ;");
-  // Serial.print(millis());
-  Serial.print("alt ;");
-  Serial.print(alt);
-  Serial.print("; alt_MSL: ;");
-  Serial.print(alt_MSL);
-  Serial.print("; speed_3d ;");
-  Serial.print(speed_3d);
-  Serial.print("; ground_speed ;");
-  Serial.print(ground_speed);
-  Serial.print("; ground_course ;");
-  Serial.print(ground_course);
-#endif
-
-}
-
-
-/****************************************************************
- * 
- ****************************************************************/
- // Join 4 bytes into a long
-int32_t join_4_bytes(byte Buffer[])
-{
-  longUnion.byte[0] = *Buffer;
-  longUnion.byte[1] = *(Buffer+1);
-  longUnion.byte[2] = *(Buffer+2);
-  longUnion.byte[3] = *(Buffer+3);
-  return(longUnion.dword);
-}
-
-/****************************************************************
- * 
- ****************************************************************/
-void checksum(byte ubx_data)
-{
-  ck_a+=ubx_data;
-  ck_b+=ck_a; 
-}
-
-
-#endif

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

@@ -1,419 +0,0 @@
-
-
-//*****I2C Output************************************************************
-// PRINT_I2C_Data
-// #if PRINT_BINARY != 1  //Print either Ascii or binary messages
-  
-void requestEvent(){
-  // 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[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));
-
-  byte* pointer;
-  pointer = (byte*) &I2C_Message_ar;
-  Wire.send(pointer, 18);
-
-  /* Uncomment for debug on serial link */
-  
-  //Serial.println();
-  /*
-  Serial.print("Time ;");
-  Serial.print(millis());
-  Serial.print(";    Roll ;");
-  Serial.print(I2C_Message_ar[0]);
-  Serial.print(";  Pitch ;");
-  Serial.print(I2C_Message_ar[1]);
-  Serial.print(";  YaW ;");
-  Serial.print(I2C_Message_ar[2]);
-  Serial.print(";  GyroX ;");
-  Serial.print(I2C_Message_ar[3]);
-  Serial.print(";  GyroY: ;");
-  Serial.print(I2C_Message_ar[4]);
-  Serial.print(";  GyroZ ;");
-  Serial.print(I2C_Message_ar[5]);
-  Serial.print(";  ACCX ;");
-  Serial.print(I2C_Message_ar[6]);
-  Serial.print(";  ACCY: ;");
-  Serial.print(I2C_Message_ar[7]);
-  Serial.print(";  ACCZ ;");
-  Serial.println(I2C_Message_ar[8]);
-  */
-  
-}
- 
-//********GPS Data from PAPArazzi UBLOX**********************************************************************
-
-void receiveEvent(int howMany){
-  Serial.print(" How Many Bytes GPS : "); 
-  Serial.println(howMany);
-
-  for(int i=0; i < howMany; i++){
-    Paparazzi_GPS_buffer[i]=Wire.receive();
-  }
-
-  parse_ubx_gps();               // Parse new GPS packet...
-  GPS_timer=DIYmillis();         //Restarting timer...
-
-  gpsDataReady=1;
-
-}
-
-
-//*************************************************************************************************************
-
-
-void printdata(void){    
-  
-  
-#if PRINT_I2C_Data == 1
-  /*
-  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[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));
-
-  Serial.println();
-  Serial.print("Time ;");
-  Serial.print(millis());
-  Serial.print(";    Roll ;");
-  Serial.print(I2C_Message_ar[0]);
-  Serial.print(";  Pitch ;");
-  Serial.print(I2C_Message_ar[1]);
-  Serial.print(";  YaW ;");
-  Serial.print(I2C_Message_ar[2]);
-  Serial.print(";  GyroX ;");
-  Serial.print(I2C_Message_ar[3]);
-  Serial.print(";  GyroY: ;");
-  Serial.print(I2C_Message_ar[4]);
-  Serial.print(";  GyroZ ;");
-  Serial.print(I2C_Message_ar[5]);
-  Serial.print(";  ACCX ;");
-  Serial.print(I2C_Message_ar[6]);
-  Serial.print(";  ACCY: ;");
-  Serial.print(I2C_Message_ar[7]);
-  Serial.print(";  ACCZ ;");
-  Serial.println(I2C_Message_ar[8]);
-  */
-#endif
-  
-  
- 
-  
-  
-#if PRINT_BINARY != 1  //Print either Ascii or binary messages
-
-	//Serial.print("!!!VER:");
-	//Serial.print(SOFTWARE_VER);  //output the software version
-	//Serial.print(",");
-	
-	#if PRINT_ANALOGS==1
-		Serial.print("AN0:");
-		Serial.print(read_adc(0)); //Reversing the sign. 
-		Serial.print(",AN1:");
-		Serial.print(read_adc(1));
-		Serial.print(",AN2:");
-		Serial.print(read_adc(2));  
-		Serial.print(",AN3:");
-		Serial.print(read_adc(3));
-		Serial.print (",AN4:");
-		Serial.print(read_adc(4));
-		Serial.print (",AN5:");
-		Serial.print(read_adc(5));
-		Serial.print (",");
-	#endif
-      
-	#if PRINT_DCM == 1
-		Serial.print ("EX0:");
-		Serial.print(convert_to_dec(DCM_Matrix[0][0]));
-		Serial.print (",EX1:");
-		Serial.print(convert_to_dec(DCM_Matrix[0][1]));
-		Serial.print (",EX2:");
-		Serial.print(convert_to_dec(DCM_Matrix[0][2]));
-		Serial.print (",EX3:");
-		Serial.print(convert_to_dec(DCM_Matrix[1][0]));
-		Serial.print (",EX4:");
-		Serial.print(convert_to_dec(DCM_Matrix[1][1]));
-		Serial.print (",EX5:");
-		Serial.print(convert_to_dec(DCM_Matrix[1][2]));
-		Serial.print (",EX6:");
-		Serial.print(convert_to_dec(DCM_Matrix[2][0]));
-		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 (",");
-	#endif
-
-	#if PRINT_EULER == 1
-		Serial.print("RLL:");
-		Serial.print(ToDeg(roll));
-		Serial.print(",PCH:");
-		Serial.print(ToDeg(pitch));
-		Serial.print(",YAW:");
-		Serial.print(ToDeg(yaw));
-		Serial.print(",IMUH:");
-		Serial.print((imu_health>>8)&0xff);
-		Serial.print (",");
-	#endif
-      
-	#if USE_MAGNETOMETER == 1
-		Serial.print("MGX:");
-		Serial.print(magnetom_x);
-		Serial.print (",MGY:");
-		Serial.print(magnetom_y);
-		Serial.print (",MGZ:");
-		Serial.print(magnetom_z);
-		Serial.print (",MGH:");
-		Serial.print(MAG_Heading);
-		Serial.print (",");
-	#endif
-      
-	#if USE_BAROMETER == 1
-		Serial.print("Temp:");
-		Serial.print(temp_unfilt/20.0);      // Convert into degrees C
-		alti();
-		Serial.print(",Pressure: ");
-		Serial.print(press);            
-//		Serial.print(press>>2);       // Convert into Pa       
-		Serial.print(",Alt: ");
-		Serial.print(press_alt/1000);  // Original floating point full solution in meters
-		Serial.print (",");
-	#endif
-      
-	#if PRINT_GPS == 1
-		if(gpsFixnew==1) {
-			gpsFixnew=0;
-			Serial.print(",ALT:");
-			Serial.print(alt_MSL/1000);    // meters
-			Serial.print(",COG:");
-			Serial.print((ground_course));
-			Serial.print(",SOG:");
-			Serial.print(ground_speed);
-			Serial.print(",FIX:");
-			Serial.print((int)gpsFix);
-			Serial.print (",");
-			#if PERFORMANCE_REPORTING == 1
-				gps_messages_sent++;
-			#endif
-		}
-	#endif
-      
-#else
-	//  This section outputs binary data messages
-	//  Conforms to new binary message standard (12/31/09)
-	byte IMU_buffer[22];
-	int tempint;
-	int ck;
-	long templong;
-	byte IMU_ck_a=0;
-	byte IMU_ck_b=0;
-      
-	//  This section outputs the gps binary message when new gps data is available
-	if(gpsFixnew==1) {
-		#if PERFORMANCE_REPORTING == 1
-			gps_messages_sent++;
-		#endif
-		gpsFixnew=0;
-		Serial.print("DIYd");  // This is the message preamble
-		IMU_buffer[0]=0x13;
-		ck=19;
-		IMU_buffer[1] = 0x03;      
-
-		templong = lon; //Longitude *10**7 in 4 bytes
-		IMU_buffer[2]=templong&0xff;
-		IMU_buffer[3]=(templong>>8)&0xff;
-		IMU_buffer[4]=(templong>>16)&0xff;
-		IMU_buffer[5]=(templong>>24)&0xff;
-      
-		templong = lat; //Latitude *10**7 in 4 bytes
-		IMU_buffer[6]=templong&0xff;
-		IMU_buffer[7]=(templong>>8)&0xff;
-		IMU_buffer[8]=(templong>>16)&0xff;
-		IMU_buffer[9]=(templong>>24)&0xff;
-      
-		#if USE_BAROMETER==0
-			tempint=alt_MSL / 100;   // Altitude MSL in meters * 10 in 2 bytes
-		#else
-			alti();
-			tempint = (press_alt * ALT_MIX + alt_MSL * (100-ALT_MIX)) / 10000;	//Blended GPS and pressure altitude
-		#endif
-		IMU_buffer[10]=tempint&0xff;
-		IMU_buffer[11]=(tempint>>8)&0xff;
-      
-		tempint=speed_3d*100;   // Speed in M/S * 100 in 2 bytes
-		IMU_buffer[12]=tempint&0xff;
-		IMU_buffer[13]=(tempint>>8)&0xff;
-        
-		tempint=ground_course*100;   // course in degreees * 100 in 2 bytes
-		IMU_buffer[14]=tempint&0xff;
-		IMU_buffer[15]=(tempint>>8)&0xff;
-        
-		IMU_buffer[16]=0;
-		IMU_buffer[17]=0;
-		IMU_buffer[18]=0;
-		IMU_buffer[19]=0;
-
-        	IMU_buffer[20]=(imu_health>>8)&0xff;
-
-		for (int i=0;i<ck+2;i++) Serial.print (IMU_buffer[i]);
-		
-		for (int i=0;i<ck+2;i++) {
-			IMU_ck_a+=IMU_buffer[i];  //Calculates checksums
-			IMU_ck_b+=IMU_ck_a;       
-		}
-      	Serial.print(IMU_ck_a);
-      	Serial.print(IMU_ck_b);
-  
-	} else {
-      
-		// This section outputs the IMU orientatiom message
-		Serial.print("DIYd");  // This is the message preamble
-		IMU_buffer[0]=0x06;
-		ck=6;
-		IMU_buffer[1] = 0x02;      
-
-		tempint=ToDeg(roll)*100;  //Roll (degrees) * 100 in 2 bytes
-		IMU_buffer[2]=tempint&0xff;
-		IMU_buffer[3]=(tempint>>8)&0xff;
-      
-		tempint=ToDeg(pitch)*100;   //Pitch (degrees) * 100 in 2 bytes
-		IMU_buffer[4]=tempint&0xff;
-		IMU_buffer[5]=(tempint>>8)&0xff;
-      
-		templong=(ToDeg(yaw)+gc_offset)*100;  //Yaw (degrees) * 100 in 2 bytes
-		if(templong>18000) templong -=36000;
-		if(templong<-18000) templong +=36000;
-		tempint = templong;
-		IMU_buffer[6]=tempint&0xff;
-		IMU_buffer[7]=(tempint>>8)&0xff;
-      
-		for (int i=0;i<ck+2;i++) Serial.print (IMU_buffer[i]);
-	  
-		for (int i=0;i<ck+2;i++) {
-			IMU_ck_a+=IMU_buffer[i];  //Calculates checksums
-			IMU_ck_b+=IMU_ck_a;       
-		}
-		Serial.print(IMU_ck_a);
-		Serial.print(IMU_ck_b);
-          
-    }
-        
-#endif  
-}
-
-void printPerfData(long time)
-{
-
-// This function outputs a performance monitoring message (used every 20 seconds) 
-//  Can be either binary or human readable
-#if PRINT_BINARY == 1
-      byte IMU_buffer[30];
-      int ck;
-      byte IMU_ck_a=0;
-      byte IMU_ck_b=0;
-      
-      	Serial.print("DIYd");  // This is the message preamble
-		IMU_buffer[0]=0x11;
-		ck=17;
-      	IMU_buffer[1] = 0x0a;      
-
-        	//Time for this reporting interval in millisecons
-        	IMU_buffer[2]=time&0xff;
-        	IMU_buffer[3]=(time>>8)&0xff;
-        	IMU_buffer[4]=(time>>16)&0xff;
-        	IMU_buffer[5]=(time>>24)&0xff;
-      
-        	IMU_buffer[6]=mainLoop_count&0xff;
-        	IMU_buffer[7]=(mainLoop_count>>8)&0xff;
-        
-        	IMU_buffer[8]=G_Dt_max&0xff;
-        	IMU_buffer[9]=(G_Dt_max>>8)&0xff;
-      
-        	IMU_buffer[10]=gyro_sat_count;
-        	IMU_buffer[11]=adc_constraints;
-        	IMU_buffer[12]=renorm_sqrt_count;
-        	IMU_buffer[13]=renorm_blowup_count;
-        	IMU_buffer[14]=gps_payload_error_count;
-        	IMU_buffer[15]=gps_checksum_error_count;
-        	IMU_buffer[16]=gps_pos_fix_count;
-        	IMU_buffer[17]=gps_nav_fix_count;
-        	IMU_buffer[18]=gps_messages_sent;
-
-      	for (int i=0;i<ck+2;i++) Serial.print (IMU_buffer[i]);  
-      	for (int i=0;i<ck+2;i++) {
-          		IMU_ck_a+=IMU_buffer[i];  //Calculates checksums
-          		IMU_ck_b+=IMU_ck_a;       
-      	}
-      	Serial.print(IMU_ck_a);
-      	Serial.print(IMU_ck_b);
-      
-#else
-
-/*
-    Serial.print("PPP");
-    Serial.print("pTm:");
-    Serial.print(time,DEC);
-    Serial.print(",mLc:");
-    Serial.print(mainLoop_count,DEC);
-    Serial.print(",DtM:");
-    Serial.print(G_Dt_max,DEC);
-    Serial.print(",gsc:");
-    Serial.print(gyro_sat_count,DEC);
-    Serial.print(",adc:");
-    Serial.print(adc_constraints,DEC);
-    Serial.print(",rsc:");
-    Serial.print(renorm_sqrt_count,DEC);
-    Serial.print(",rbc:");
-    Serial.print(renorm_blowup_count,DEC);
-    Serial.print(",gpe:");
-    Serial.print(gps_payload_error_count,DEC);
-    Serial.print(",gce:");
-    Serial.print(gps_checksum_error_count,DEC);
-    Serial.print(",gpf:");
-    Serial.print(gps_pos_fix_count,DEC);
-    Serial.print(",gnf:");
-    Serial.print(gps_nav_fix_count,DEC);
-    Serial.print(",gms:");
-    Serial.print(gps_messages_sent,DEC);
-    Serial.print(",imu:");
-    Serial.print((imu_health>>8),DEC);
-    Serial.print(",***");
-    
-    */
-#endif
-		// Reset counters
-        mainLoop_count = 0;
-        G_Dt_max  = 0;
-        gyro_sat_count = 0;
-        adc_constraints = 0;
-        renorm_sqrt_count = 0;
-        renorm_blowup_count = 0;
-        gps_payload_error_count = 0;
-        gps_checksum_error_count = 0;
-        gps_pos_fix_count = 0;
-        gps_nav_fix_count = 0;
-        gps_messages_sent = 0;
-        
-
-}
-
-
-long convert_to_dec(float x)
-{
-  return x*10000000;
-}
-

sw/airborne/firmwares/non_ap/arduimu_Firmware_WithGps/arduimu_modified/press_alt.pde

@@ -1,244 +0,0 @@
-/* VTI SCP1000-D11 I2C pressure sensor */
-/* Based on code by Chris Barnes February 2010 */
-/* Based on code by Jose Julio */
-
-/* ATTENTION: SCP1000 is a 3v3 device */
-
-#if USE_BAROMETER == 1
-void setup_scp()
-{
-  unsigned char temp_byte = 0;
-  delay(6);  
-//  Init_Diagnostics();              // Initialise diagnostics output
-
-  // If we know the starting altitude we could initialise it
-  // else just treat this as the altitude relative to the start
-  // init_alt(START_ALTITUDE);     // altitude in m     ****************
-  
-
-  Wire.begin();
-  TWIinit();
-  SCP1000_StopAcquisition();    // Stop acquisition first - in case the sensor had already been put into a non-standby mode (e.g. Arduino s/w reset but sensor not)
-  delay(100);
-  SCP1000_StartAcquisition();   // SCP1000 only accepts new mode from standby - otherwise device will be left in previous mode
-/*  
-  Serial.print("Status: ");
-  temp_byte = SCP1000_GetStatus2();  
-  Serial.println((int)temp_byte);
-  DecodeStatus(temp_byte);
-  
-  Serial.print("Operation Mode: ");
-  Serial.println((int)SCP1000_GetOperation2());
-*/
-  delay(100); 
-}
-
-/********************************************************************
-	Altitude Calculation
-*********************************************************************/
-
-void alti(void)
-{
-  
-	double x;
-	double p = (double)press_gnd/(double)press;
-	double temp = (float)temperature/20.f + 273.15f;
-	x = log(p) * temp * 29271.267f;
-	//x = log(p) * temp * 29.271267 * 1000;
-	press_alt = x + ground_alt;
-	//  Need to add comments for theory.....
-}
-
-/********************************************************************
-	Data Retrieval
-*********************************************************************/
- void SCP1000_StartAcquisition()
-{
-  Wire.beginTransmission((uint8_t)PRESSURE_ADDR);
-  Wire.send((uint8_t)SNS_ADDR_POPERATION);  // Start acquisition
-  Wire.send((uint8_t)SCP_MODE); 
-  Wire.endTransmission();
-}
-
-void SCP1000_StopAcquisition()
-{
-  Wire.beginTransmission((uint8_t)PRESSURE_ADDR);
-  Wire.send((uint8_t)SNS_ADDR_POPERATION);  
-  Wire.send(0x00);   // Stop acquisition
-  Wire.endTransmission();
-}
-   
-unsigned char SCP1000_GetStatus2()
-{
-  unsigned char ready[] = {
-    2, WRITE_PRESSURE_ADDR, SNS_ADDR_PSTATUS,
-    2, READ_PRESSURE_ADDR, 0,
-    0
-  };  
-  TWIdocmd(ready);
- 
-  return(ready[5]);
-}
-
-unsigned char SCP1000_GetOperation2()
-{
-  unsigned char oper[] = {
-    2, WRITE_PRESSURE_ADDR, SNS_ADDR_POPERATION,
-    2, READ_PRESSURE_ADDR, 0,
-    0
-  };  
-  TWIdocmd(oper);
- 
-  return(oper[5]);
-}
-
-// Return the raw temperature value provided by the SCP1000
-// This has units of degrees C x 20
-int SCP1000_GetTemp2()
-{
-  int temp;
- 
-  unsigned char getdatat[] = {
-    2, WRITE_PRESSURE_ADDR, SNS_ADDR_PTEMP,
-    3, READ_PRESSURE_ADDR, 0, 0,
-    0
-  };
-
-  TWIdocmd(getdatat);
-
-  // check sign bit of temperature
-  if (0x20U & getdatat[5])
-  {
-    // negative temperature - extend sign
-    getdatat[5] |= 0xC0;
-  }
-  temp = getdatat[5] << 8;    // MSByte
-  temp |= getdatat[6];        // LSByte
-
-  return(temp);
-}
-
-// Return the raw pressure value provided by the SCP1000
-unsigned long SCP1000_GetPressure2()
-{
-  unsigned long press;
-  
-  unsigned char getdatap1[] = {
-    2, WRITE_PRESSURE_ADDR, SNS_ADDR_DATARD8,
-    2, READ_PRESSURE_ADDR, 0, 
-    0
-  };
-  unsigned char getdatap2[] = {
-    2, WRITE_PRESSURE_ADDR, SNS_ADDR_PPRESSURE,
-    3, READ_PRESSURE_ADDR, 0, 0,
-    0
-  };
-  
-  TWIdocmd(getdatap1);
-
-  press = 0x07 & getdatap1[5];  // MSByte (either 5 or 6 for the sort of altitude we are interested in)
-  
-  TWIdocmd(getdatap2);
-  
-  press <<= 8;
-  press |= getdatap2[5];
-  press <<= 8; 
-  press |= getdatap2[6];
- 
-  return(press);
-}
-
-// Decode bits from status byte and return TRUE is there is a measurement ready to be read
-byte DecodeStatus(byte u8Status)
-{
-  static byte u8Starting = 0U;
-  
-  if (0U != (0x01 & u8Status))
-  {
-    //SCP1000 Startup procedure is still running
-    if (u8Starting)
-    { 
-      Serial.println("Starting");
-      u8Starting = 1;  // remember that we have already output this diagnostic
-    }
-  }
-  else
-  {
-    u8Starting = 0;    // remember that we ahve seen the device out of starting mode
-    if (0U != (0x10 & u8Status))
-    {
-      // Real Time Error - data was not read in time - clear by reading it
-      // Note - this "error" is to be expected on startup when the SCP1000 is already running 
-      // as we will not have been reading out all of the measurements that have been made...
-      Serial.println("RTErr");						
-    }
-    if (0U != (0x20 & u8Status))
-    {
-      // DRDY - new data ready
-      return (TRUE);
-    }
-  }
-  return (FALSE);
-}
-  
-void ReadSCP1000(void)
-{
-  temp_unfilt = SCP1000_GetTemp2();
-  press = SCP1000_GetPressure2();
-
-}
-
-
-
-/********************************************************************
-	This Library is needed for the SCP1000-D11
-	because I2C commands needs a RESTART between commands and not and STOP-START (like Wire library do)
-	This code is from http://harleyhacking.blogspot.com/
-*********************************************************************/
-
-
-#include <avr/io.h>
-
-// TWI operations
-#define ENABTW ((1<<TWINT)|(1<<TWEN)|(0<<TWIE)) // 0x80 4 1
-#define START TWCR = (ENABTW|(1<<TWSTA))        // 0x20
-#define STOP TWCR = (ENABTW|(1<<TWSTO)) // 0x10
-#define SEND(x)  TWDR = x;  TWCR = ENABTW;
-#define RECV(ack) TWCR = ENABTW | (ack? (1<<TWEA) : 0 );
-unsigned char twista;
-unsigned twitmo;
-#define WAIT twitmo=0; while (!((twista = TWCR) & (1 << TWINT)) && ++twitmo);
- 
-/////===================================////////////////////
-void TWIinit(void)
-{
-    DDRC &= ~0x30;              // pullup
-    PORTC |= 0x30;              // pullup
-    TWBR = (((F_CPU/400000)-16)/2); // 400 khz
-    TWCR |= (1 << TWEN);
-}
- 
-void TWIdocmd(unsigned char *msg)
-{
-    unsigned int mlen, rdwrf;
- 
-    while ((mlen = *msg++)) {
-        rdwrf = *msg & 1;
-        START;
-        WAIT;
-        do {
-            SEND(*msg++);
-            WAIT;
-            // should check for ACK - twista == SAWA or SDWA
-        } while (--mlen && !rdwrf);
-        // read
-        while (mlen--) {
-            RECV(mlen);
-            WAIT;
-            *msg++ = TWDR;
-        }
-    }
-    STOP;
-}
-
-#endif

sw/airborne/modules/ins/ins_arduimu_basic.c

@@ -6,8 +6,8 @@ Autoren@ZHAW:   schmiemi
 
 
 #include <stdbool.h>
-#include "modules/ins/ins_arduimu_modified.h"
+#include "modules/ins/ins_arduimu_basic.h"
-#include "firmwares/fixedwing/main_fbw.h"
+//#include "firmwares/fixedwing/main_fbw.h"
 #include "mcu_periph/i2c.h"
 
 // test
@@ -15,7 +15,6 @@ Autoren@ZHAW:   schmiemi
 
 // für das Senden von GPS-Daten an den ArduIMU
 #include "gps.h"
-int32_t GPS_Data[10];
 
 #define NB_DATA 9
 
@@ -71,25 +70,12 @@ void ArduIMU_periodicGPS( void ) {
 
   if (ardu_gps_trans.status != I2CTransDone) { return; }
 
-  //velned
+  FillBufWith32bit(ardu_gps_trans.buf, 0, (int32_t)gps_speed_3d); // speed 3D
-  GPS_Data [0] = gps_speed_3d;  //speed 3D
+  FillBufWith32bit(ardu_gps_trans.buf, 4, (int32_t)gps_gspeed);   // ground speed
-  GPS_Data [1] = gps_gspeed;    //ground speed
+  FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps_course);   // course
-  GPS_Data [2] = gps_course * 100000;	//Kurs
+  ardu_gps_trans.buf[12] = gps_mode;                              // status gps fix
-  //alt
+  ardu_gps_trans.buf[13] = gps_status_flags;                      // status flags
-  GPS_Data [3] = gps_alt;   // height above elipsoid
+  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 14);
-  GPS_Data [4] = gps_hmsl;  // height above sea level
-  //status
-  GPS_Data [5] = gps_mode;          //fix
-  GPS_Data [6] = gps_status_flags;  //flags
-
-  FillBufWith32bit(ardu_gps_trans.buf, 0, GPS_Data[0]);
-  FillBufWith32bit(ardu_gps_trans.buf, 4, GPS_Data[1]);
-  FillBufWith32bit(ardu_gps_trans.buf, 8, GPS_Data[2]);
-  FillBufWith32bit(ardu_gps_trans.buf, 12, GPS_Data[3]);
-  FillBufWith32bit(ardu_gps_trans.buf, 16, GPS_Data[4]);
-  ardu_gps_trans.buf[20] = GPS_Data[5]; // status gps fix
-  ardu_gps_trans.buf[21] = GPS_Data[6]; // status flags
-  I2CTransmit(ARDUIMU_I2C_DEV, ardu_gps_trans, ArduIMU_SLAVE_ADDR, 22);
 
 }
 
@@ -100,21 +86,20 @@ void ArduIMU_periodic( void ) {
     I2CReceive(ARDUIMU_I2C_DEV, ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA*2);
   }
 
-  /*
-     Buffer O:	Roll
-     Buffer 1:	Pitch
-     Buffer 2:	Yaw
-     Buffer 3:	Gyro X
-     Buffer 4:	Gyro Y
-     Buffer 5:	Gyro Z
-     Buffer 6:	Accel X
-     Buffer 7:	Accel Y
-     Buffer 8:	Accel Z
-     */
-
 }
 
 #include "math/pprz_algebra_int.h"
+/*
+   Buffer O:	Roll
+   Buffer 1:	Pitch
+   Buffer 2:	Yaw
+   Buffer 3:	Gyro X
+   Buffer 4:	Gyro Y
+   Buffer 5:	Gyro Z
+   Buffer 6:	Accel X
+   Buffer 7:	Accel Y
+   Buffer 8:	Accel Z
+   */
 
 void ArduIMU_event( void ) {
   // Handle INS I2C event