diff --git a/Makefile b/Makefile
index 63830bd45b..e5b574b082 100755
--- a/Makefile
+++ b/Makefile
@@ -74,6 +74,8 @@ USBPERIPH_SRC = $(notdir $(wildcard $(USBFS_DIR)/src/*.c))
 
 ifeq ($(TARGET),$(filter $(TARGET),STM32F3DISCOVERY CHEBUZZF3 NAZE32PRO SPRACINGF3 SPARKY ALIENWIIF3 COLIBRI_RACE MOTOLAB))
 
+CSOURCES        := $(shell find $(SRC_DIR) -name '*.c')
+
 STDPERIPH_DIR	= $(ROOT)/lib/main/STM32F30x_StdPeriph_Driver
 
 STDPERIPH_SRC = $(notdir $(wildcard $(STDPERIPH_DIR)/src/*.c))
@@ -637,6 +639,11 @@ LDFLAGS		 = -lm \
 # No user-serviceable parts below
 ###############################################################################
 
+CPPCHECK         = cppcheck $(CSOURCES) --enable=all --platform=unix64 \
+		   --std=c99 --inline-suppr --quiet --force \
+		   $(addprefix -I,$(INCLUDE_DIRS)) \
+		   -I/usr/include -I/usr/include/linux
+
 #
 # Things we will build
 #
@@ -706,6 +713,13 @@ unbrick_$(TARGET): $(TARGET_HEX)
 
 unbrick: unbrick_$(TARGET)
 
+## cppcheck    : run static analysis on C source code
+cppcheck: $(CSOURCES)
+	$(CPPCHECK)
+
+cppcheck-result.xml: $(CSOURCES)
+	$(CPPCHECK) --xml-version=2 2> cppcheck-result.xml
+
 help:
 	@echo ""
 	@echo "Makefile for the $(FORKNAME) firmware"
diff --git a/docs/Cli.md b/docs/Cli.md
index 6c4dbe9d68..3fe6d7cb33 100644
--- a/docs/Cli.md
+++ b/docs/Cli.md
@@ -198,9 +198,9 @@ Re-apply any new defaults as desired.
 | `yaw_rate`                      |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 0             | Rate Profile | UINT8    |
 | `tpa_rate`                      | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 | 0      | 100    | 0             | Rate Profile | UINT8    |
 | `tpa_breakpoint`                | See tpa_rate.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          | 1000   | 2000   | 1500          | Rate Profile | UINT16   |
-| `failsafe_delay`                |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 200    | 10            | Profile      | UINT8    |
-| `failsafe_off_delay`            |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 200    | 200           | Profile      | UINT8    |
-| `failsafe_throttle`             |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 1000   | 2000   | 1200          | Profile      | UINT16   |
+| `failsafe_delay`                | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay).                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  | 0      | 200    | 10            | Profile      | UINT8    |
+| `failsafe_off_delay`            | Time in deciseconds to wait before turning off motors when failsafe is activated. See [Failsafe documentation](Failsafe.md#failsafe_off_delay).                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 200    | 200           | Profile      | UINT8    |
+| `failsafe_throttle`             | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle).                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 | 1000   | 2000   | 1000          | Profile      | UINT16   |
 | `rx_min_usec`                   | Defines the shortest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 2000   | 985           | Profile      | UINT16   |
 | `rx_max_usec`                   | Defines the longest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 3000   | 2115          | Profile      | UINT16   |
 | `gimbal_mode`                  | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal                                                                                                                                                                                                                                                                                                    | 0      | 1      | 0             | Profile      | UINT8    |
diff --git a/docs/Mixer.md b/docs/Mixer.md
index 144156e4e8..8a4d1408a1 100644
--- a/docs/Mixer.md
+++ b/docs/Mixer.md
@@ -42,6 +42,10 @@ You can also use the Command Line Interface (CLI) to set the mixer type:
 | CUSTOM AIRPLANE  | User-defined airplane     |                |                  |
 | CUSTOM TRICOPTER | User-defined tricopter    |                |                  |
 
+## Servo configuration
+
+The cli `servo` command defines the settings for the servo outputs. 
+The cli mixer `smix` command controllers how the mixer maps internal FC data (RC input, PID stabilisation output, channel forwarding, etc) to servo outputs.
 
 ## Servo filtering
 
@@ -98,6 +102,7 @@ Note: the `mmix` command may show a motor mix that is not active, custom motor m
 Custom servo mixing rules can be applied to each servo.  Rules are applied in the order they are defined.
 
 | id | Servo slot |
+|----|--------------|
 | 0  | GIMBAL PITCH |
 | 1  | GIMBAL ROLL |
 | 2  | FLAPS |
@@ -109,7 +114,7 @@ Custom servo mixing rules can be applied to each servo.  Rules are applied in th
 
 
 | id | Input sources |
-| -- | ------------- |
+|----|-----------------|
 | 0  | Stabilised ROLL |
 | 1  | Stabilised PITCH |
 | 2  | Stabilised YAW |
diff --git a/docs/development/Development.md b/docs/development/Development.md
index 873d5de259..3a97075cf8 100755
--- a/docs/development/Development.md
+++ b/docs/development/Development.md
@@ -13,7 +13,7 @@ This document is primarily for developers only.
 7. Don't be afraid of moving code to a new file - it helps to reduce test dependencies.
 8. Avoid noise-words in variable names, like 'data' or 'info'.  Think about what you're naming and name it well.  Don't be afraid to rename anything.
 9. Avoid comments that describe what the code is doing, the code should describe itself.  Comments are useful however for big-picture purposes and to document content of variables.
-10. If you need to document a variable do it at the declarion, don't copy the comment to the `extern` usage since it will lead to comment rot.
+10. If you need to document a variable do it at the declaration, don't copy the comment to the `extern` usage since it will lead to comment rot.
 11. Seek advice from other developers - know you can always learn more.
 12. Be professional - attempts at humor or slating existing code in the codebase itself is not helpful when you have to change/fix it.
 13. Know that there's always more than one way to do something and that code is never final - but it does have to work.
@@ -46,16 +46,15 @@ Note: Tests are written in C++ and linked with with firmware's C code.
 
 ### Running the tests.
 
-The tests and test build system is very simple and based of the googletest example files, it will be improved in due course.
+The tests and test build system is very simple and based off the googletest example files, it will be improved in due course. From the root folder of the project simply do:
 
 ```
-cd test
-make
+make test
 ```
 
-This will build a set of executable files, one for each `*_unittest.cc` file.
+This will build a set of executable files in the `obj/test` folder, one for each `*_unittest.cc` file.
 
-You can run them on the command line to execute the tests and to see the test report.
+After they have been executed by the make invocation, you can still run them on the command line to execute the tests and to see the test report.
 
 You can also step-debug the tests in eclipse and you can use the GoogleTest test runner to make building and re-running the tests simple.
 
@@ -75,7 +74,7 @@ https://help.github.com/articles/creating-a-pull-request/
 
 The main flow for a contributing is as follows:
 
-1. Login to github, goto the cleanflight repository and press `fork`.
+1. Login to github, go to the cleanflight repository and press `fork`.
 2. Then using the command line/terminal on your computer: `git clone <url to YOUR fork>`
 3. `cd cleanflight`
 4. `git checkout master`
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 4f23b3c2ee..0c77728bef 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -807,6 +807,11 @@ void validateAndFixConfig(void)
     }
 #endif
 
+#ifdef STM32F303xC
+    // hardware supports serial port inversion, make users life easier for those that want to connect SBus RX's
+    masterConfig.telemetryConfig.telemetry_inversion = 1;
+#endif
+
     /*
      * The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
      * The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
diff --git a/src/main/drivers/barometer_bmp085.c b/src/main/drivers/barometer_bmp085.c
index 4708215e99..a46b8f20c4 100644
--- a/src/main/drivers/barometer_bmp085.c
+++ b/src/main/drivers/barometer_bmp085.c
@@ -34,9 +34,8 @@
 #ifdef BARO
 
 #if defined(BARO_EOC_GPIO)
-// BMP085, Standard address 0x77
 static bool isConversionComplete = false;
-static uint16_t bmp085ConversionOverrun = 0;
+static bool isEOCConnected = true;
 
 // EXTI14 for BMP085 End of Conversion Interrupt
 void BMP085_EOC_EXTI_Handler(void) {
@@ -107,6 +106,10 @@ typedef struct {
 #define SMD500_PARAM_MI      3791        //calibration parameter
 
 STATIC_UNIT_TESTED bmp085_t bmp085;
+
+#define UT_DELAY    6000        // 1.5ms margin according to the spec (4.5ms T conversion time)
+#define UP_DELAY    27000       // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P conversion time with OSS=3)
+
 static bool bmp085InitDone = false;
 STATIC_UNIT_TESTED uint16_t bmp085_ut;  // static result of temperature measurement
 STATIC_UNIT_TESTED uint32_t bmp085_up;  // static result of pressure measurement
@@ -133,7 +136,6 @@ void bmp085InitXCLRGpio(const bmp085Config_t *config) {
 
     RCC_APB2PeriphClockCmd(config->gpioAPB2Peripherals, ENABLE);
 
-    // PC13, PC14 (Barometer XCLR reset output, EOC input)
     gpio.pin = config->xclrGpioPin;
     gpio.speed = Speed_2MHz;
     gpio.mode = Mode_Out_PP;
@@ -163,7 +165,7 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
         bmp085InitXCLRGpio(config);
 
         gpio.pin = config->eocGpioPin;
-        gpio.mode = Mode_IN_FLOATING;
+        gpio.mode = Mode_IPD;
         gpioInit(config->eocGpioPort, &gpio);
         BMP085_ON;
 
@@ -200,19 +202,22 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
             bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
             bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
             bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
-            bmp085InitDone = true;
-            baro->ut_delay = 6000; // 1.5ms margin according to the spec (4.5ms T conversion time)
-            baro->up_delay = 27000; // 6000+21000=27000 1.5ms margin according to the spec (25.5ms P conversion time with OSS=3)
+            baro->ut_delay = UT_DELAY;
+            baro->up_delay = UP_DELAY;
             baro->start_ut = bmp085_start_ut;
             baro->get_ut = bmp085_get_ut;
             baro->start_up = bmp085_start_up;
             baro->get_up = bmp085_get_up;
             baro->calculate = bmp085_calculate;
+#if defined(BARO_EOC_GPIO)
+            isEOCConnected = bmp085TestEOCConnected(config);
+#endif
+            bmp085InitDone = true;
             return true;
         }
     }
 
-#ifdef BARO_EOC_GPIO
+#if defined(BARO_EOC_GPIO)
     EXTI_InitTypeDef EXTI_InitStructure;
     EXTI_StructInit(&EXTI_InitStructure);
     EXTI_InitStructure.EXTI_Line = EXTI_Line14;
@@ -293,9 +298,9 @@ static void bmp085_get_ut(void)
     uint8_t data[2];
 
 #if defined(BARO_EOC_GPIO)
-    if (!isConversionComplete) {
-        bmp085ConversionOverrun++;
-        return; // keep old value
+    // return old baro value if conversion time exceeds datasheet max when EOC is connected
+    if ((isEOCConnected) && (!isConversionComplete)) {
+        return;
     }
 #endif
 
@@ -324,11 +329,10 @@ static void bmp085_get_up(void)
 {
     uint8_t data[3];
 
-#if  defined(BARO_EOC_GPIO)
-    // wait in case of cockup
-    if (!isConversionComplete) {
-        bmp085ConversionOverrun++;
-        return; // keep old value
+#if defined(BARO_EOC_GPIO)
+    // return old baro value if conversion time exceeds datasheet max when EOC is connected
+    if ((isEOCConnected) && (!isConversionComplete)) {
+        return;
     }
 #endif
 
@@ -372,4 +376,21 @@ static void bmp085_get_cal_param(void)
     bmp085.cal_param.md = (data[20] << 8) | data[21];
 }
 
+#if defined(BARO_EOC_GPIO)
+bool bmp085TestEOCConnected(const bmp085Config_t *config)
+{
+    if (!bmp085InitDone) {
+        bmp085_start_ut();
+        delayMicroseconds(UT_DELAY * 2); // wait twice as long as normal, just to be sure
+
+        // conversion should have finished now so check if EOC is high
+        uint8_t status = GPIO_ReadInputDataBit(config->eocGpioPort, config->eocGpioPin);
+        if (status) {
+            return true;
+        }
+    } 
+    return false; // assume EOC is not connected
+}
 #endif
+
+#endif /* BARO */
diff --git a/src/main/drivers/barometer_bmp085.h b/src/main/drivers/barometer_bmp085.h
index b0b3b74b3e..c16c688ae5 100644
--- a/src/main/drivers/barometer_bmp085.h
+++ b/src/main/drivers/barometer_bmp085.h
@@ -28,6 +28,10 @@ typedef struct bmp085Config_s {
 bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);
 void bmp085Disable(const bmp085Config_t *config);
 
+#if defined(BARO_EOC_GPIO)
+bool bmp085TestEOCConnected(const bmp085Config_t *config);
+#endif
+
 #ifdef UNIT_TEST
 void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState NewState);
 #endif
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index ba1c4a26cd..13c806291e 100755
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -622,12 +622,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
 
         if (type == MAP_TO_PPM_INPUT) {
 #ifdef CC3D
-            if (init->useOneshot) {
+            if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
                 ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4);
             }
 #endif
 #ifdef SPARKY
-            if (init->useOneshot) {
+            if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) {
                 ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
             }
 #endif
@@ -638,7 +638,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
         } else if (type == MAP_TO_MOTOR_OUTPUT) {
             if (init->useOneshot) {
                 pwmOneshotMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount);
-            } else if (init->motorPwmRate > 500) {
+            } else if (isMotorBrushed(init->motorPwmRate)) {
                 pwmBrushedMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
             } else {
                 pwmBrushlessMotorConfig(timerHardwarePtr, pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse);
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index ad7c88b502..be663abfa0 100644
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -35,6 +35,8 @@
 
 #define PWM_TIMER_MHZ 1
 #define ONESHOT125_TIMER_MHZ 8
+#define PWM_BRUSHED_TIMER_MHZ 8
+
 
 typedef struct sonarGPIOConfig_s {
     GPIO_TypeDef *gpio;
diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c
index 52ada5f8e5..4806f0df57 100644
--- a/src/main/drivers/pwm_output.c
+++ b/src/main/drivers/pwm_output.c
@@ -47,7 +47,6 @@ static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
 #ifdef USE_SERVOS
 static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
 #endif
-#define PWM_BRUSHED_TIMER_MHZ 8
 
 static uint8_t allocatedOutputPortCount = 0;
 
@@ -172,6 +171,11 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
     }
 }
 
+bool isMotorBrushed(uint16_t motorPwmRate)
+{
+    return (motorPwmRate > 500);
+}
+
 void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
 {
     uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h
index 4475e8b0d7..19c5aa8202 100644
--- a/src/main/drivers/pwm_output.h
+++ b/src/main/drivers/pwm_output.h
@@ -22,3 +22,5 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
 void pwmCompleteOneshotMotorUpdate(uint8_t motorCount);
 
 void pwmWriteServo(uint8_t index, uint16_t value);
+
+bool isMotorBrushed(uint16_t motorPwmRate);
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index f02b318f8a..037c424140 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -132,6 +132,17 @@ bool failsafeIsReceivingRxData(void)
     return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
 }
 
+void failsafeOnRxSuspend(uint32_t usSuspendPeriod)
+{
+    failsafeState.validRxDataReceivedAt += (usSuspendPeriod / 1000);    // / 1000 to convert micros to millis
+}
+
+void failsafeOnRxResume(void)
+{
+    failsafeState.validRxDataReceivedAt = millis();                     // prevent RX link down trigger, restart rx link up
+    failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;                     // do so while rx link is up
+}
+
 void failsafeOnValidDataReceived(void)
 {
     failsafeState.validRxDataReceivedAt = millis();
diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h
index 4e2c4b52d6..7d841dd111 100644
--- a/src/main/flight/failsafe.h
+++ b/src/main/flight/failsafe.h
@@ -73,6 +73,8 @@ failsafePhase_e failsafePhase();
 bool failsafeIsMonitoring(void);
 bool failsafeIsActive(void);
 bool failsafeIsReceivingRxData(void);
+void failsafeOnRxSuspend(uint32_t suspendPeriod);
+void failsafeOnRxResume(void);
 
 void failsafeOnValidDataReceived(void);
 void failsafeOnValidDataFailed(void);
diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c
index 429ffe8ade..bfc6007ddc 100644
--- a/src/main/io/flashfs.c
+++ b/src/main/io/flashfs.c
@@ -481,7 +481,7 @@ int flashfsIdentifyStartOfFreeSpace()
         /* We can choose whatever power of 2 size we like, which determines how much wastage of free space we'll have
          * at the end of the last written data. But smaller blocksizes will require more searching.
          */
-        FREE_BLOCK_SIZE = 65536,
+        FREE_BLOCK_SIZE = 2048,
 
         /* We don't expect valid data to ever contain this many consecutive uint32_t's of all 1 bits: */
         FREE_BLOCK_TEST_SIZE_INTS = 4, // i.e. 16 bytes
@@ -493,16 +493,20 @@ int flashfsIdentifyStartOfFreeSpace()
         uint32_t ints[FREE_BLOCK_TEST_SIZE_INTS];
     } testBuffer;
 
-    int left = 0;
-    int right = flashfsGetSize() / FREE_BLOCK_SIZE;
-    int mid, result = right;
+    int left = 0; // Smallest block index in the search region
+    int right = flashfsGetSize() / FREE_BLOCK_SIZE; // One past the largest block index in the search region
+    int mid;
+    int result = right;
     int i;
     bool blockErased;
 
     while (left < right) {
         mid = (left + right) / 2;
 
-        m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES);
+        if (m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES) < FREE_BLOCK_TEST_SIZE_BYTES) {
+            // Unexpected timeout from flash, so bail early (reporting the device fuller than it really is)
+            break;
+        }
 
         // Checking the buffer 4 bytes at a time like this is probably faster than byte-by-byte, but I didn't benchmark it :)
         blockErased = true;
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 42f71ce393..51f8e61d60 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -475,7 +475,7 @@ const clivalue_t valueTable[] = {
     { "baro_noise_lpf",             VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_noise_lpf, 0, 1 },
     { "baro_cf_vel",                VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_vel, 0, 1 },
     { "baro_cf_alt",                VAR_FLOAT  | PROFILE_VALUE, &masterConfig.profile[0].barometerConfig.baro_cf_alt, 0, 1 },
-	{ "baro_hardware",              VAR_UINT8  | MASTER_VALUE,  &masterConfig.baro_hardware, 0, BARO_MAX },
+    { "baro_hardware",              VAR_UINT8  | MASTER_VALUE,  &masterConfig.baro_hardware, 0, BARO_MAX },
 
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_MAX },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index ac49ce9487..02166e37e7 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -40,6 +40,7 @@
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
+#include "drivers/system.h"
 #include "rx/pwm.h"
 #include "rx/sbus.h"
 #include "rx/spektrum.h"
@@ -50,6 +51,7 @@
 
 #include "rx/rx.h"
 
+
 //#define DEBUG_RX_SIGNAL_LOSS
 
 void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback);
@@ -71,6 +73,7 @@ static bool rxFlightChannelsValid = false;
 
 static uint32_t rxUpdateAt = 0;
 static uint32_t needRxSignalBefore = 0;
+static uint32_t suspendRxSignalUntil = 0;
 static uint8_t  skipRxSamples = 0;
 
 int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
@@ -80,8 +83,8 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 
 #define DELAY_50_HZ (1000000 / 50)
 #define DELAY_10_HZ (1000000 / 10)
-#define SKIP_RC_SAMPLES_ON_SUSPEND 75               // approx. 1.5 seconds of samples at 50Hz
-#define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements
+#define SKIP_RC_ON_SUSPEND_PERIOD 1500000           // 1.5 second period in usec (call frequency independent)
+#define SKIP_RC_SAMPLES_ON_RESUME  2                // flush 2 samples to drop wrong measurements (timing independent)
 
 rxRuntimeConfig_t rxRuntimeConfig;
 static rxConfig_t *rxConfig;
@@ -276,12 +279,16 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime)
 
 void suspendRxSignal(void)
 {
-    skipRxSamples = SKIP_RC_SAMPLES_ON_SUSPEND;
+    suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
+    skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
+    failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
 }
 
 void resumeRxSignal(void)
 {
+    suspendRxSignalUntil = micros();
     skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
+    failsafeOnRxResume();
 }
 
 void updateRx(uint32_t currentTime)
@@ -491,8 +498,11 @@ void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
         }
     }
 
+    // only proceed when no more samples to skip and suspend period is over
     if (skipRxSamples) {
-        skipRxSamples--;
+        if (currentTime > suspendRxSignalUntil) {
+            skipRxSamples--;
+        }
         return;
     }
 
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index 8c7b065105..1368e7634c 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -28,6 +28,7 @@ typedef enum {
 #define BARO_MAX BARO_BMP280
 
 #define BARO_SAMPLE_COUNT_MAX   48
+#define BARO_MAX BARO_MS5611
 
 typedef struct barometerConfig_s {
     uint8_t baro_sample_count;              // size of baro filter array
diff --git a/src/main/sensors/boardalignment.c b/src/main/sensors/boardalignment.c
index a455936e6a..f927ba72ff 100644
--- a/src/main/sensors/boardalignment.c
+++ b/src/main/sensors/boardalignment.c
@@ -44,9 +44,9 @@ void initBoardAlignment(boardAlignment_t *boardAlignment)
     standardBoardAlignment = false;
 
     fp_angles_t rotationAngles;
-    rotationAngles.angles.roll = degreesToRadians(boardAlignment->rollDegrees);
+    rotationAngles.angles.roll  = degreesToRadians(boardAlignment->rollDegrees );
     rotationAngles.angles.pitch = degreesToRadians(boardAlignment->pitchDegrees);
-    rotationAngles.angles.yaw = degreesToRadians(boardAlignment->yawDegrees);
+    rotationAngles.angles.yaw   = degreesToRadians(boardAlignment->yawDegrees  );
 
     buildRotationMatrix(&rotationAngles, boardRotation);
 }
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index 19480d65e0..6b81f002cb 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -47,7 +47,7 @@
 #define L3GD20_CS_GPIO                  GPIOE
 #define L3GD20_CS_PIN                   GPIO_Pin_3
 
-#define GYRO_L3GD20_ALIGN CW90_DEG
+#define GYRO_L3GD20_ALIGN CW270_DEG
 #define GYRO_MPU6050_ALIGN CW0_DEG
 
 #define ACC
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index d3640e4640..5e275c3b3b 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -47,7 +47,7 @@
 #define L3GD20_CS_GPIO                  GPIOE
 #define L3GD20_CS_PIN                   GPIO_Pin_3
 
-#define GYRO_L3GD20_ALIGN CW90_DEG
+#define GYRO_L3GD20_ALIGN CW270_DEG
 
 #define ACC
 #define USE_ACC_LSM303DLHC
diff --git a/src/test/Makefile b/src/test/Makefile
index 582602d846..b48353663c 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -94,44 +94,447 @@ TEST_INCLUDE_DIRS := $(TEST_DIR) \
 
 TEST_CFLAGS	 = $(addprefix -I,$(TEST_INCLUDE_DIRS))
 
-LIBCLEANFLIGHT_SRC = \
-	common/encoding.c \
-	common/maths.c \
-	drivers/barometer_ms5611.c \
-	drivers/barometer_bmp085.c \
-	drivers/light_ws2811strip.c \
-	flight/altitudehold.c \
-	flight/failsafe.c \
-	flight/gps_conversion.c \
-	flight/imu.c \
-	flight/lowpass.c \
-	flight/mixer.c \
-	io/ledstrip.c \
-	io/rc_controls.c \
-	io/serial.c \
-	rx/rx.c \
-	sensors/battery.c \
-	telemetry/hott.c
+DEPS = $(TEST_BINARIES:%=%.d)
 
-LIBCLEANFLIGHT_OBJ = $(LIBCLEANFLIGHT_SRC:%.c=$(OBJECT_DIR)/%.o)
+$(OBJECT_DIR)/common/maths.o : \
+	$(USER_DIR)/common/maths.c \
+	$(USER_DIR)/common/maths.h \
+	$(GTEST_HEADERS)
 
-DEPS =	$(LIBCLEANFLIGHT_OBJ:%.o=%.d) \
-	$(TEST_BINARIES:%=%.d)
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
 
-LIBS = $(OBJECT_DIR)/libcleanflight.a $(OBJECT_DIR)/gtest_main.a
 
-$(OBJECT_DIR)/libcleanflight.a: $(LIBCLEANFLIGHT_OBJ)
-	$(AR) $(ARFLAGS) $@ $^
+$(OBJECT_DIR)/sensors/battery.o : $(USER_DIR)/sensors/battery.c $(USER_DIR)/sensors/battery.h $(GTEST_HEADERS)
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@
 
-# Build a module from the flight software.
-$(OBJECT_DIR)/%.o: $(USER_DIR)/%.c
-	mkdir -p $(@D)
-	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $< -o $@
+$(OBJECT_DIR)/battery_unittest.o : \
+	$(TEST_DIR)/battery_unittest.cc \
+	$(USER_DIR)/sensors/battery.h \
+	$(GTEST_HEADERS)
 
-# Build the unit test executable.
-$(OBJECT_DIR)/%: $(TEST_DIR)/%.cc $(LIBS)
-	@mkdir -p $(@D)
-	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -o $@ $< $(LIBS)
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $< -o $@
+
+$(OBJECT_DIR)/battery_unittest : \
+	$(OBJECT_DIR)/sensors/battery.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/battery_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $@
+
+$(OBJECT_DIR)/common/encoding.o : $(USER_DIR)/common/encoding.c $(USER_DIR)/common/encoding.h $(GTEST_HEADERS)
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/encoding.c -o $@
+
+$(OBJECT_DIR)/encoding_unittest.o : \
+	$(TEST_DIR)/encoding_unittest.cc \
+	$(USER_DIR)/common/encoding.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/encoding_unittest.cc -o $@
+
+$(OBJECT_DIR)/encoding_unittest : \
+	$(OBJECT_DIR)/common/encoding.o \
+	$(OBJECT_DIR)/encoding_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/flight/imu.o : \
+	$(USER_DIR)/flight/imu.c \
+	$(USER_DIR)/flight/imu.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/imu.c -o $@
+
+$(OBJECT_DIR)/flight_imu_unittest.o : \
+	$(TEST_DIR)/flight_imu_unittest.cc \
+	$(USER_DIR)/flight/imu.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_imu_unittest.cc -o $@
+
+$(OBJECT_DIR)/flight_imu_unittest : \
+	$(OBJECT_DIR)/flight/imu.o \
+	$(OBJECT_DIR)/flight/altitudehold.o \
+	$(OBJECT_DIR)/flight_imu_unittest.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/maths_unittest.o : \
+	$(TEST_DIR)/maths_unittest.cc \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/maths_unittest.cc -o $@
+
+$(OBJECT_DIR)/maths_unittest : \
+	$(OBJECT_DIR)/maths_unittest.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/flight/altitudehold.o : \
+	$(USER_DIR)/flight/altitudehold.c \
+	$(USER_DIR)/flight/altitudehold.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/altitudehold.c -o $@
+
+$(OBJECT_DIR)/altitude_hold_unittest.o : \
+	$(TEST_DIR)/altitude_hold_unittest.cc \
+	$(USER_DIR)/flight/altitudehold.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/altitude_hold_unittest.cc -o $@
+
+$(OBJECT_DIR)/altitude_hold_unittest : \
+	$(OBJECT_DIR)/flight/altitudehold.o \
+	$(OBJECT_DIR)/altitude_hold_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+
+$(OBJECT_DIR)/flight/gps_conversion.o : \
+	$(USER_DIR)/flight/gps_conversion.c \
+	$(USER_DIR)/flight/gps_conversion.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/gps_conversion.c -o $@
+
+$(OBJECT_DIR)/gps_conversion_unittest.o : \
+	$(TEST_DIR)/gps_conversion_unittest.cc \
+	$(USER_DIR)/flight/gps_conversion.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/gps_conversion_unittest.cc -o $@
+
+$(OBJECT_DIR)/gps_conversion_unittest : \
+	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/gps_conversion_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+
+
+$(OBJECT_DIR)/telemetry/hott.o : \
+	$(USER_DIR)/telemetry/hott.c \
+	$(USER_DIR)/telemetry/hott.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/hott.c -o $@
+
+$(OBJECT_DIR)/telemetry_hott_unittest.o : \
+	$(TEST_DIR)/telemetry_hott_unittest.cc \
+	$(USER_DIR)/telemetry/hott.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -o $@
+
+$(OBJECT_DIR)/telemetry_hott_unittest : \
+	$(OBJECT_DIR)/telemetry/hott.o \
+	$(OBJECT_DIR)/telemetry_hott_unittest.o \
+	$(OBJECT_DIR)/flight/gps_conversion.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+
+
+$(OBJECT_DIR)/io/rc_controls.o : \
+	$(USER_DIR)/io/rc_controls.c \
+	$(USER_DIR)/io/rc_controls.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/rc_controls.c -o $@
+
+$(OBJECT_DIR)/rc_controls_unittest.o : \
+	$(TEST_DIR)/rc_controls_unittest.cc \
+	$(USER_DIR)/io/rc_controls.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@
+
+$(OBJECT_DIR)/rc_controls_unittest : \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/io/rc_controls.o \
+	$(OBJECT_DIR)/rc_controls_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+
+$(OBJECT_DIR)/io/ledstrip.o : \
+	$(USER_DIR)/io/ledstrip.c \
+	$(USER_DIR)/io/ledstrip.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/ledstrip.c -o $@
+
+$(OBJECT_DIR)/ledstrip_unittest.o : \
+	$(TEST_DIR)/ledstrip_unittest.cc \
+	$(USER_DIR)/io/ledstrip.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ledstrip_unittest.cc -o $@
+
+$(OBJECT_DIR)/ledstrip_unittest : \
+	$(OBJECT_DIR)/io/ledstrip.o \
+	$(OBJECT_DIR)/ledstrip_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+
+
+$(OBJECT_DIR)/drivers/light_ws2811strip.o : \
+	$(USER_DIR)/drivers/light_ws2811strip.c \
+	$(USER_DIR)/drivers/light_ws2811strip.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/light_ws2811strip.c -o $@
+
+$(OBJECT_DIR)/ws2811_unittest.o : \
+	$(TEST_DIR)/ws2811_unittest.cc \
+	$(USER_DIR)/drivers/light_ws2811strip.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/ws2811_unittest.cc -o $@
+
+$(OBJECT_DIR)/ws2811_unittest : \
+	$(OBJECT_DIR)/drivers/light_ws2811strip.o \
+	$(OBJECT_DIR)/ws2811_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+
+$(OBJECT_DIR)/flight/lowpass.o : \
+	$(USER_DIR)/flight/lowpass.c \
+	$(USER_DIR)/flight/lowpass.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/lowpass.c -o $@
+
+$(OBJECT_DIR)/lowpass_unittest.o : \
+	$(TEST_DIR)/lowpass_unittest.cc \
+	$(USER_DIR)/flight/lowpass.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/lowpass_unittest.cc -o $@
+
+$(OBJECT_DIR)/lowpass_unittest : \
+	$(OBJECT_DIR)/flight/lowpass.o \
+	$(OBJECT_DIR)/lowpass_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/flight/mixer.o : \
+	$(USER_DIR)/flight/mixer.c \
+	$(USER_DIR)/flight/mixer.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/mixer.c -o $@
+
+$(OBJECT_DIR)/flight_mixer_unittest.o : \
+	$(TEST_DIR)/flight_mixer_unittest.cc \
+	$(USER_DIR)/flight/mixer.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_mixer_unittest.cc -o $@
+
+$(OBJECT_DIR)/flight_mixer_unittest : \
+	$(OBJECT_DIR)/flight/mixer.o \
+	$(OBJECT_DIR)/flight_mixer_unittest.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/flight/failsafe.o : \
+	$(USER_DIR)/flight/failsafe.c \
+	$(USER_DIR)/flight/failsafe.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/flight/failsafe.c -o $@
+
+$(OBJECT_DIR)/flight_failsafe_unittest.o : \
+	$(TEST_DIR)/flight_failsafe_unittest.cc \
+	$(USER_DIR)/flight/failsafe.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/flight_failsafe_unittest.cc -o $@
+
+$(OBJECT_DIR)/flight_failsafe_unittest : \
+	$(OBJECT_DIR)/flight/failsafe.o \
+	$(OBJECT_DIR)/flight_failsafe_unittest.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/io/serial.o : \
+	$(USER_DIR)/io/serial.c \
+	$(USER_DIR)/io/serial.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/io/serial.c -o $@
+
+$(OBJECT_DIR)/io_serial_unittest.o : \
+	$(TEST_DIR)/io_serial_unittest.cc \
+	$(USER_DIR)/io/serial.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/io_serial_unittest.cc -o $@
+
+$(OBJECT_DIR)/io_serial_unittest : \
+	$(OBJECT_DIR)/io/serial.o \
+	$(OBJECT_DIR)/io_serial_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/rx/rx.o : \
+	$(USER_DIR)/rx/rx.c \
+	$(USER_DIR)/rx/rx.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/rx/rx.c -o $@
+
+$(OBJECT_DIR)/rx_rx_unittest.o : \
+	$(TEST_DIR)/rx_rx_unittest.cc \
+	$(USER_DIR)/rx/rx.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rx_rx_unittest.cc -o $@
+
+$(OBJECT_DIR)/rx_rx_unittest : \
+	$(OBJECT_DIR)/rx/rx.o \
+	$(OBJECT_DIR)/rx_rx_unittest.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/rx_ranges_unittest.o : \
+	$(TEST_DIR)/rx_ranges_unittest.cc \
+	$(USER_DIR)/rx/rx.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rx_ranges_unittest.cc -o $@
+
+$(OBJECT_DIR)/rx_ranges_unittest : \
+	$(OBJECT_DIR)/rx/rx.o \
+	$(OBJECT_DIR)/rx_ranges_unittest.o \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/drivers/barometer_ms5611.o : \
+    $(USER_DIR)/drivers/barometer_ms5611.c \
+    $(USER_DIR)/drivers/barometer_ms5611.h \
+    $(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_ms5611.c -o $@
+
+$(OBJECT_DIR)/baro_ms5611_unittest.o : \
+	$(TEST_DIR)/baro_ms5611_unittest.cc \
+	$(USER_DIR)/drivers/barometer_ms5611.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_ms5611_unittest.cc -o $@
+
+$(OBJECT_DIR)/baro_ms5611_unittest : \
+	$(OBJECT_DIR)/drivers/barometer_ms5611.o \
+	$(OBJECT_DIR)/baro_ms5611_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/drivers/barometer_bmp085.o : \
+    $(USER_DIR)/drivers/barometer_bmp085.c \
+    $(USER_DIR)/drivers/barometer_bmp085.h \
+    $(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/barometer_bmp085.c -o $@
+
+$(OBJECT_DIR)/baro_bmp085_unittest.o : \
+	$(TEST_DIR)/baro_bmp085_unittest.cc \
+	$(USER_DIR)/drivers/barometer_bmp085.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/baro_bmp085_unittest.cc -o $@
+
+$(OBJECT_DIR)/baro_bmp085_unittest : \
+	$(OBJECT_DIR)/drivers/barometer_bmp085.o \
+	$(OBJECT_DIR)/baro_bmp085_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
+$(OBJECT_DIR)/sensors/boardalignment.o : \
+	$(USER_DIR)/sensors/boardalignment.c \
+	$(USER_DIR)/sensors/boardalignment.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/boardalignment.c -o $@
+
+$(OBJECT_DIR)/alignsensor_unittest.o : \
+	$(TEST_DIR)/alignsensor_unittest.cc \
+	$(USER_DIR)/sensors/boardalignment.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/alignsensor_unittest.cc -o $@
+
+$(OBJECT_DIR)/alignsensor_unittest : \
+	$(OBJECT_DIR)/common/maths.o \
+	$(OBJECT_DIR)/sensors/boardalignment.o \
+	$(OBJECT_DIR)/alignsensor_unittest.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
 
 test: $(TESTS:%=test-%)
 
diff --git a/src/test/unit/alignsensor_unittest.cc b/src/test/unit/alignsensor_unittest.cc
new file mode 100644
index 0000000000..674ed9572a
--- /dev/null
+++ b/src/test/unit/alignsensor_unittest.cc
@@ -0,0 +1,269 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "math.h"
+#include "stdint.h"
+#include "time.h"
+
+extern "C" {
+#include "common/axis.h"
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+}
+
+#include "gtest/gtest.h"
+
+/*
+ * This test file contains an independent method of rotating a vector.
+ * The output of alignSensor() is compared to the output of the test
+ * rotation method.
+ * 
+ * For each alignment condition (CW0, CW90, etc) the source vector under
+ * test is set to a unit vector along each axis (x-axis, y-axis, z-axis) 
+ * plus one additional random vector is tested.
+ */
+
+#define DEG2RAD 0.01745329251
+
+static void rotateVector(int16_t mat[3][3], int16_t vec[3], int16_t *out)
+{
+    int16_t tmp[3];
+
+    for(int i=0; i<3; i++) {
+        tmp[i] = 0;
+        for(int j=0; j<3; j++) {
+            tmp[i] += mat[j][i] * vec[j];
+        }
+    }
+    
+    out[0]=tmp[0];
+    out[1]=tmp[1];
+    out[2]=tmp[2];
+
+}
+
+//static void initXAxisRotation(int16_t mat[][3], int16_t angle)
+//{
+//    mat[0][0] =  1;
+//    mat[0][1] =  0;
+//    mat[0][2] =  0;
+//    mat[1][0] =  0;
+//    mat[1][1] =  cos(angle*DEG2RAD);
+//    mat[1][2] = -sin(angle*DEG2RAD);
+//    mat[2][0] =  0;
+//    mat[2][1] =  sin(angle*DEG2RAD);
+//    mat[2][2] =  cos(angle*DEG2RAD);
+//}
+
+static void initYAxisRotation(int16_t mat[][3], int16_t angle)
+{
+    mat[0][0] =  cos(angle*DEG2RAD);
+    mat[0][1] =  0;
+    mat[0][2] =  sin(angle*DEG2RAD);
+    mat[1][0] =  0;
+    mat[1][1] =  1;
+    mat[1][2] =  0;
+    mat[2][0] = -sin(angle*DEG2RAD);
+    mat[2][1] =  0;
+    mat[2][2] =  cos(angle*DEG2RAD);
+}
+
+static void initZAxisRotation(int16_t mat[][3], int16_t angle)
+{
+    mat[0][0] =  cos(angle*DEG2RAD);
+    mat[0][1] = -sin(angle*DEG2RAD);
+    mat[0][2] =  0;
+    mat[1][0] =  sin(angle*DEG2RAD);
+    mat[1][1] =  cos(angle*DEG2RAD);
+    mat[1][2] =  0;
+    mat[2][0] =  0;
+    mat[2][1] =  0;
+    mat[2][2] =  1;
+}
+
+static void testCW(sensor_align_e rotation, int16_t angle)
+{
+    int16_t src[XYZ_AXIS_COUNT];
+    int16_t dest[XYZ_AXIS_COUNT];
+    int16_t test[XYZ_AXIS_COUNT];
+
+    // unit vector along x-axis
+    src[X] = 1;
+    src[Y] = 0;
+    src[Z] = 0;
+    
+    int16_t matrix[3][3];
+    initZAxisRotation(matrix, angle);
+    rotateVector(matrix, src, test);
+
+    alignSensors(src, dest, rotation);
+    EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+
+    // unit vector along y-axis
+    src[X] = 0;
+    src[Y] = 1;
+    src[Z] = 0;
+
+    rotateVector(matrix, src, test);
+    alignSensors(src, dest, rotation);
+    EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+
+    // unit vector along z-axis
+    src[X] = 0;
+    src[Y] = 0;
+    src[Z] = 1;
+
+    rotateVector(matrix, src, test);
+    alignSensors(src, dest, rotation);
+    EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+
+    // random vector to test
+    src[X] = rand() % 5;
+    src[Y] = rand() % 5;
+    src[Z] = rand() % 5;
+
+    rotateVector(matrix, src, test);
+    alignSensors(src, dest, rotation);
+    EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+}
+
+/*
+ * Since the order of flip and rotation matters, these tests make the
+ * assumption that the 'flip' occurs first, followed by clockwise rotation
+ */
+static void testCWFlip(sensor_align_e rotation, int16_t angle)
+{
+    int16_t src[XYZ_AXIS_COUNT];
+    int16_t dest[XYZ_AXIS_COUNT];
+    int16_t test[XYZ_AXIS_COUNT];
+
+    // unit vector along x-axis
+    src[X] = 1;
+    src[Y] = 0;
+    src[Z] = 0;
+    
+    int16_t matrix[3][3];
+    initYAxisRotation(matrix, 180);
+    rotateVector(matrix, src, test);
+    initZAxisRotation(matrix, angle);
+    rotateVector(matrix, test, test);
+
+    alignSensors(src, dest, rotation);
+
+    EXPECT_EQ(test[X], dest[X]) << "X-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "X-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "X-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+
+    // unit vector along y-axis
+    src[X] = 0;
+    src[Y] = 1;
+    src[Z] = 0;
+
+    initYAxisRotation(matrix, 180);
+    rotateVector(matrix, src, test);
+    initZAxisRotation(matrix, angle);
+    rotateVector(matrix, test, test);
+
+    alignSensors(src, dest, rotation);
+
+    EXPECT_EQ(test[X], dest[X]) << "Y-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "Y-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "Y-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+
+    // unit vector along z-axis
+    src[X] = 0;
+    src[Y] = 0;
+    src[Z] = 1;
+
+    initYAxisRotation(matrix, 180);
+    rotateVector(matrix, src, test);
+    initZAxisRotation(matrix, angle);
+    rotateVector(matrix, test, test);
+
+    alignSensors(src, dest, rotation);
+
+    EXPECT_EQ(test[X], dest[X]) << "Z-Unit alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "Z-Unit alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "Z-Unit alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+
+     // random vector to test
+    src[X] = rand() % 5;
+    src[Y] = rand() % 5;
+    src[Z] = rand() % 5;
+
+    initYAxisRotation(matrix, 180);
+    rotateVector(matrix, src, test);
+    initZAxisRotation(matrix, angle);
+    rotateVector(matrix, test, test);
+
+    alignSensors(src, dest, rotation);
+
+    EXPECT_EQ(test[X], dest[X]) << "Random alignment does not match in X-Axis. " << test[X] << " " << dest[X];
+    EXPECT_EQ(test[Y], dest[Y]) << "Random alignment does not match in Y-Axis. " << test[Y] << " " << dest[Y];
+    EXPECT_EQ(test[Z], dest[Z]) << "Random alignment does not match in Z-Axis. " << test[Z] << " " << dest[Z];
+}
+ 
+
+TEST(AlignSensorTest, ClockwiseZeroDegrees)
+{
+    srand(time(NULL));
+    testCW(CW0_DEG, 0);
+}
+
+TEST(AlignSensorTest, ClockwiseNinetyDegrees)
+{
+    testCW(CW90_DEG, 90);
+}
+
+TEST(AlignSensorTest, ClockwiseOneEightyDegrees)
+{
+    testCW(CW180_DEG, 180);
+}
+
+TEST(AlignSensorTest, ClockwiseTwoSeventyDegrees)
+{
+    testCW(CW270_DEG, 270);
+}
+
+TEST(AlignSensorTest, ClockwiseZeroDegreesFlip)
+{
+    testCWFlip(CW0_DEG_FLIP, 0);
+}
+
+TEST(AlignSensorTest, ClockwiseNinetyDegreesFlip)
+{
+    testCWFlip(CW90_DEG_FLIP, 90);
+}
+
+TEST(AlignSensorTest, ClockwiseOneEightyDegreesFlip)
+{
+    testCWFlip(CW180_DEG_FLIP, 180);
+}
+
+TEST(AlignSensorTest, ClockwiseTwoSeventyDegreesFlip)
+{
+    testCWFlip(CW270_DEG_FLIP, 270);
+}
+
diff --git a/src/test/unit/baro_bmp085_unittest.cc b/src/test/unit/baro_bmp085_unittest.cc
index eb7d2c70b4..60889ae50e 100644
--- a/src/test/unit/baro_bmp085_unittest.cc
+++ b/src/test/unit/baro_bmp085_unittest.cc
@@ -182,8 +182,8 @@ TEST(baroBmp085Test, TestBmp085CalculateOss3Hot)
 
 extern "C" {
 
-    void gpioInit(){}
-    void RCC_APB2PeriphClockCmd(){}
+    void gpioInit() {}
+    void RCC_APB2PeriphClockCmd() {}
     void delay(uint32_t) {}
     void delayMicroseconds(uint32_t) {}
     bool i2cWrite(uint8_t, uint8_t, uint8_t) {
diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index 9283fa8452..e49b8dcf75 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -20,6 +20,7 @@
 #define MAG
 #define BARO
 #define GPS
+#define DISPLAY
 #define TELEMETRY
 #define LED_STRIP
 #define USE_SERVOS
diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index 073fed3244..a5bad1c9d3 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -693,9 +693,9 @@ void accSetCalibrationCycles(uint16_t) {}
 void gyroSetCalibrationCycles(uint16_t) {}
 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
 void handleInflightCalibrationStickPosition(void) {}
+bool feature(uint32_t) { return false;}
+bool sensors(uint32_t) { return false;}
 void mwArm(void) {}
-void feature(uint32_t) {}
-void sensors(uint32_t) {}
 void mwDisarm(void) {}
 void displayDisablePageCycling() {}
 void displayEnablePageCycling() {}
diff --git a/src/test/unit/rx_ranges_unittest.cc b/src/test/unit/rx_ranges_unittest.cc
index d559d52693..427cabd8f9 100755
--- a/src/test/unit/rx_ranges_unittest.cc
+++ b/src/test/unit/rx_ranges_unittest.cc
@@ -97,6 +97,12 @@ TEST(RxChannelRangeTest, TestRxChannelRanges)
 // stubs
 extern "C" {
 
+void failsafeOnRxSuspend(uint32_t ) {}
+void failsafeOnRxResume(void) {}
+
+uint32_t micros(void) { return 0; }
+uint32_t millis(void) { return 0; }
+
 void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
 {
     UNUSED(rxRuntimeConfig);
@@ -143,20 +149,8 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR
     return true;
 }
 
-void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
-{
-    UNUSED(modeActivationConditions);
-}
-
-void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig)
-{
-    UNUSED(index);
-    UNUSED(auxChannelIndex);
-    UNUSED(adjustmentConfig);
-}
-
-void feature(uint32_t)
-{
+bool feature(uint32_t) {
+    return false;
 }
 
 bool rxMspFrameComplete(void)
@@ -164,10 +158,6 @@ bool rxMspFrameComplete(void)
     return false;
 }
 
-void failsafeReset(void)
-{
-}
-
 bool isPPMDataBeingReceived(void)
 {
     return false;
@@ -182,10 +172,6 @@ void resetPPMDataReceivedState(void)
 {
 }
 
-void failsafeOnRxCycle(void)
-{
-}
-
 void failsafeOnValidDataReceived(void)
 {
 }
@@ -194,11 +180,5 @@ void failsafeOnValidDataFailed(void)
 {
 }
 
-void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
-{
-    UNUSED(channel);
-    UNUSED(pulseDuration);
-}
-
 }
 
diff --git a/src/test/unit/rx_rx_unittest.cc b/src/test/unit/rx_rx_unittest.cc
index db3e0f2f11..c654594ea8 100644
--- a/src/test/unit/rx_rx_unittest.cc
+++ b/src/test/unit/rx_rx_unittest.cc
@@ -139,6 +139,12 @@ extern "C" {
     void failsafeOnValidDataFailed() {}
     void failsafeOnValidDataReceived() {}
 
+    void failsafeOnRxSuspend(uint32_t ) {}
+    void failsafeOnRxResume(void) {}
+
+    uint32_t micros(void) { return 0; }
+    uint32_t millis(void) { return 0; }
+
     bool feature(uint32_t mask) {
         UNUSED(mask);
         return false;
@@ -159,6 +165,4 @@ extern "C" {
     void rxMspInit(rxConfig_t *, rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
 
     void rxPwmInit(rxRuntimeConfig_t *, rcReadRawDataPtr *) {}
-
-
 }