some fixes for ins modules to use new gps interface

paparazzi · Jun 18, 2011 · 9cae0a3 · 9cae0a3
1 parent e7f4281
commit 9cae0a3
Show file tree

Hide file tree

Showing 3 changed files with 31 additions and 33 deletions.
diff --git a/sw/airborne/modules/ins/alt_filter.c b/sw/airborne/modules/ins/alt_filter.c
@@ -21,7 +21,7 @@
  */
 
 #include "modules/ins/alt_filter.h"
-#include "gps.h"
+#include "subsystems/gps.h"
 #include "modules/sensors/baro_ets.h"
 
 #ifndef DOWNLINK_DEVICE
@@ -65,7 +65,7 @@ void alt_filter_periodic(void) {
   kalmanEstimation(&alt_filter,0.);
 
   // update on new data
-  float ga = (float)gps_alt / 100.;
+  float ga = (float)gps.hmsl / 1000.;
   if (baro_ets_altitude != last_baro_alt) {
     kalmanCorrectionAltimetre(&alt_filter, baro_ets_altitude);
     last_baro_alt = baro_ets_altitude;
@@ -204,7 +204,7 @@ void kalmanEstimation(TypeKalman *k, float accVert){
     I[0][0] = Kf[0]; I[0][1] = 0; I[0][2] = 0;
     I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = 0;
     I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = 0;
-	
+
     for(i=0;i<3;i++){
       for (j=0;j<3;j++){
         k->P[i][j] = k->P[i][j] - I[i][0]*k->P[0][j] - I[i][1]*k->P[1][j] - I[i][2]*k->P[2][j];
@@ -231,7 +231,7 @@ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){
   // C = [1 0 1]
   // div = C*P*C' + R
   div = k->P[0][0] + k->P[2][0] + k->P[0][2] + k->P[2][2] + SigAltiAltimetre*SigAltiAltimetre;
-  
+
   if (fabs(div) > 1e-5) {
     // Kf = P*C'*inv(div)
     Kf[0] = (k->P[0][0] + k->P[0][2]) / div;
@@ -259,4 +259,3 @@ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){
   }
 
 }
-
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -13,7 +13,7 @@ C code to connect a CHIMU using uart
 #include "estimator.h"
 
 // For centripedal corrections
-#include "gps.h"
+#include "subsystems/gps.h"
 
 // Telemetry
 #ifndef DOWNLINK_DEVICE
@@ -34,22 +34,22 @@ INS_FORMAT ins_pitch_neutral;
 
 volatile uint8_t new_ins_attitude;
 
-void ins_init( void ) 
+void ins_init( void )
 {
   uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 };	// 50Hz attitude only + SPI
 //  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
 //  uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
   uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
-  
+
   CHIMU_Checksum(rate,12);
-  
+
   new_ins_attitude = 0;
-  
+
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
-  
-  CHIMU_Init(&CHIMU_DATA);  
-  
+
+  CHIMU_Init(&CHIMU_DATA);
+
   // Quat Filter
   for (int i=0;i<7;i++)
   {
@@ -68,55 +68,54 @@ void ins_init( void )
   {
     InsSend1(rate[i]);
   }
-  
+
 }
 
 void parse_ins_msg( void )
 {
   while (InsLink(ChAvailable()))
   {
     uint8_t ch = InsLink(Getch());
-    
+
     if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
     {
       if(CHIMU_DATA.m_MsgID==0x03)
       {
-	new_ins_attitude = 1;
-	RunOnceEvery(25, LED_TOGGLE(3) );
-	if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
-	{
-	  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
-	}
-	
-	EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
-	EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
+    new_ins_attitude = 1;
+    RunOnceEvery(25, LED_TOGGLE(3) );
+    if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
+    {
+      CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
+    }
+
+    EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
+    EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
       }
       else if(CHIMU_DATA.m_MsgID==0x02)
       {
-	
-	RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
-	
+
+    RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
+
       }
     }
   }
 }
 
 
 //Frequency defined in conf *.xml
-void ins_periodic_task( void ) 
+void ins_periodic_task( void )
 {
   // Send SW Centripetal Corrections
   uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
-  
+
   // Fill X-speed
-  
+
   CHIMU_Checksum(centripedal,19);
-  
+
   for (int i=0;i<19;i++)
   {
     InsSend1(centripedal[i]);
   }
 
   // Downlink Send
 }
-
diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -13,7 +13,7 @@ C code to connect a CHIMU using uart
 #include "estimator.h"
 
 // For centripedal corrections
-#include "gps.h"
+#include "subsystems/gps.h"
 
 // Telemetry
 #ifndef DOWNLINK_DEVICE