diff --git a/Makefile b/Makefile
index 2ee87a465..f22bde515 100755
--- a/Makefile
+++ b/Makefile
@@ -575,7 +575,7 @@ OPTIMIZE	 = -Os
 LTO_FLAGS	 = -flto -fuse-linker-plugin $(OPTIMIZE)
 endif
 
-DEBUG_FLAGS	 = -ggdb3
+DEBUG_FLAGS	 = -ggdb3 -DDEBUG
 
 CFLAGS		 = $(ARCH_FLAGS) \
 		   $(LTO_FLAGS) \
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 6cb1a7569..6189322f5 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -840,7 +840,7 @@ void readEEPROM(void)
 {
     // Sanity check
     if (!isEEPROMContentValid())
-        failureMode(10);
+        failureMode(FAILURE_INVALID_EEPROM_CONTENTS);
 
     suspendRxSignal();
 
@@ -920,7 +920,7 @@ void writeEEPROM(void)
 
     // Flash write failed - just die now
     if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
-        failureMode(10);
+        failureMode(FAILURE_FLASH_WRITE_FAILED);
     }
 
     resumeRxSignal();
diff --git a/src/main/drivers/accgyro_adxl345.c b/src/main/drivers/accgyro_adxl345.c
index f1466accd..82082e161 100644
--- a/src/main/drivers/accgyro_adxl345.c
+++ b/src/main/drivers/accgyro_adxl345.c
@@ -57,7 +57,7 @@
 #define ADXL345_FIFO_STREAM 0x80
 
 static void adxl345Init(void);
-static void adxl345Read(int16_t *accelData);
+static bool adxl345Read(int16_t *accelData);
 
 static bool useFifo = false;
 
@@ -96,7 +96,7 @@ static void adxl345Init(void)
 
 uint8_t acc_samples = 0;
 
-static void adxl345Read(int16_t *accelData)
+static bool adxl345Read(int16_t *accelData)
 {
     uint8_t buf[8];
 
@@ -109,7 +109,11 @@ static void adxl345Read(int16_t *accelData)
 
         do {
             i++;
-            i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf);
+
+            if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
+                return false;;
+            }
+
             x += (int16_t)(buf[0] + (buf[1] << 8));
             y += (int16_t)(buf[2] + (buf[3] << 8));
             z += (int16_t)(buf[4] + (buf[5] << 8));
@@ -120,9 +124,15 @@ static void adxl345Read(int16_t *accelData)
         accelData[2] = z / i;
         acc_samples = i;
     } else {
-        i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
+
+        if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) {
+            return false;
+        }
+
         accelData[0] = buf[0] + (buf[1] << 8);
         accelData[1] = buf[2] + (buf[3] << 8);
         accelData[2] = buf[4] + (buf[5] << 8);
     }
+
+    return true;
 }
diff --git a/src/main/drivers/accgyro_bma280.c b/src/main/drivers/accgyro_bma280.c
index 536401ea8..193ae09ce 100644
--- a/src/main/drivers/accgyro_bma280.c
+++ b/src/main/drivers/accgyro_bma280.c
@@ -33,7 +33,7 @@
 #define BMA280_PMU_RANGE   0x0F
 
 static void bma280Init(void);
-static void bma280Read(int16_t *accelData);
+static bool bma280Read(int16_t *accelData);
 
 bool bma280Detect(acc_t *acc)
 {
@@ -57,14 +57,18 @@ static void bma280Init(void)
     acc_1G = 512 * 8;
 }
 
-static void bma280Read(int16_t *accelData)
+static bool bma280Read(int16_t *accelData)
 {
     uint8_t buf[6];
 
-    i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf);
+    if (!i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) {
+        return false;
+    }
 
     // Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
     accelData[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
     accelData[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
     accelData[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
+
+    return true;
 }
diff --git a/src/main/drivers/accgyro_l3g4200d.c b/src/main/drivers/accgyro_l3g4200d.c
index dd083ea73..82bd43e76 100644
--- a/src/main/drivers/accgyro_l3g4200d.c
+++ b/src/main/drivers/accgyro_l3g4200d.c
@@ -57,7 +57,7 @@
 static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
 
 static void l3g4200dInit(void);
-static void l3g4200dRead(int16_t *gyroADC);
+static bool l3g4200dRead(int16_t *gyroADC);
 
 bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
 {
@@ -103,20 +103,24 @@ static void l3g4200dInit(void)
 
     ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
     if (!ack)
-        failureMode(3);
+        failureMode(FAILURE_ACC_INIT);
 
     delay(5);
     i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
-static void l3g4200dRead(int16_t *gyroADC)
+static bool l3g4200dRead(int16_t *gyroADC)
 {
     uint8_t buf[6];
 
-    i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
+    if (!i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
+        return false;
+    }
 
     gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
     gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
     gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
diff --git a/src/main/drivers/accgyro_l3gd20.c b/src/main/drivers/accgyro_l3gd20.c
index 2253548d3..fd56d2512 100644
--- a/src/main/drivers/accgyro_l3gd20.c
+++ b/src/main/drivers/accgyro_l3gd20.c
@@ -68,8 +68,8 @@
 
 static void l3gd20SpiInit(SPI_TypeDef *SPIx)
 {
+    UNUSED(SPIx); // FIXME
     GPIO_InitTypeDef GPIO_InitStructure;
-    SPI_InitTypeDef SPI_InitStructure;
 
     RCC_AHBPeriphClockCmd(L3GD20_CS_GPIO_CLK_PERIPHERAL, ENABLE);
 
@@ -120,7 +120,7 @@ void l3gd20GyroInit(void)
     delay(100);
 }
 
-static void l3gd20GyroRead(int16_t *gyroADC)
+static bool l3gd20GyroRead(int16_t *gyroADC)
 {
     uint8_t buf[6];
 
@@ -143,6 +143,8 @@ static void l3gd20GyroRead(int16_t *gyroADC)
     debug[1] = (int16_t)((buf[3] << 8) | buf[2]);
     debug[2] = (int16_t)((buf[5] << 8) | buf[4]);
 #endif
+
+    return true;
 }
 
 // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
diff --git a/src/main/drivers/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_lsm303dlhc.c
index 4aaa7188d..fecf4801a 100644
--- a/src/main/drivers/accgyro_lsm303dlhc.c
+++ b/src/main/drivers/accgyro_lsm303dlhc.c
@@ -131,14 +131,15 @@ void lsm303dlhcAccInit(void)
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
-static void lsm303dlhcAccRead(int16_t *gyroADC)
+static bool lsm303dlhcAccRead(int16_t *gyroADC)
 {
     uint8_t buf[6];
 
-    bool ok = i2cRead(LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf);
+    bool ack = i2cRead(LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf);
 
-    if (!ok)
-        return;
+    if (!ack) {
+        return false;
+    }
 
     // the values range from -8192 to +8191
     gyroADC[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2;
@@ -150,6 +151,8 @@ static void lsm303dlhcAccRead(int16_t *gyroADC)
     debug[1] = (int16_t)((buf[3] << 8) | buf[2]);
     debug[2] = (int16_t)((buf[5] << 8) | buf[4]);
 #endif
+
+    return true;
 }
 
 bool lsm303dlhcAccDetect(acc_t *acc)
diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c
index ab3a01aca..bf39f41b0 100644
--- a/src/main/drivers/accgyro_mma845x.c
+++ b/src/main/drivers/accgyro_mma845x.c
@@ -78,7 +78,7 @@
 static uint8_t device_id;
 
 static void mma8452Init(void);
-static void mma8452Read(int16_t *accelData);
+static bool mma8452Read(int16_t *accelData);
 
 bool mma8452Detect(acc_t *acc)
 {
@@ -132,12 +132,17 @@ static void mma8452Init(void)
     acc_1G = 256;
 }
 
-static void mma8452Read(int16_t *accelData)
+static bool mma8452Read(int16_t *accelData)
 {
     uint8_t buf[6];
 
-    i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf);
+    if (!i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf)) {
+        return false;
+    }
+
     accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
     accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
     accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
+
+    return true;
 }
diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c
index ff90dd06c..055edd940 100644
--- a/src/main/drivers/accgyro_mpu3050.c
+++ b/src/main/drivers/accgyro_mpu3050.c
@@ -58,8 +58,8 @@
 static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
 
 static void mpu3050Init(void);
-static void mpu3050Read(int16_t *gyroADC);
-static void mpu3050ReadTemp(int16_t *tempData);
+static bool mpu3050Read(int16_t *gyroADC);
+static bool mpu3050ReadTemp(int16_t *tempData);
 
 bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
 {
@@ -112,7 +112,7 @@ static void mpu3050Init(void)
 
     ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
     if (!ack)
-        failureMode(3);
+        failureMode(FAILURE_ACC_INIT);
 
     i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
     i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
@@ -121,21 +121,29 @@ static void mpu3050Init(void)
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
-static void mpu3050Read(int16_t *gyroADC)
+static bool mpu3050Read(int16_t *gyroADC)
 {
     uint8_t buf[6];
 
-    i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
+    if (!i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf)) {
+        return false;
+    }
 
     gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
     gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
     gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
 
-static void mpu3050ReadTemp(int16_t *tempData)
+static bool mpu3050ReadTemp(int16_t *tempData)
 {
     uint8_t buf[2];
-    i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);
+    if (!i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf)) {
+        return false;
+    }
 
     *tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280;
+
+    return true;
 }
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index b67da25d2..8b71ad2cb 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -173,9 +173,9 @@ enum accel_fsr_e {
 
 static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
 static void mpu6050AccInit(void);
-static void mpu6050AccRead(int16_t *accData);
+static bool mpu6050AccRead(int16_t *accData);
 static void mpu6050GyroInit(void);
-static void mpu6050GyroRead(int16_t *gyroADC);
+static bool mpu6050GyroRead(int16_t *gyroADC);
 
 typedef enum {
     MPU_6050_HALF_RESOLUTION,
@@ -336,13 +336,13 @@ bool mpu6050AccDetect(const mpu6050Config_t *configToUse, acc_t *acc)
         } else if (revision == 2) {
             mpuAccelTrim = MPU_6050_FULL_RESOLUTION;
         } else {
-            failureMode(5);
+            failureMode(FAILURE_ACC_INCOMPATIBLE);
         }
     } else {
         i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
         revision = productId & 0x0F;
         if (!revision) {
-            failureMode(5);
+            failureMode(FAILURE_ACC_INCOMPATIBLE);
         } else if (revision == 4) {
             mpuAccelTrim = MPU_6050_HALF_RESOLUTION;
         } else {
@@ -406,52 +406,62 @@ static void mpu6050AccInit(void)
     }
 }
 
-static void mpu6050AccRead(int16_t *accData)
+static bool mpu6050AccRead(int16_t *accData)
 {
     uint8_t buf[6];
 
-    if (!i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf)) {
-        return;
+    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
+    if (!ack) {
+        return false;
     }
 
     accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
     accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
     accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
 
 static void mpu6050GyroInit(void)
 {
     mpu6050GpioInit();
 
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
+    bool ack;
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure 
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 
     // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
     // Accel scale 8g (4096 LSB/g)
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
-            0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
+//    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
+//            0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
+            0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
 
 #ifdef USE_MPU_DATA_READY_SIGNAL
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
+    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 #endif
+    UNUSED(ack);
 }
 
-static void mpu6050GyroRead(int16_t *gyroADC)
+static bool mpu6050GyroRead(int16_t *gyroADC)
 {
     uint8_t buf[6];
 
-    if (!i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf)) {
-        return;
+    bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
+    if (!ack) {
+        return false;
     }
 
     gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
     gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
     gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c
index 56406ffaf..3bee15fbe 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.c
+++ b/src/main/drivers/accgyro_spi_mpu6000.c
@@ -125,8 +125,8 @@ static bool mpuSpi6000InitDone = false;
 #define DISABLE_MPU6000       GPIO_SetBits(MPU6000_CS_GPIO,   MPU6000_CS_PIN)
 #define ENABLE_MPU6000        GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
 
-void mpu6000SpiGyroRead(int16_t *gyroADC);
-void mpu6000SpiAccRead(int16_t *gyroADC);
+bool mpu6000SpiGyroRead(int16_t *gyroADC);
+bool mpu6000SpiAccRead(int16_t *gyroADC);
 
 static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
 {
@@ -313,7 +313,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
     return true;
 }
 
-void mpu6000SpiGyroRead(int16_t *gyroData)
+bool mpu6000SpiGyroRead(int16_t *gyroData)
 {
     uint8_t buf[6];
 
@@ -324,9 +324,11 @@ void mpu6000SpiGyroRead(int16_t *gyroData)
     gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
     gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
     gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
 
-void mpu6000SpiAccRead(int16_t *gyroData)
+bool mpu6000SpiAccRead(int16_t *gyroData)
 {
     uint8_t buf[6];
 
@@ -337,4 +339,6 @@ void mpu6000SpiAccRead(int16_t *gyroData)
     gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
     gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
     gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h
index d296b0427..26e5ba037 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.h
+++ b/src/main/drivers/accgyro_spi_mpu6000.h
@@ -16,5 +16,5 @@
 bool mpu6000SpiAccDetect(acc_t *acc);
 bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
 
-void mpu6000SpiGyroRead(int16_t *gyroADC);
-void mpu6000SpiAccRead(int16_t *gyroADC);
+bool mpu6000SpiGyroRead(int16_t *gyroADC);
+bool mpu6000SpiAccRead(int16_t *gyroADC);
diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
index e83c5d888..08b14ebbf 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro_spi_mpu6500.c
@@ -72,9 +72,9 @@ enum accel_fsr_e {
 static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
 
 static void mpu6500AccInit(void);
-static void mpu6500AccRead(int16_t *accData);
+static bool mpu6500AccRead(int16_t *accData);
 static void mpu6500GyroInit(void);
-static void mpu6500GyroRead(int16_t *gyroADC);
+static bool mpu6500GyroRead(int16_t *gyroADC);
 
 extern uint16_t acc_1G;
 
@@ -194,7 +194,7 @@ static void mpu6500AccInit(void)
     acc_1G = 512 * 8;
 }
 
-static void mpu6500AccRead(int16_t *accData)
+static bool mpu6500AccRead(int16_t *accData)
 {
     uint8_t buf[6];
 
@@ -203,6 +203,8 @@ static void mpu6500AccRead(int16_t *accData)
     accData[X] = (int16_t)((buf[0] << 8) | buf[1]);
     accData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
     accData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
 
 static void mpu6500GyroInit(void)
@@ -229,7 +231,7 @@ static void mpu6500GyroInit(void)
     mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
 }
 
-static void mpu6500GyroRead(int16_t *gyroADC)
+static bool mpu6500GyroRead(int16_t *gyroADC)
 {
     uint8_t buf[6];
 
@@ -238,4 +240,6 @@ static void mpu6500GyroRead(int16_t *gyroADC)
     gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
     gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
     gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+
+    return true;
 }
diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c
index c0aeca930..365e24968 100644
--- a/src/main/drivers/compass_ak8975.c
+++ b/src/main/drivers/compass_ak8975.c
@@ -111,7 +111,7 @@ void ak8975Init()
 #define BIT_STATUS2_REG_DATA_ERROR              (1 << 2)
 #define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW     (1 << 3)
 
-void ak8975Read(int16_t *magData)
+bool ak8975Read(int16_t *magData)
 {
     bool ack;
     UNUSED(ack);
@@ -120,7 +120,7 @@ void ak8975Read(int16_t *magData)
 
     ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
     if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
-        return;
+        return false;
     }
 
 #if 1 // USE_I2C_SINGLE_BYTE_READS
@@ -129,22 +129,22 @@ void ak8975Read(int16_t *magData)
     for (uint8_t i = 0; i < 6; i++) {
         ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
         if (!ack) {
-            break;
+            break false
         }
     }
 #endif
 
     ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
     if (!ack) {
-        return;
+        return false;
     }
 
     if (status & BIT_STATUS2_REG_DATA_ERROR) {
-        return;
+        return false;
     }
 
     if (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW) {
-        return;
+        return false;
     }
 
     magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * 4;
@@ -153,4 +153,5 @@ void ak8975Read(int16_t *magData)
 
 
     ack = i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
+    return true;
 }
diff --git a/src/main/drivers/compass_ak8975.h b/src/main/drivers/compass_ak8975.h
index d0d78ecdf..f80e64613 100644
--- a/src/main/drivers/compass_ak8975.h
+++ b/src/main/drivers/compass_ak8975.h
@@ -19,4 +19,4 @@
 
 bool ak8975detect(mag_t *mag);
 void ak8975Init(void);
-void ak8975Read(int16_t *magData);
+bool ak8975Read(int16_t *magData);
diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c
index 4bfdfa41e..a59186d51 100644
--- a/src/main/drivers/compass_hmc5883l.c
+++ b/src/main/drivers/compass_hmc5883l.c
@@ -305,14 +305,19 @@ void hmc5883lInit(void)
     hmc5883lConfigureDataReadyInterruptHandling();
 }
 
-void hmc5883lRead(int16_t *magData)
+bool hmc5883lRead(int16_t *magData)
 {
     uint8_t buf[6];
 
-    i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
+    bool ack = i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
+    if (ack) {
+        return false;
+    }
     // During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
     // After calibration is done, magGain is set to calculated gain values.
     magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
     magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
     magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
+
+    return true;
 }
diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h
index a4338849b..53c4c9f3e 100644
--- a/src/main/drivers/compass_hmc5883l.h
+++ b/src/main/drivers/compass_hmc5883l.h
@@ -35,4 +35,4 @@ typedef struct hmc5883Config_s {
 
 bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
 void hmc5883lInit(void);
-void hmc5883lRead(int16_t *magData);
+bool hmc5883lRead(int16_t *magData);
diff --git a/src/main/drivers/sensor.h b/src/main/drivers/sensor.h
index f494c5713..e2e30b779 100644
--- a/src/main/drivers/sensor.h
+++ b/src/main/drivers/sensor.h
@@ -18,4 +18,4 @@
 #pragma once
 
 typedef void (*sensorInitFuncPtr)(void);                    // sensor init prototype
-typedef void (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
+typedef bool (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index ba4dd6b0a..0f99b1bec 100644
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -47,7 +47,7 @@ void registerExti15_10_CallbackHandler(extiCallbackHandler *fn)
             return;
         }
     }
-    failureMode(15); // EXTI15_10_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
+    failureMode(FAILURE_DEVELOPER); // EXTI15_10_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
 }
 
 void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn)
@@ -194,21 +194,49 @@ void delay(uint32_t ms)
         delayMicroseconds(1000);
 }
 
-// FIXME replace mode with an enum so usage can be tracked, currently mode is a magic number
-void failureMode(uint8_t mode)
-{
-    uint8_t flashesRemaining = 10;
+#define SHORT_FLASH_DURATION 50
+#define CODE_FLASH_DURATION 250
 
-    LED1_ON;
-    LED0_OFF;
-    while (flashesRemaining--) {
-        LED1_TOGGLE;
-        LED0_TOGGLE;
-        delay(475 * mode - 2);
-        BEEP_ON;
-        delay(25);
-        BEEP_OFF;
+void failureMode(failureMode_e mode)
+{
+    int codeRepeatsRemaining = 10;
+    int codeFlashesRemaining;
+    int shortFlashesRemaining;
+
+    while (codeRepeatsRemaining--) {
+        LED1_ON;
+        LED0_OFF;
+        shortFlashesRemaining = 5;
+        codeFlashesRemaining = mode + 1;
+        uint8_t flashDuration = SHORT_FLASH_DURATION;
+
+        while (shortFlashesRemaining || codeFlashesRemaining) {
+            LED1_TOGGLE;
+            LED0_TOGGLE;
+            BEEP_ON;
+            delay(flashDuration);
+
+            LED1_TOGGLE;
+            LED0_TOGGLE;
+            BEEP_OFF;
+            delay(flashDuration);
+
+            if (shortFlashesRemaining) {
+                shortFlashesRemaining--;
+                if (shortFlashesRemaining == 0) {
+                    delay(500);
+                    flashDuration = CODE_FLASH_DURATION;
+                }
+            } else {
+                codeFlashesRemaining--;
+            }
+        }
+        delay(1000);
     }
 
+#ifdef DEBUG
+    systemReset();
+#else
     systemResetToBootloader();
+#endif
 }
diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h
index 33f3108a0..67ea4a3f4 100644
--- a/src/main/drivers/system.h
+++ b/src/main/drivers/system.h
@@ -42,3 +42,14 @@ void registerExti15_10_CallbackHandler(extiCallbackHandler *fn);
 void unregisterExti15_10_CallbackHandler(extiCallbackHandler *fn);
 
 extern uint32_t cachedRccCsrValue;
+
+typedef enum {
+    FAILURE_DEVELOPER = 0,
+    FAILURE_MISSING_ACC,
+    FAILURE_ACC_INIT,
+    FAILURE_ACC_INCOMPATIBLE,
+    FAILURE_INVALID_EEPROM_CONTENTS,
+    FAILURE_FLASH_WRITE_FAILED
+} failureMode_e;
+
+#define FAILURE_MODE_COUNT 4
diff --git a/src/main/main.c b/src/main/main.c
index 6f5f04f74..2626224c9 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -373,7 +373,7 @@ void init(void)
 
     if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, masterConfig.mag_hardware, masterConfig.baro_hardware, currentProfile->mag_declination)) {
         // if gyro was not detected due to whatever reason, we give up now.
-        failureMode(3);
+        failureMode(FAILURE_MISSING_ACC);
     }
 
     systemState |= SYSTEM_STATE_SENSORS_READY;
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 152cfdd60..17d58e866 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -171,7 +171,9 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
 
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
 {
-    acc.read(accADC);
+    if (!acc.read(accADC)) {
+        return;
+    }
     alignSensors(accADC, accADC, accAlign);
 
     if (!isAccelerationCalibrationComplete()) {
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 4a631a5a3..4f703b676 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -116,10 +116,10 @@ static void applyGyroZero(void)
 
 void gyroUpdate(void)
 {
-    // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time.
-
     // range: +/- 8192; +/- 2000 deg/sec
-    gyro.read(gyroADC);
+    if (!gyro.read(gyroADC)) {
+        return;
+    }
     alignSensors(gyroADC, gyroADC, gyroAlign);
 
     if (!isGyroCalibrationComplete()) {
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 9505bbc1e..333e39c0a 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -125,11 +125,13 @@ const mpu6050Config_t *selectMPU6050Config(void)
 
 #ifdef USE_FAKE_GYRO
 static void fakeGyroInit(void) {}
-static void fakeGyroRead(int16_t *gyroADC) {
+static bool fakeGyroRead(int16_t *gyroADC) {
     memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
+    return true;
 }
-static void fakeGyroReadTemp(int16_t *tempData) {
+static bool fakeGyroReadTemp(int16_t *tempData) {
     UNUSED(tempData);
+    return true;
 }
 
 bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
@@ -144,8 +146,9 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
 
 #ifdef USE_FAKE_ACC
 static void fakeAccInit(void) {}
-static void fakeAccRead(int16_t *accData) {
+static bool fakeAccRead(int16_t *accData) {
     memset(accData, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
+    return true;
 }
 
 bool fakeAccDetect(acc_t *acc)