diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c
index 28d1d8cb24..7e2cd20984 100644
--- a/sw/airborne/main_ap.c
+++ b/sw/airborne/main_ap.c
@@ -551,7 +551,8 @@ void periodic_task_ap( void ) {
   if (max11040_data == MAX11040_DATA_AVAILABLE) {
     LED_TOGGLE(3);
     for (i=0; i<16; i++) {
-      max11040_values_f[i] = (max11040_values[i] * 2.2) / 8388608.0;
+      /* we assume that the buffer will be full always in this test mode anyway */
+      max11040_values_f[i] = (max11040_values[max11040_buf_in][i] * 2.2) / 8388608.0;
     }
 
     DOWNLINK_SEND_TURB_PRESSURE_VOLTAGE(
diff --git a/sw/airborne/max11040.c b/sw/airborne/max11040.c
index 3ecbd7151a..9b4aa547cc 100644
--- a/sw/airborne/max11040.c
+++ b/sw/airborne/max11040.c
@@ -188,7 +188,7 @@ static void SSP_ISR(void) {
         if (i != max11040_buf_out) {
           max11040_buf_in = i;
  	} else {
-          max11040_buf_in = i;
+          //throw error;
 	}
       }
     }