diff --git a/conf/modules/ins_chimu_spi.xml b/conf/modules/ins_chimu_spi.xml
index 953c253232..cfced9f4c7 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="60"/>
   <event fun="parse_ins_msg()"/>
   <makefile>
     <define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_module.h\\\""/>
diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
index e4397b34f7..c7ac320e07 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
+++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
@@ -38,6 +38,8 @@
 /* High Speed SPI Slave Circular Buffer */
 uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx;
 uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
+uint16_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
+uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
 
 /* Prototypes */
 // void spi_init( void ); // -> declared in spi.h
@@ -133,8 +135,21 @@ void spi_init(void) {
 static void SSP_ISR(void) {
   ISR_ENTRY();
 
-  LED_TOGGLE(3);
+  //LED_TOGGLE(3);
 
+  // If any TX bytes are pending
+  if (spi_slave_hs_tx_insert_idx != spi_slave_hs_tx_extract_idx)
+  {
+    uint8_t ret = spi_slave_hs_tx_buffer[spi_slave_hs_tx_extract_idx];
+    spi_slave_hs_tx_extract_idx = (spi_slave_hs_tx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE;
+    SSP_Write(ret);
+  }
+  else
+  {
+    SSP_Write(0x00);
+  }
+
+  
   //do
   {
     uint16_t temp;
diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h
index ef73da3a5f..8794123fe8 100644
--- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h
+++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h
@@ -57,6 +57,19 @@ extern uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
    ret;                                                 \
 })
 
+#define SPI_SLAVE_HS_TX_BUFFER_SIZE 128
+
+extern uint16_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
+extern uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
+
+#define SpiSlaveTransmit(data) {\
+  uint16_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; \
+  if (temp != spi_slave_hs_tx_extract_idx)	/* there is room left */	\
+  { \
+    spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = (uint8_t)data; \
+    spi_slave_hs_tx_insert_idx = temp; \
+  } \
+}
 
 
 
diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c
index 2fff4457f8..978b4456c2 100644
--- a/sw/airborne/modules/ins/ins_chimu_spi.c
+++ b/sw/airborne/modules/ins/ins_chimu_spi.c
@@ -35,17 +35,44 @@ INS_FORMAT ins_pitch_neutral;
 
 volatile uint8_t new_ins_attitude;
 
+#define INS_LINK	SpiSlave
+
 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 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
+  
+  new_ins_attitude = 0;
+  
   ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
   ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
-  new_ins_attitude= 0;
   
   CHIMU_Init(&CHIMU_DATA);  
-    
+  
+  // Quat Filter
+  for (int i=0;i<7;i++)
+  {
+    InsSend1(quaternions[i]);
+  }
+  // Wait a second
+  InsSend1(0);
+  InsSend1(0);
+  InsSend1(0);
+  InsSend1(0);
+  InsSend1(0);
+
+  // 50Hz data: attitude only
+  for (int i=0;i<12;i++)
+  {
+    InsSend1(rate[i]);
+  }
+  
 }
 
-#define INS_LINK	SpiSlave
+
+//float tempang = 0;
 
 void parse_ins_msg( void )
 {
@@ -64,8 +91,9 @@ void parse_ins_msg( void )
 	{
 	  CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
 	}
-	/*
-	if (CHIMU_DATA.m_attitude.euler.phi == tempang)
+	
+	LED_TOGGLE(3);
+/*	if (CHIMU_DATA.m_attitude.euler.phi == tempang)
 	{
 	  LED_ON(3);
 	}
@@ -74,12 +102,12 @@ void parse_ins_msg( void )
 	  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);
 	
 	//DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
-
+	
       }
     }
   }
@@ -89,6 +117,13 @@ void parse_ins_msg( void )
 //Frequency defined in conf *.xml
 void ins_periodic_task( void ) 
 {
+  // Send Centripetal Corrections
+  uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
+  for (int i=0;i<7;i++)
+  {
+    InsSend1(quaternions[i]);
+  }
+
   // Downlink Send
 }
 
diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h
index 0aefdc2e20..aaa8bd1a36 100644
--- a/sw/airborne/modules/ins/ins_module.h
+++ b/sw/airborne/modules/ins/ins_module.h
@@ -84,7 +84,8 @@ void parse_ins_buffer( uint8_t );
 
 #define InsBuffer() InsLink(ChAvailable())
 #define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); }
-#define InsUartSend1(c) InsLink(Transmit(c))
+#define InsSend1(c) InsLink(Transmit(c))
+#define InsUartSend1(c) InsSend1(c)
 #define InsUartInitParam(_a,_b,_c) InsLink(InitParam(_a,_b,_c))
 #define InsUartRunning InsLink(TxRunning)