diff --git a/flight/INS/ins.c b/flight/INS/ins.c
index 32a479713..d70bb4003 100644
--- a/flight/INS/ins.c
+++ b/flight/INS/ins.c
@@ -88,7 +88,7 @@ void affine_rotate(float scale[3][4], float rotation[3]);
 void calibration(float result[3], float scale[3][4], float arg[3]);
 
 extern void PIOS_Board_Init(void);
-static void panic(uint32_t blinks);
+void panic(uint32_t blinks);
 static void print_ekf_binary();
 void simple_update();
 
@@ -158,6 +158,7 @@ int main()
 	// Sensors need a second to start
 	PIOS_DELAY_WaitmS(100);
 
+#if 0
 	// Sensor test
 	if(PIOS_IMU3000_Test() != 0)
 		panic(1);
@@ -169,8 +170,9 @@ int main()
 		panic(3);	
 	
 	if(PIOS_BMP085_Test() != 0)
-		panic(4); 
-	
+		panic(4); 	
+#endif
+
 	PIOS_LED_On(LED1);
 	PIOS_LED_Off(LED2);
 	
@@ -340,7 +342,7 @@ static void print_ekf_binary()
 	altitude_data.updated = 0;
 }
 
-static void panic(uint32_t blinks) 
+void panic(uint32_t blinks) 
 {
 	int blinked = 0;
 	while(1) {
diff --git a/flight/PiOS/STM32F2xx/pios_bma180.c b/flight/PiOS/STM32F2xx/pios_bma180.c
index 377d25400..d949d46dc 100644
--- a/flight/PiOS/STM32F2xx/pios_bma180.c
+++ b/flight/PiOS/STM32F2xx/pios_bma180.c
@@ -386,15 +386,12 @@ static inline void PIOS_BMA180_SPI_Callback()
  * @brief IRQ Handler
  */
 const static uint8_t pios_bma180_req_buf[7] = {BMA_X_LSB_ADDR | 0x80,0,0,0,0,0};
-int32_t setup_time;
 static void PIOS_BMA180_IRQHandler(void)
 {
-	uint32_t timeval = PIOS_DELAY_GetRaw();
 	// If we can't get the bus then just move on for efficiency
 	if(PIOS_BMA180_ClaimBus() == 0)
 		PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, 
 							   sizeof(pios_bma180_dmabuf), NULL);	
-	setup_time = PIOS_DELAY_DiffuS(timeval);
 	PIOS_BMA180_SPI_Callback();
 }
 
diff --git a/flight/PiOS/STM32F2xx/pios_bmp085.c b/flight/PiOS/STM32F2xx/pios_bmp085.c
index d25d4b220..a98bae4e4 100644
--- a/flight/PiOS/STM32F2xx/pios_bmp085.c
+++ b/flight/PiOS/STM32F2xx/pios_bmp085.c
@@ -303,15 +303,11 @@ int32_t PIOS_BMP085_Test()
 /**
  * Handle external line 2 interrupt requests
  */
-uint32_t bmp_eocs = 0;
 void EXTI2_IRQHandler(void)
 {
 	if (EXTI_GetITStatus(dev_cfg->eoc_exti.init.EXTI_Line) != RESET) {
 		/* Read the ADC Value */
 		PIOS_BMP085_EOC=1;
-		
-		bmp_eocs++;
-		
 		/* Clear the EXTI line pending bit */
 		EXTI_ClearITPendingBit(dev_cfg->eoc_exti.init.EXTI_Line);
 	}
diff --git a/flight/PiOS/STM32F2xx/pios_hmc5883.c b/flight/PiOS/STM32F2xx/pios_hmc5883.c
index f44955f32..0ea02d5c9 100644
--- a/flight/PiOS/STM32F2xx/pios_hmc5883.c
+++ b/flight/PiOS/STM32F2xx/pios_hmc5883.c
@@ -162,8 +162,6 @@ static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg * cfg)
  * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
  * \return 0 for success or -1 for failure
  */
-uint32_t fail = 0;
-uint32_t succeed = 0;
 int32_t PIOS_HMC5883_ReadMag(int16_t out[3])
 {
 	pios_hmc5883_data_ready = false;
@@ -172,10 +170,8 @@ int32_t PIOS_HMC5883_ReadMag(int16_t out[3])
 	int32_t sensitivity;
 	
 	if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) {
-		fail++;
 		return -1;
 	}
-	succeed++;
 		
 	switch (CTRLB & 0xE0) {
 		case 0x00:
@@ -414,13 +410,9 @@ void PIOS_HMC5883_IRQHandler(void)
  * Soon this will be generic in pios_exti and the BMA180 will register
  * against it.  Right now this is crap!
  */
-uint32_t count = 0;
-uint32_t count2 = 0;
 void EXTI9_5_IRQHandler(void)
 {
-	count++;
 	if (EXTI_GetITStatus(dev_cfg->eoc_exti.init.EXTI_Line) != RESET) {
-		count2++;
 		PIOS_HMC5883_IRQHandler();
 		EXTI_ClearITPendingBit(dev_cfg->eoc_exti.init.EXTI_Line);
 	}
diff --git a/flight/PiOS/STM32F2xx/pios_imu3000.c b/flight/PiOS/STM32F2xx/pios_imu3000.c
index 4ce05691b..17eec3cd9 100644
--- a/flight/PiOS/STM32F2xx/pios_imu3000.c
+++ b/flight/PiOS/STM32F2xx/pios_imu3000.c
@@ -327,7 +327,6 @@ out:
 uint32_t imu3000_irq = 0;
 uint16_t fifo_level;
 uint8_t fifo_level_data[2];
-uint32_t imu3000_readtime;
 uint32_t cb_not_ready = 0;
 void PIOS_IMU3000_IRQHandler(void)
 {
@@ -368,13 +367,11 @@ void PIOS_IMU3000_IRQHandler(void)
 		if(fifo_level < (sizeof(imu3000_read_buffer) + 2))
 			return;
 		
-		uint32_t timeval = PIOS_DELAY_GetRaw();
 		imu3000_cb_ready = false;
 		
 		// Leave footer in buffer
 		PIOS_IMU3000_Read_Callback(PIOS_IMU3000_FIFO_REG, imu3000_read_buffer, sizeof(imu3000_read_buffer), imu3000_callback);
 		
-		imu3000_readtime = PIOS_DELAY_DiffuS(timeval);		
 	}
 }