From 71da5b05ee1983604531411f09e4c4c3aa226bca Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Tue, 30 Aug 2011 17:17:16 +0200
Subject: [PATCH] CHIMU GPS speed feedback update

---
 conf/modules/ins_chimu_spi.xml          |  2 +-
 sw/airborne/modules/ins/imu_chimu.c     | 28 ------------------------
 sw/airborne/modules/ins/imu_chimu.h     | 29 +++++++++++++++++++++++++
 sw/airborne/modules/ins/ins_chimu_spi.c | 15 ++++++++++++-
 sw/airborne/modules/ins/ins_xsens.c     |  4 ++++
 5 files changed, 48 insertions(+), 30 deletions(-)

diff --git a/conf/modules/ins_chimu_spi.xml b/conf/modules/ins_chimu_spi.xml
index ea57d2ff2e..db4f90c226 100644
--- a/conf/modules/ins_chimu_spi.xml
+++ b/conf/modules/ins_chimu_spi.xml
@@ -6,7 +6,7 @@
     <file name="ins_module.h"/>
   </header>
   <init fun="ins_init()"/>
-  <periodic fun="ins_periodic_task()" freq="60"/>
+  <periodic fun="ins_periodic_task()" freq="8"/>
   <event fun="parse_ins_msg()"/>
   <makefile>
     <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_module.h\\\""/>
diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c
index 4b0111bee5..8ee2f11cc7 100644
--- a/sw/airborne/modules/ins/imu_chimu.c
+++ b/sw/airborne/modules/ins/imu_chimu.c
@@ -106,34 +106,6 @@ void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
 // Communication Definitions
 #define CHIMU_COM_ID_HIGH	0x1F  //Must set this to the max ID expected above
 
-/***************************************************************************
- * Endianness Swapping Functions
- */
-
-#ifdef CHIMU_BIG_ENDIAN
-
-static float FloatSwap( float f )
-{
-  union
-  {
-    float f;
-    unsigned char b[4];
-  } dat1, dat2;
-
-  dat1.f = f;
-  dat2.b[0] = dat1.b[3];
-  dat2.b[1] = dat1.b[2];
-  dat2.b[2] = dat1.b[1];
-  dat2.b[3] = dat1.b[0];
-  return dat2.f;
-}
-
-#else
-
-#define FloatSwap(X) (X)
-
-#endif
-
 /*---------------------------------------------------------------------------
         Name: CHIMU_Init
 
diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h
index d1caf15286..d246bf6a6f 100644
--- a/sw/airborne/modules/ins/imu_chimu.h
+++ b/sw/airborne/modules/ins/imu_chimu.h
@@ -86,6 +86,35 @@
 #define CHIMU_Msg_15_SFCheck                    15
 
 
+/***************************************************************************
+ * Endianness Swapping Functions
+ */
+
+#ifdef CHIMU_BIG_ENDIAN
+
+static inline float FloatSwap( float f )
+{
+  union
+  {
+    float f;
+    unsigned char b[4];
+  } dat1, dat2;
+
+  dat1.f = f;
+  dat2.b[0] = dat1.b[3];
+  dat2.b[1] = dat1.b[2];
+  dat2.b[2] = dat1.b[1];
+  dat2.b[3] = dat1.b[0];
+  return dat2.f;
+}
+
+#else
+
+#define FloatSwap(X) (X)
+
+#endif
+
+
 typedef struct {
 	float phi;
 	float theta;
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
index 6ea0deaca2..87dd9f519f 100644
--- a/sw/airborne/modules/ins/ins_chimu_spi.c
+++ b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -27,6 +27,9 @@ C code to connect a CHIMU using uart
 #include "ins_module.h"
 #include "imu_chimu.h"
 
+#include "subsystems/gps.h"
+
+
 CHIMU_PARSER_DATA CHIMU_DATA;
 
 INS_FORMAT ins_roll_neutral;
@@ -101,7 +104,17 @@ void parse_ins_msg( 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 };
+  uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0x00, 0x00, 0x00, 0x00,   0xc2 };
+
+  float gps_speed = 0;
+
+  if (gps.fix == GPS_FIX_3D) 
+  {
+    gps_speed = gps.speed_3d/100.; 
+  }
+  gps_speed = FloatSwap(gps_speed);
+
+  memmove (&centripedal[6], &gps_speed, 4);
 
   // Fill X-speed
 
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 9b55080239..f16d597a69 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -306,6 +306,10 @@ void handle_ins_msg( void) {
   hmsl /= 1000.0f;
   EstimatorSetAlt(hmsl);
 
+  #ifndef ALT_KALMAN
+  #warning NO_VZ
+  #endif
+
   // Horizontal speed
   float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
   if (gps.fix != GPS_FIX_3D)