diff --git a/flight/pios/common/pios_mpu6000.c b/flight/pios/common/pios_mpu6000.c
index a95dc846f..ace15cac5 100644
--- a/flight/pios/common/pios_mpu6000.c
+++ b/flight/pios/common/pios_mpu6000.c
@@ -570,9 +570,9 @@ static int32_t PIOS_MPU6000_I2C_GetReg(uint8_t address)
     return data;
 }
 
-static bool PIOS_MPU6000_I2C_Read_Completed()
+static bool PIOS_MPU6000_I2C_Read_Completed(enum pios_i2c_transfer_result result)
 {
-    return PIOS_MPU6000_HandleData(gyro_read_timestamp);
+    return (result == PIOS_I2C_TRANSFER_OK) ? PIOS_MPU6000_HandleData(gyro_read_timestamp) : false;
 }
 
 static bool PIOS_MPU6000_I2C_ReadSensor(bool *woken)
diff --git a/flight/pios/inc/pios_i2c.h b/flight/pios/inc/pios_i2c.h
index 546a51e2f..b37537bda 100644
--- a/flight/pios/inc/pios_i2c.h
+++ b/flight/pios/inc/pios_i2c.h
@@ -73,7 +73,17 @@ enum pios_i2c_error_count {
     PIOS_I2C_ERROR_COUNT_NUMELEM,
 };
 
-typedef bool (*pios_i2c_callback)();
+enum pios_i2c_transfer_result {
+    PIOS_I2C_TRANSFER_OK = 0,
+    PIOS_I2C_TRANSFER_BUSY = -2,
+    PIOS_I2C_TRANSFER_BUS_ERROR = -1,
+    PIOS_I2C_TRANSFER_NACK = -3,
+    PIOS_I2C_TRANSFER_TIMEOUT = -4,
+    PIOS_I2C_TRANSFER_UNSPECIFIED_ERROR = -5,
+    PIOS_I2C_TRANSFER_DEVICE_ERROR = -6,
+};
+
+typedef bool (*pios_i2c_callback)(enum pios_i2c_transfer_result result);
 
 /* Public Functions */
 extern int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[], uint32_t num_txns);
diff --git a/flight/pios/inc/pios_i2c_priv.h b/flight/pios/inc/pios_i2c_priv.h
index 20fec3e5d..7be45ba77 100644
--- a/flight/pios/inc/pios_i2c_priv.h
+++ b/flight/pios/inc/pios_i2c_priv.h
@@ -73,6 +73,8 @@ struct pios_i2c_adapter {
 
     uint8_t *active_byte;
     uint8_t *last_byte;
+    
+    enum pios_i2c_transfer_result *transfer_result;
 };
 
 int32_t PIOS_I2C_Init(uint32_t *i2c_id, const struct pios_i2c_adapter_cfg *cfg);
diff --git a/flight/pios/stm32f10x/pios_i2c.c b/flight/pios/stm32f10x/pios_i2c.c
index e18edc2fe..c411d7eac 100644
--- a/flight/pios/stm32f10x/pios_i2c.c
+++ b/flight/pios/stm32f10x/pios_i2c.c
@@ -980,7 +980,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
     if (!PIOS_I2C_validate(i2c_adapter)) {
-        return -1;
+        return PIOS_I2C_TRANSFER_DEVICE_ERROR;
     }
 
     PIOS_DEBUG_Assert(txn_list);
@@ -993,9 +993,10 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     portTickType timeout;
     timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
     if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
 #else
+#error PIOS_I2C_Transfer is broken for use without FreeRTOS
     uint32_t timeout = 0xfff;
     while (i2c_adapter->busy && --timeout) {
         ;
@@ -1071,9 +1072,9 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     }
 #endif /* USE_FREERTOS */
 
-    return !semaphore_success ? -2 :
-           i2c_adapter->bus_error ? -1 :
-           0;
+    return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT :
+           i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
+           PIOS_I2C_TRANSFER_OK;
 }
 
 
diff --git a/flight/pios/stm32f30x/pios_i2c.c b/flight/pios/stm32f30x/pios_i2c.c
index 2a8b83535..831c5f30e 100644
--- a/flight/pios/stm32f30x/pios_i2c.c
+++ b/flight/pios/stm32f30x/pios_i2c.c
@@ -135,7 +135,6 @@ static void i2c_adapter_process_auto(struct pios_i2c_adapter *i2c_adapter, bool
 static void i2c_adapter_inject_event(struct pios_i2c_adapter *i2c_adapter, enum i2c_adapter_event event, bool *woken);
 static void i2c_adapter_fsm_init(struct pios_i2c_adapter *i2c_adapter);
 static void i2c_adapter_reset_bus(struct pios_i2c_adapter *i2c_adapter);
-static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, bool *woken);
 
 #if defined(PIOS_I2C_DIAGNOSTICS)
 static void i2c_adapter_log_fault(struct pios_i2c_adapter *i2c_adapter, enum pios_i2c_error_type type);
@@ -234,9 +233,18 @@ static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken)
 {
     I2C_ITConfig(i2c_adapter->cfg->regs, I2C_IT_ERRI | I2C_IT_TCI | I2C_IT_NACKI | I2C_IT_RXI | I2C_IT_STOPI | I2C_IT_TXI, DISABLE);
 
-    if (i2c_adapter->callback) {
-        i2c_adapter_callback_handler(i2c_adapter, woken);
-    } else {
+    enum pios_i2c_transfer_result result =
+                              i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
+                              i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK :
+                              PIOS_I2C_TRANSFER_OK;
+
+    if (i2c_adapter->transfer_result) {
+        *(i2c_adapter->transfer_result) = result;
+    }
+    
+    pios_i2c_callback cb = i2c_adapter->callback;
+
+    if(!cb) { /* No callback? Signal that we are done */
 #ifdef PIOS_INCLUDE_FREERTOS
         signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE;
         if (xSemaphoreGiveFromISR(i2c_adapter->sem_ready, &pxHigherPriorityTaskWoken) != pdTRUE) {
@@ -249,6 +257,23 @@ static void go_stopped(struct pios_i2c_adapter *i2c_adapter, bool *woken)
         }
 #endif /* PIOS_INCLUDE_FREERTOS */
     }
+
+#ifdef PIOS_INCLUDE_FREERTOS
+    /* Unlock the bus */
+    signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
+    xSemaphoreGiveFromISR(i2c_adapter->sem_busy, &xHigherPriorityTaskWoken);
+    if (xHigherPriorityTaskWoken == pdTRUE) {
+        *woken = true;
+    }
+#else
+    PIOS_IRQ_Disable();
+    i2c_adapter->busy = 0;
+    PIOS_IRQ_Enable();
+#endif /* PIOS_INCLUDE_FREERTOS */
+
+    if(cb && cb(result)) { /* User provided callback? Do it, but with bus unlocked */
+        *woken = true;
+    }
 }
 
 static void go_starting(struct pios_i2c_adapter *i2c_adapter, __attribute__((unused)) bool *woken)
@@ -609,7 +634,8 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
 {
     // FIXME: only supports transfer sizes up to 255 bytes
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
-
+    enum pios_i2c_transfer_result result = PIOS_I2C_TRANSFER_UNSPECIFIED_ERROR;
+    
     bool valid = PIOS_I2C_validate(i2c_adapter);
 
     PIOS_Assert(valid)
@@ -624,13 +650,13 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     portTickType timeout;
     timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
     if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
 #else
     PIOS_IRQ_Disable();
     if (i2c_adapter->busy) {
         PIOS_IRQ_Enable();
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
     i2c_adapter->busy = 1;
     PIOS_IRQ_Enable();
@@ -638,6 +664,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
 
     PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED);
 
+    i2c_adapter->transfer_result = &result;
     i2c_adapter->last_txn   = &txn_list[num_txns - 1];
     i2c_adapter->active_txn = &txn_list[0];
     i2c_adapter->bus_error  = false;
@@ -659,30 +686,22 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     xSemaphoreGive(i2c_adapter->sem_ready);
 #endif /* PIOS_INCLUDE_FREERTOS */
 
-#ifdef PIOS_INCLUDE_FREERTOS
-    /* Unlock the bus */
-    xSemaphoreGive(i2c_adapter->sem_busy);
+    /* After this point, the bus is already unlocked from ISR
+     * Touching i2c_adapter is not allowed */
 #if defined(PIOS_I2C_DIAGNOSTICS)
     if (!semaphore_success) {
         i2c_timeout_counter++;
+        result = PIOS_I2C_TRANSFER_TIMEOUT;
     }
 #endif
-#else
-    PIOS_IRQ_Disable();
-    i2c_adapter->busy = 0;
-    PIOS_IRQ_Enable();
-#endif /* PIOS_INCLUDE_FREERTOS */
-
-    return !semaphore_success ? -2 :
-           i2c_adapter->bus_error ? -1 :
-           i2c_adapter->nack ? -3 :
-           0;
+    return result;
 }
 
 static int32_t PIOS_I2C_Transfer_Callback_Internal(struct pios_i2c_adapter *i2c_adapter, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback)
 {
     PIOS_DEBUG_Assert(i2c_adapter->curr_state == I2C_STATE_STOPPED);
 
+    i2c_adapter->transfer_result = 0;
     i2c_adapter->last_txn   = &txn_list[num_txns - 1];
     i2c_adapter->active_txn = &txn_list[0];
     i2c_adapter->bus_error  = false;
@@ -773,33 +792,6 @@ int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c
     return PIOS_I2C_Transfer_Callback_Internal(i2c_adapter, txn_list, num_txns, callback);
 }
 
-
-static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, bool *woken)
-{
-#ifdef PIOS_INCLUDE_FREERTOS
-    /* Unlock the bus */
-    signed portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
-    xSemaphoreGiveFromISR(i2c_adapter->sem_busy, &xHigherPriorityTaskWoken);
-    if (xHigherPriorityTaskWoken == pdTRUE) {
-        *woken = true;
-    }
-#else
-    PIOS_IRQ_Disable();
-    i2c_adapter->busy = 0;
-    PIOS_IRQ_Enable();
-#endif /* PIOS_INCLUDE_FREERTOS */
-
-
-    // Execute user supplied function
-
-    if (i2c_adapter->callback()) {
-        *woken = true;
-    }
-
-    return !i2c_adapter->bus_error;
-}
-
-
 void PIOS_I2C_EV_IRQ_Handler(uint32_t i2c_id)
 {
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
diff --git a/flight/pios/stm32f4xx/pios_i2c.c b/flight/pios/stm32f4xx/pios_i2c.c
index 308ed8236..92bb65bc5 100644
--- a/flight/pios/stm32f4xx/pios_i2c.c
+++ b/flight/pios/stm32f4xx/pios_i2c.c
@@ -888,7 +888,7 @@ static bool i2c_adapter_callback_handler(struct pios_i2c_adapter *i2c_adapter, s
 
 
     // Execute user supplied function
-    if (i2c_adapter->callback()) {
+    if (i2c_adapter->callback(PIOS_I2C_TRANSFER_OK)) {
         *pxHigherPriorityTaskWoken = pdTRUE;
     }
 
@@ -1048,7 +1048,7 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     struct pios_i2c_adapter *i2c_adapter = (struct pios_i2c_adapter *)i2c_id;
 
     if (!PIOS_I2C_validate(i2c_adapter)) {
-        return -1;
+        return PIOS_I2C_TRANSFER_DEVICE_ERROR;
     }
 
     PIOS_DEBUG_Assert(txn_list);
@@ -1061,13 +1061,13 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     portTickType timeout;
     timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
     if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
 #else
     PIOS_IRQ_Disable();
     if (i2c_adapter->busy) {
         PIOS_IRQ_Enable();
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
     i2c_adapter->busy = 1;
     PIOS_IRQ_Enable();
@@ -1134,10 +1134,10 @@ int32_t PIOS_I2C_Transfer(uint32_t i2c_id, const struct pios_i2c_txn txn_list[],
     PIOS_IRQ_Enable();
 #endif /* USE_FREERTOS */
 
-    return !semaphore_success ? -2 :
-           i2c_adapter->bus_error ? -1 :
-           i2c_adapter->nack ? -3 :
-           0;
+    return !semaphore_success ? PIOS_I2C_TRANSFER_TIMEOUT :
+           i2c_adapter->bus_error ? PIOS_I2C_TRANSFER_BUS_ERROR :
+           i2c_adapter->nack ? PIOS_I2C_TRANSFER_NACK :
+           PIOS_I2C_TRANSFER_OK;
 }
 
 static int32_t PIOS_I2C_Transfer_Callback_Internal(struct pios_i2c_adapter *i2c_adapter, const struct pios_i2c_txn txn_list[], uint32_t num_txns, pios_i2c_callback callback)
@@ -1182,13 +1182,13 @@ int32_t PIOS_I2C_Transfer_Callback(uint32_t i2c_id, const struct pios_i2c_txn tx
     portTickType timeout;
     timeout = i2c_adapter->cfg->transfer_timeout_ms / portTICK_RATE_MS;
     if (xSemaphoreTake(i2c_adapter->sem_busy, timeout) == pdFALSE) {
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
 #else
     PIOS_IRQ_Disable();
     if (i2c_adapter->busy) {
         PIOS_IRQ_Enable();
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
     i2c_adapter->busy = 1;
     PIOS_IRQ_Enable();
@@ -1220,13 +1220,13 @@ int32_t PIOS_I2C_Transfer_CallbackFromISR(uint32_t i2c_id, const struct pios_i2c
     }
 
     if (!locked) {
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
 #else
     PIOS_IRQ_Disable();
     if (i2c_adapter->busy) {
         PIOS_IRQ_Enable();
-        return -2;
+        return PIOS_I2C_TRANSFER_BUSY;
     }
     i2c_adapter->busy = 1;
     PIOS_IRQ_Enable();