Chimu Cleanup

paparazzi · Apr 22, 2011 · bcf40ab · bcf40ab
2 parents 73d7af9 + 223e331
commit bcf40ab
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 42 deletions.
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -27,7 +27,6 @@ C code to connect a CHIMU using uart
 #include "ins_module.h"
 #include "imu_chimu.h"
 
-
 CHIMU_PARSER_DATA CHIMU_DATA;
 
 INS_FORMAT ins_roll_neutral;
@@ -37,15 +36,13 @@ volatile uint8_t new_ins_attitude;
 
 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 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;
@@ -58,6 +55,7 @@ void ins_init( void )
   {
     InsSend1(quaternions[i]);
   }
+
   // Wait a bit (SPI send zero)
   InsSend1(0);
   InsSend1(0);
@@ -73,9 +71,6 @@ void ins_init( void )
 
 }
 
-
-//float tempang = 0;
-
 void parse_ins_msg( void )
 {
   while (InsLink(ChAvailable()))
@@ -87,28 +82,12 @@ void parse_ins_msg( void )
       if(CHIMU_DATA.m_MsgID==0x03)
       {
 	new_ins_attitude = 1;
-	// RunOnceEvery(25, LED_TOGGLE(3) );
-	// LED_TOGGLE(3);
+	RunOnceEvery(25, LED_TOGGLE(3) );
 	if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
 	{
 	  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
 	}
-	if (CHIMU_DATA.m_attrates.euler.phi > M_PI)
-	{
-	  CHIMU_DATA.m_attrates.euler.phi -= 2 * M_PI;
-	}
 
-	LED_TOGGLE(3);
-/*	if (CHIMU_DATA.m_attitude.euler.phi == tempang)
-	{
-	  LED_ON(3);
-	}
-	else
-	{
-	  LED_OFF(3);
-	}
-	tempang = CHIMU_DATA.m_attitude.euler.phi;
-*/	
 	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);
       }

diff --git a/sw/airborne/modules/ins/ins_chimu_uart.c b/sw/airborne/modules/ins/ins_chimu_uart.c
@@ -4,8 +4,11 @@ C code to connect a CHIMU using uart
 
 
 #include <stdbool.h>
+
 //#include "modules/ins/ins_chimu_uart.h"
 
+
+
 // Output
 #include "estimator.h"
 
@@ -33,11 +36,13 @@ volatile uint8_t new_ins_attitude;
 
 void ins_init( void ) 
 {
-  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 };	// 50Hz attitude only + SPI
-//  uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI
+  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;
@@ -50,6 +55,14 @@ void ins_init( void )
   {
     InsUartSend1(quaternions[i]);
   }
+
+
+
+
+
+
+
+
   // 50Hz
   for (int i=0;i<12;i++)
   {
@@ -58,8 +71,6 @@ void ins_init( void )
 
 }
 
-float tempang = 0;
-
 void parse_ins_msg( void )
 {
   while (InsLink(ChAvailable()))
@@ -71,23 +82,12 @@ void parse_ins_msg( void )
       if(CHIMU_DATA.m_MsgID==0x03)
       {
 	new_ins_attitude = 1;
-	// RunOnceEvery(25, LED_TOGGLE(3) );
-	// LED_TOGGLE(3);
+	RunOnceEvery(25, LED_TOGGLE(3) );
 	if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
 	{
 	  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
 	}
-
-	if (CHIMU_DATA.m_attitude.euler.phi == tempang)
-	{
-	  LED_ON(3);
-	}
-	else
-	{
-	  LED_OFF(3);
-	}
-	tempang = CHIMU_DATA.m_attitude.euler.phi;
-
+
 	EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
 	//EstimatorSetRate(ins_p,ins_q);