diff --git a/Makefile b/Makefile
index ac610e7e2..a44a3549b 100644
--- a/Makefile
+++ b/Makefile
@@ -203,7 +203,7 @@ COMMON_SRC	 = build_config.c \
 		   mw.c \
 		   flight/altitudehold.c \
 		   flight/failsafe.c \
-		   flight/flight.c \
+		   flight/pid.c \
 		   flight/imu.c \
 		   flight/mixer.c \
 		   drivers/bus_i2c_soft.c \
@@ -397,8 +397,6 @@ CJMCU_SRC	 = \
 		   drivers/system_stm32f10x.c \
 		   drivers/timer.c \
 		   drivers/timer_stm32f10x.c \
-		   blackbox/blackbox.c \
-		   blackbox/blackbox_io.c \
 		   hardware_revision.c \
 		   $(COMMON_SRC)
 
@@ -502,6 +500,7 @@ SPRACINGF3_SRC	 = \
 		   drivers/accgyro_mpu6050.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/compass_hmc5883l.c \
+		   drivers/display_ug2864hsweg01.h \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 
diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md
index 0f725e533..e672fee61 100644
--- a/docs/Board - CC3D.md	
+++ b/docs/Board - CC3D.md	
@@ -66,16 +66,24 @@ The 6 pin RC_Output connector has the following pinouts when used in RX_PARALLEL
 
 # Serial Ports
 
-| Value | Identifier   | Board Markings | Notes                                    |
-| ----- | ------------ | -------------- | -----------------------------------------|
-| 1     | VCP          | USB PORT       |                                          |
-| 2     | USART1       | MAIN PORT      | Has a hardware inverter for S.BUS        |
-| 3     | USART3       | FLEX PORT      |                                          |
-| 4     | SoftSerial   | RC connector   | Pins 4 and 5 (Tx and Rx respectively)    |
+| Value | Identifier   | Board Markings | Notes                                     |
+| ----- | ------------ | -------------- | ------------------------------------------|
+| 1     | VCP          | USB PORT       |                                           |
+| 2     | USART1       | MAIN PORT      | Connected to an MCU controllable inverter |
+| 3     | USART3       | FLEX PORT      |                                           |
+| 4     | SoftSerial   | RC connector   | Pins 4 and 5 (Tx and Rx respectively)     |
 
 The SoftSerial port is not available when RX_PARALLEL_PWM is used. The transmission data rate is limited to 19200 baud.
 
-To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP).
+To connect the GUI to the flight controller you just need a USB cable to use the Virtual Com Port (VCP) or you can use UART1 (Main Port).
+
+CLI access is only available via the VCP by default.
+
+# Main Port
+
+The main port has MSP support enabled on it by default.
+
+The main port is connected to an inverter which is automatically enabled as required.  For example, if the main port is used for SBus Serial RX then an external inverter is not required.
 
 # Flex Port
 
diff --git a/docs/Board - Sparky.md b/docs/Board - Sparky.md
index 9501b5912..8bef9801f 100644
--- a/docs/Board - Sparky.md	
+++ b/docs/Board - Sparky.md	
@@ -17,8 +17,6 @@ Flyable!
 Tested with revision 1 board. 
 
 ## TODO
-* Baro - detection works but sending bad readings, disabled for now.
-* LED Strip
 * ADC
 * Sonar
 * Display (via Flex port)
diff --git a/docs/LedStrip.md b/docs/LedStrip.md
index dffc9d99f..07f08a94f 100644
--- a/docs/LedStrip.md
+++ b/docs/LedStrip.md
@@ -178,7 +178,7 @@ Note: Armed State cannot be used with Flight Mode.
 
 This mode fades the LED current LED color to the previous/next color in the HSB color space depending on throttle stick position.  When the
 throttle is in the middle position the color is unaffected, thus it can be mixed with orientation colors to indicate orientation and throttle at
-the same time.
+the same time.  Thrust should normally be combined with Color or Mode/Orientation.
 
 #### Thrust ring state
 
diff --git a/docs/PID tuning.md b/docs/PID tuning.md
index 4ae7fb154..b206d751c 100644
--- a/docs/PID tuning.md	
+++ b/docs/PID tuning.md	
@@ -63,7 +63,7 @@ affecting yaw, roll or pitch rotation rates (which are tuned by the dedicated ro
 settings).
 
 In Angle mode, this controller uses the LEVEL "P" PID setting to decide how strong the auto-level correction should
-be.  
+be. Note that the default value for P_Level is 90.  This is more than likely too high of a value for most, and will cause the model to be very unstable in Angle Mode, and could result in loss of control.  It is recommended to change this value to 20 before using PID Controller 2 in Angle Mode.
 
 In Horizon mode, this controller uses the LEVEL "I" PID setting to decide how much auto-level correction should be
 applied. The default Cleanflight setting for "I" will result in virtually no auto-leveling being applied, so that will
@@ -85,7 +85,7 @@ nebbian in v1.6.0. The autotune feature does not work on this controller, so don
 It is the first PID Controller designed for 32-bit processors and not derived from MultiWii.
 
 The strength of the auto-leveling correction applied during Angle mode is set by the parameter "level_angle" which
-is labeled "LEVEL Integral" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to
+is labeled "LEVEL Proportional" in the GUI. This can be used to tune the auto-leveling strength in Angle mode compared to
 Horizon mode. The default is 5.0.
 
 The strength of the auto-leveling correction applied during Horizon mode is set by the parameter "level_horizon" which
diff --git a/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c b/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
index 1b0209447..299e03745 100755
--- a/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
+++ b/lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c
@@ -145,7 +145,7 @@ void SetSysClock(bool overclock)
     // On CJMCU new revision boards (Late 2014) bit 15 of GPIOC->IDR is '1'.
     RCC_CFGR_PLLMUL = RCC_CFGR_PLLMULL9;
 #else
-    RCC_CFGR_PLLMUL = GPIOC->IDR & CAN_MCR_RESET ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9;
+    RCC_CFGR_PLLMUL = GPIOC->IDR & GPIO_IDR_IDR15 ? hse_value = 12000000, RCC_CFGR_PLLMULL6 : RCC_CFGR_PLLMULL9;
 #endif
     switch (clocksrc) {
         case SRC_HSE:
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 0b5e705f4..a0e357930 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -36,7 +36,6 @@
 #include "drivers/light_led.h"
 #include "drivers/sound_beeper.h"
 
-#include "flight/flight.h"
 #include "sensors/sensors.h"
 #include "sensors/boardalignment.h"
 #include "sensors/sonar.h"
@@ -45,17 +44,11 @@
 #include "sensors/barometer.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
+
 #include "io/beeper.h"
 #include "io/display.h"
 #include "io/escservo.h"
-#include "rx/rx.h"
 #include "io/rc_controls.h"
-#include "flight/mixer.h"
-#include "flight/altitudehold.h"
-#include "flight/failsafe.h"
-#include "flight/imu.h"
-#include "flight/autotune.h"
-#include "flight/navigation.h"
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
@@ -63,9 +56,18 @@
 #include "io/serial_cli.h"
 #include "io/serial_msp.h"
 #include "io/statusindicator.h"
+
+#include "rx/rx.h"
 #include "rx/msp.h"
+
 #include "telemetry/telemetry.h"
 
+#include "flight/mixer.h"
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/navigation.h"
+
 #include "config/runtime_config.h"
 #include "config/config.h"
 #include "config/config_profile.h"
diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c
index a8971ff72..f41c44982 100644
--- a/src/main/blackbox/blackbox_io.c
+++ b/src/main/blackbox/blackbox_io.c
@@ -22,24 +22,19 @@
 #include "drivers/light_led.h"
 #include "drivers/sound_beeper.h"
 
-#include "flight/flight.h"
 #include "sensors/sensors.h"
 #include "sensors/boardalignment.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
+
 #include "io/beeper.h"
 #include "io/display.h"
 #include "io/escservo.h"
 #include "rx/rx.h"
 #include "io/rc_controls.h"
-#include "flight/mixer.h"
-#include "flight/altitudehold.h"
-#include "flight/failsafe.h"
-#include "flight/imu.h"
-#include "flight/autotune.h"
-#include "flight/navigation.h"
+
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
@@ -51,6 +46,12 @@
 #include "telemetry/telemetry.h"
 #include "common/printf.h"
 
+#include "flight/mixer.h"
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/imu.h"
+#include "flight/navigation.h"
+
 #include "config/runtime_config.h"
 #include "config/config.h"
 #include "config/config_profile.h"
diff --git a/src/main/common/axis.h b/src/main/common/axis.h
index fffd76432..c7f1a96a8 100644
--- a/src/main/common/axis.h
+++ b/src/main/common/axis.h
@@ -25,3 +25,18 @@ typedef enum {
 
 #define XYZ_AXIS_COUNT 3
 
+// See http://en.wikipedia.org/wiki/Flight_dynamics
+typedef enum {
+    FD_ROLL = 0,
+    FD_PITCH,
+    FD_YAW
+} flight_dynamics_index_t;
+
+#define FLIGHT_DYNAMICS_INDEX_COUNT 3
+
+typedef enum {
+    AI_ROLL = 0,
+    AI_PITCH,
+} angle_index_t;
+
+#define ANGLE_INDEX_COUNT 2
diff --git a/src/main/common/printf.c b/src/main/common/printf.c
index 46e5b3902..50d986bc1 100644
--- a/src/main/common/printf.c
+++ b/src/main/common/printf.c
@@ -185,7 +185,7 @@ static void _putc(void *p, char c)
     serialWrite(printfSerialPort, c);
 }
 
-void initPrintfSupport(void)
+void printfSupportInit(void)
 {
     init_printf(NULL, _putc);
 }
@@ -201,7 +201,7 @@ int fputc(int c, FILE *f)
     return c;
 }
 
-void initPrintfSupport(serialPort_t *serialPort)
+void printfSupportInit(void)
 {
     // Nothing to do
 }
diff --git a/src/main/config/config.c b/src/main/config/config.c
index d7e93ce17..11c807896 100644
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -27,45 +27,46 @@
 #include "common/axis.h"
 #include "common/maths.h"
 
-#include "flight/flight.h"
-
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/compass.h"
-
 #include "drivers/system.h"
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
+#include "drivers/serial.h"
 
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
 #include "sensors/compass.h"
-
-#include "io/statusindicator.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
-#include "drivers/serial.h"
-#include "io/serial.h"
-#include "telemetry/telemetry.h"
-
-#include "flight/mixer.h"
 #include "sensors/boardalignment.h"
 #include "sensors/battery.h"
+
+#include "io/statusindicator.h"
+#include "io/serial.h"
 #include "io/gimbal.h"
 #include "io/escservo.h"
-#include "rx/rx.h"
 #include "io/rc_controls.h"
 #include "io/rc_curves.h"
 #include "io/ledstrip.h"
 #include "io/gps.h"
+
+#include "rx/rx.h"
+
+#include "telemetry/telemetry.h"
+
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
 #include "flight/failsafe.h"
 #include "flight/altitudehold.h"
-#include "flight/imu.h"
 #include "flight/navigation.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
+
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
@@ -110,7 +111,7 @@ profile_t *currentProfile;
 static uint8_t currentControlRateProfileIndex = 0;
 controlRateConfig_t *currentControlRateProfile;
 
-static const uint8_t EEPROM_CONF_VERSION = 90;
+static const uint8_t EEPROM_CONF_VERSION = 91;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
@@ -121,6 +122,8 @@ static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 
 static void resetPidProfile(pidProfile_t *pidProfile)
 {
+    pidProfile->pidController = 0;
+
     pidProfile->P8[ROLL] = 40;
     pidProfile->I8[ROLL] = 30;
     pidProfile->D8[ROLL] = 23;
@@ -231,15 +234,26 @@ void resetBatteryConfig(batteryConfig_t *batteryConfig)
     batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
 }
 
+#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
+#define FIRST_PORT_INDEX 1
+#define SECOND_PORT_INDEX 0
+#else
+#define FIRST_PORT_INDEX 0
+#define SECOND_PORT_INDEX 1
+#endif
+
 void resetSerialConfig(serialConfig_t *serialConfig)
 {
-#ifdef SWAP_SERIAL_PORT_1_AND_2_DEFAULTS
-    serialConfig->serial_port_scenario[0] = lookupScenarioIndex(SCENARIO_UNUSED);
-    serialConfig->serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH);
+    serialConfig->serial_port_scenario[FIRST_PORT_INDEX] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH);
+
+#ifdef CC3D
+    // Temporary workaround for CC3D non-functional VCP when using OpenPilot bootloader.
+    // This allows MSP connection via USART so the board can be reconfigured.
+    serialConfig->serial_port_scenario[SECOND_PORT_INDEX] = lookupScenarioIndex(SCENARIO_MSP_ONLY);
 #else
-    serialConfig->serial_port_scenario[0] = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH);
-    serialConfig->serial_port_scenario[1] = lookupScenarioIndex(SCENARIO_UNUSED);
+    serialConfig->serial_port_scenario[SECOND_PORT_INDEX] = lookupScenarioIndex(SCENARIO_UNUSED);
 #endif
+
 #if (SERIAL_PORT_COUNT > 2)
     serialConfig->serial_port_scenario[2] = lookupScenarioIndex(SCENARIO_UNUSED);
 #if (SERIAL_PORT_COUNT > 3)
@@ -386,7 +400,6 @@ static void resetConf(void)
     masterConfig.looptime = 3500;
     masterConfig.emf_avoidance = 0;
 
-    currentProfile->pidController = 0;
     resetPidProfile(&currentProfile->pidProfile);
 
     resetControlRateConfig(&masterConfig.controlRateProfiles[0]);
@@ -472,7 +485,7 @@ static void resetConf(void)
     masterConfig.escAndServoConfig.maxthrottle = 2000;
     masterConfig.motor_pwm_rate = 32000;
     masterConfig.looptime = 2000;
-    currentProfile->pidController = 3;
+    currentProfile->pidProfile.pidController = 3;
     currentProfile->pidProfile.P8[ROLL] = 36;
     currentProfile->pidProfile.P8[PITCH] = 36;
     currentProfile->failsafeConfig.failsafe_delay = 2;
@@ -604,7 +617,7 @@ void activateConfig(void)
     useTelemetryConfig(&masterConfig.telemetryConfig);
 #endif
 
-    setPIDController(currentProfile->pidController);
+    setPIDController(currentProfile->pidProfile.pidController);
 
 #ifdef GPS
     gpsUseProfile(&currentProfile->gpsProfile);
@@ -630,7 +643,7 @@ void activateConfig(void)
     imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
     imuRuntimeConfig.small_angle = masterConfig.small_angle;
 
-    configureIMU(
+    imuConfigure(
         &imuRuntimeConfig,
         &currentProfile->pidProfile,
         &currentProfile->accDeadband,
@@ -676,7 +689,9 @@ void validateAndFixConfig(void)
         // rssi adc needs the same ports
         featureClear(FEATURE_RSSI_ADC);
         // current meter needs the same ports
-        featureClear(FEATURE_CURRENT_METER);
+        if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
+            featureClear(FEATURE_CURRENT_METER);
+        }
 #endif
 
 #if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
@@ -709,13 +724,13 @@ void validateAndFixConfig(void)
 #endif
 
 #if defined(NAZE) && defined(SONAR)
-    if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) {
+    if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
         featureClear(FEATURE_CURRENT_METER);
     }
 #endif
 
 #if defined(OLIMEXINO) && defined(SONAR)
-    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) {
+    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
         featureClear(FEATURE_CURRENT_METER);
     }
 #endif
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index 990654833..79a093cf4 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -85,9 +85,11 @@ typedef struct master_t {
     uint8_t current_profile_index;
     controlRateConfig_t controlRateProfiles[MAX_CONTROL_RATE_PROFILE_COUNT];
 
-    uint8_t blackbox_device;
+#ifdef BLACKBOX
     uint8_t blackbox_rate_num;
     uint8_t blackbox_rate_denom;
+    uint8_t blackbox_device;
+#endif
 
     uint8_t magic_ef;                       // magic number, should be 0xEF
     uint8_t chk;                            // XOR checksum
diff --git a/src/main/config/config_profile.h b/src/main/config/config_profile.h
index 1aafc14bd..7968b97c0 100644
--- a/src/main/config/config_profile.h
+++ b/src/main/config/config_profile.h
@@ -18,8 +18,6 @@
 #pragma once
 
 typedef struct profile_s {
-    uint8_t pidController;                  // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
-
     pidProfile_t pidProfile;
 
     uint8_t defaultRateProfileIndex;
diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c
index 4215df21d..958a831b4 100644
--- a/src/main/drivers/adc.c
+++ b/src/main/drivers/adc.c
@@ -18,11 +18,16 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "build_config.h"
+
 #include "platform.h"
 #include "system.h"
 
 #include "adc.h"
 
+//#define DEBUG_ADC_CHANNELS
+
+#ifdef USE_ADC
 adc_config_t adcConfig[ADC_CHANNEL_COUNT];
 volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
 
@@ -30,7 +35,7 @@ extern int16_t debug[4];
 
 uint16_t adcGetChannel(uint8_t channel)
 {
-#if DEBUG_ADC_CHANNELS
+#ifdef DEBUG_ADC_CHANNELS
     if (adcConfig[0].enabled) {
         debug[0] = adcValues[adcConfig[0].dmaIndex];
     }
@@ -47,3 +52,10 @@ uint16_t adcGetChannel(uint8_t channel)
     return adcValues[adcConfig[channel].dmaIndex];
 }
 
+#else
+uint16_t adcGetChannel(uint8_t channel)
+{
+    UNUSED(channel);
+    return 0;
+}
+#endif
diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c
index 5f591ed0a..c6c97c2f4 100644
--- a/src/main/drivers/adc_stm32f10x.c
+++ b/src/main/drivers/adc_stm32f10x.c
@@ -64,7 +64,7 @@ void adcInit(drv_adc_config_t *init)
     GPIO_StructInit(&GPIO_InitStructure);
     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AIN;
 
-#ifdef VBAT_ADC_GPIO_PIN
+#ifdef VBAT_ADC_GPIO
     GPIO_InitStructure.GPIO_Pin = VBAT_ADC_GPIO_PIN;
     GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure);
     adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL;
diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c
index 6c129d546..c18b37417 100644
--- a/src/main/drivers/adc_stm32f30x.c
+++ b/src/main/drivers/adc_stm32f30x.c
@@ -32,6 +32,12 @@
 extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
 extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
 
+#ifndef ADC_INSTANCE
+#define ADC_INSTANCE                ADC1
+#define ADC_AHB_PERIPHERAL          RCC_AHBPeriph_DMA1
+#define ADC_DMA_CHANNEL             DMA1_Channel1
+#endif
+
 void adcInit(drv_adc_config_t *init)
 {
     ADC_InitTypeDef ADC_InitStructure;
@@ -44,50 +50,64 @@ void adcInit(drv_adc_config_t *init)
     memset(&adcConfig, 0, sizeof(adcConfig));
 
     GPIO_StructInit(&GPIO_InitStructure);
-    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_0 | GPIO_Pin_3;
     GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AN;
     GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL ;
 
-    adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_6;
+#ifdef VBAT_ADC_GPIO
+    GPIO_InitStructure.GPIO_Pin   = VBAT_ADC_GPIO_PIN;
+    GPIO_Init(VBAT_ADC_GPIO, &GPIO_InitStructure);
+
+    adcConfig[ADC_BATTERY].adcChannel = VBAT_ADC_CHANNEL;
     adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount;
     adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_601Cycles5;
     adcConfig[ADC_BATTERY].enabled = true;
     adcChannelCount++;
+#endif
 
+#ifdef CURRENT_METER_ADC_GPIO
     if (init->enableCurrentMeter) {
-        GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_1;
+        GPIO_InitStructure.GPIO_Pin = CURRENT_METER_ADC_GPIO_PIN;
+        GPIO_Init(CURRENT_METER_ADC_GPIO, &GPIO_InitStructure);
 
-        adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_7;
+        adcConfig[ADC_CURRENT].adcChannel = CURRENT_METER_ADC_CHANNEL;
         adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount;
         adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_601Cycles5;
         adcConfig[ADC_CURRENT].enabled = true;
         adcChannelCount++;
-
     }
+#endif
 
+#ifdef RSSI_ADC_GPIO
     if (init->enableRSSI) {
-        GPIO_InitStructure.GPIO_Pin |= GPIO_Pin_2;
+        GPIO_InitStructure.GPIO_Pin = RSSI_ADC_GPIO_PIN;
+        GPIO_Init(RSSI_ADC_GPIO, &GPIO_InitStructure);
 
-        adcConfig[ADC_RSSI].adcChannel = ADC_Channel_8;
+        adcConfig[ADC_RSSI].adcChannel = RSSI_ADC_CHANNEL;
         adcConfig[ADC_RSSI].dmaIndex = adcChannelCount;
         adcConfig[ADC_RSSI].sampleTime = ADC_SampleTime_601Cycles5;
         adcConfig[ADC_RSSI].enabled = true;
         adcChannelCount++;
     }
+#endif
 
-    adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_9;
+#ifdef EXTERNAL1_ADC_GPIO
+    GPIO_InitStructure.GPIO_Pin   = EXTERNAL1_ADC_GPIO_PIN;
+    GPIO_Init(EXTERNAL1_ADC_GPIO, &GPIO_InitStructure);
+
+    adcConfig[ADC_EXTERNAL1].adcChannel = EXTERNAL1_ADC_CHANNEL;
     adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount;
     adcConfig[ADC_EXTERNAL1].sampleTime = ADC_SampleTime_601Cycles5;
     adcConfig[ADC_EXTERNAL1].enabled = true;
     adcChannelCount++;
+#endif
 
     RCC_ADCCLKConfig(RCC_ADC12PLLCLK_Div256);  // 72 MHz divided by 256 = 281.25 kHz
-    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | RCC_AHBPeriph_ADC12, ENABLE);
+    RCC_AHBPeriphClockCmd(ADC_AHB_PERIPHERAL | RCC_AHBPeriph_ADC12, ENABLE);
 
-    DMA_DeInit(DMA1_Channel1);
+    DMA_DeInit(ADC_DMA_CHANNEL);
 
     DMA_StructInit(&DMA_InitStructure);
-    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
+    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC_INSTANCE->DR;
     DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues;
     DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
     DMA_InitStructure.DMA_BufferSize = adcChannelCount;
@@ -99,20 +119,19 @@ void adcInit(drv_adc_config_t *init)
     DMA_InitStructure.DMA_Priority = DMA_Priority_High;
     DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
 
-    DMA_Init(DMA1_Channel1, &DMA_InitStructure);
+    DMA_Init(ADC_DMA_CHANNEL, &DMA_InitStructure);
 
-    DMA_Cmd(DMA1_Channel1, ENABLE);
+    DMA_Cmd(ADC_DMA_CHANNEL, ENABLE);
 
-    GPIO_Init(GPIOC, &GPIO_InitStructure);
 
     // calibrate
 
-    ADC_VoltageRegulatorCmd(ADC1, ENABLE);
+    ADC_VoltageRegulatorCmd(ADC_INSTANCE, ENABLE);
     delay(10);
-    ADC_SelectCalibrationMode(ADC1, ADC_CalibrationMode_Single);
-    ADC_StartCalibration(ADC1);
-    while(ADC_GetCalibrationStatus(ADC1) != RESET);
-    ADC_VoltageRegulatorCmd(ADC1, DISABLE);
+    ADC_SelectCalibrationMode(ADC_INSTANCE, ADC_CalibrationMode_Single);
+    ADC_StartCalibration(ADC_INSTANCE);
+    while(ADC_GetCalibrationStatus(ADC_INSTANCE) != RESET);
+    ADC_VoltageRegulatorCmd(ADC_INSTANCE, DISABLE);
 
 
     ADC_CommonInitTypeDef ADC_CommonInitStructure;
@@ -123,7 +142,7 @@ void adcInit(drv_adc_config_t *init)
     ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1;
     ADC_CommonInitStructure.ADC_DMAMode = ADC_DMAMode_Circular;
     ADC_CommonInitStructure.ADC_TwoSamplingDelay = 0;
-    ADC_CommonInit(ADC1, &ADC_CommonInitStructure);
+    ADC_CommonInit(ADC_INSTANCE, &ADC_CommonInitStructure);
 
     ADC_StructInit(&ADC_InitStructure);
 
@@ -136,24 +155,24 @@ void adcInit(drv_adc_config_t *init)
     ADC_InitStructure.ADC_AutoInjMode           = ADC_AutoInjec_Disable;
     ADC_InitStructure.ADC_NbrOfRegChannel       = adcChannelCount;
 
-    ADC_Init(ADC1, &ADC_InitStructure);
+    ADC_Init(ADC_INSTANCE, &ADC_InitStructure);
 
     uint8_t rank = 1;
     for (i = 0; i < ADC_CHANNEL_COUNT; i++) {
         if (!adcConfig[i].enabled) {
             continue;
         }
-        ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime);
+        ADC_RegularChannelConfig(ADC_INSTANCE, adcConfig[i].adcChannel, rank++, adcConfig[i].sampleTime);
     }
 
-    ADC_Cmd(ADC1, ENABLE);
+    ADC_Cmd(ADC_INSTANCE, ENABLE);
 
-    while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_RDY));
+    while(!ADC_GetFlagStatus(ADC_INSTANCE, ADC_FLAG_RDY));
 
-    ADC_DMAConfig(ADC1, ADC_DMAMode_Circular);
+    ADC_DMAConfig(ADC_INSTANCE, ADC_DMAMode_Circular);
 
-    ADC_DMACmd(ADC1, ENABLE);
+    ADC_DMACmd(ADC_INSTANCE, ENABLE);
 
-    ADC_StartConversion(ADC1);
+    ADC_StartConversion(ADC_INSTANCE);
 }
 
diff --git a/src/main/drivers/inverter.c b/src/main/drivers/inverter.c
index b8f014c8f..000597087 100644
--- a/src/main/drivers/inverter.c
+++ b/src/main/drivers/inverter.c
@@ -33,7 +33,8 @@ void initInverter(void)
         gpio_config_t cfg;
     } gpio_setup = {
         .gpio = INVERTER_GPIO,
-        .cfg = { INVERTER_PIN, Mode_Out_OD, Speed_2MHz }
+        // configure for Push-Pull
+        .cfg = { INVERTER_PIN, Mode_Out_PP, Speed_2MHz } 
     };
 
     RCC_APB2PeriphClockCmd(INVERTER_PERIPHERAL, ENABLE);
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index 7a6e83b1f..0057b7ba9 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -40,6 +40,10 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode);
 uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
 
 static void usartConfigurePinInversion(uartPort_t *uartPort) {
+#if !defined(INVERTER) && !defined(STM32F303xC)
+    UNUSED(uartPort);
+#endif
+
 #ifdef INVERTER
     if (uartPort->port.inversion == SERIAL_INVERTED && uartPort->USARTx == INVERTER_USART) {
         // Enable hardware inverter if available.
diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index bfd7be91d..66195165a 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -30,20 +30,20 @@
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 
-#include "flight/flight.h"
-
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/sonar.h"
 
-#include "flight/mixer.h"
-#include "flight/imu.h"
-
 #include "rx/rx.h"
+
 #include "io/rc_controls.h"
 #include "io/escservo.h"
 
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+
 #include "config/runtime_config.h"
 
 extern int16_t debug[4];
@@ -280,7 +280,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
 
     // Integrator - velocity, cm/sec
-    accZ_tmp = (float)accSum[2] / (float)accSumCount;
+    if (accSumCount) {
+        accZ_tmp = (float)accSum[2] / (float)accSumCount;
+    } else {
+        accZ_tmp = 0;
+    }
     vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
 
     // Integrator - Altitude in cm
@@ -288,13 +292,13 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt);    // complementary filter for altitude estimation (baro & acc)
     vel += vel_acc;
 
-#if 0
+#if 1
     debug[1] = accSum[2] / accSumCount; // acceleration
     debug[2] = vel;                     // velocity
     debug[3] = accAlt;                  // height
 #endif
 
-    accSum_reset();
+    imuResetAccelerationSum();
 
 #ifdef BARO
     if (!isBaroCalibrationComplete()) {
diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h
index 1e42c65ef..e8ccfec92 100644
--- a/src/main/flight/altitudehold.h
+++ b/src/main/flight/altitudehold.h
@@ -15,13 +15,15 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "flight/flight.h"
-
 #include "io/escservo.h"
 #include "io/rc_controls.h"
+#include "flight/pid.h"
 
 #include "sensors/barometer.h"
 
+extern int32_t AltHold;
+extern int32_t vario;
+
 void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
 void applyAltHold(airplaneConfig_t *airplaneConfig);
 void updateAltHoldState(void);
diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c
index 2770b6824..bfc0f53b2 100644
--- a/src/main/flight/autotune.c
+++ b/src/main/flight/autotune.c
@@ -28,8 +28,14 @@
 #include "common/maths.h"
 
 #include "drivers/system.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
 
-#include "flight/flight.h"
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+
+#include "flight/pid.h"
+#include "flight/imu.h"
 
 #include "config/config.h"
 #include "blackbox/blackbox.h"
@@ -226,8 +232,8 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin
         return errorAngle;
     }
 
-    if (pidController == 2) {
-        // TODO support new baseflight pid controller
+    if (IS_PID_CONTROLLER_FP_BASED(pidController)) {
+        // TODO support floating point based pid controllers
         return errorAngle;
     }
 
@@ -415,7 +421,7 @@ void restorePids(pidProfile_t *pidProfileToTune)
     memcpy(pidProfileToTune, &pidBackup, sizeof(pidBackup));
 }
 
-void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse)
+void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune)
 {
     phase = nextPhase;
 
@@ -439,7 +445,7 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle
     firstPeakAngle = secondPeakAngle = 0;
 
     pidProfile = pidProfileToTune;
-    pidController = pidControllerInUse;
+    pidController = pidProfile->pidController;
 
     updatePidIndex();
     updateTargetAngle();
diff --git a/src/main/flight/autotune.h b/src/main/flight/autotune.h
index 095309aaf..8d4296e3c 100644
--- a/src/main/flight/autotune.h
+++ b/src/main/flight/autotune.h
@@ -18,7 +18,7 @@
 #pragma once
 
 void autotuneReset();
-void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
+void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune);
 float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
 void autotuneEndPhase();
 
diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h
deleted file mode 100644
index 51fd74a0a..000000000
--- a/src/main/flight/flight.h
+++ /dev/null
@@ -1,121 +0,0 @@
-/*
- * 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/>.
- */
-
-#pragma once
-
-
-typedef enum {
-    PIDROLL,
-    PIDPITCH,
-    PIDYAW,
-    PIDALT,
-    PIDPOS,
-    PIDPOSR,
-    PIDNAVR,
-    PIDLEVEL,
-    PIDMAG,
-    PIDVEL,
-    PID_ITEM_COUNT
-} pidIndex_e;
-
-typedef struct pidProfile_s {
-    uint8_t P8[PID_ITEM_COUNT];
-    uint8_t I8[PID_ITEM_COUNT];
-    uint8_t D8[PID_ITEM_COUNT];
-
-    float P_f[3];                           // float p i and d factors for the new baseflight pid
-    float I_f[3];
-    float D_f[3];
-    float A_level;
-    float H_level;
-    uint8_t H_sensitivity;
-} pidProfile_t;
-
-typedef enum {
-    AI_ROLL = 0,
-    AI_PITCH,
-} angle_index_t;
-
-#define ANGLE_INDEX_COUNT 2
-
-// See http://en.wikipedia.org/wiki/Flight_dynamics
-typedef enum {
-    FD_ROLL = 0,
-    FD_PITCH,
-    FD_YAW
-} flight_dynamics_index_t;
-
-#define FLIGHT_DYNAMICS_INDEX_COUNT 3
-
-typedef struct int16_flightDynamicsTrims_s {
-    int16_t roll;
-    int16_t pitch;
-    int16_t yaw;
-} flightDynamicsTrims_def_t;
-
-typedef union {
-    int16_t raw[3];
-    flightDynamicsTrims_def_t values;
-} flightDynamicsTrims_t;
-
-typedef struct rollAndPitchTrims_s {
-    int16_t roll;
-    int16_t pitch;
-} rollAndPitchTrims_t_def;
-
-typedef union {
-    int16_t raw[2];
-    rollAndPitchTrims_t_def values;
-} rollAndPitchTrims_t;
-
-typedef struct rollAndPitchInclination_s {
-    // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
-    int16_t rollDeciDegrees;
-    int16_t pitchDeciDegrees;
-} rollAndPitchInclination_t_def;
-
-typedef union {
-    int16_t raw[ANGLE_INDEX_COUNT];
-    rollAndPitchInclination_t_def values;
-} rollAndPitchInclination_t;
-
-
-#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
-#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
-
-extern rollAndPitchInclination_t inclination;
-
-extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
-extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
-
-extern int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
-extern int32_t accSum[XYZ_AXIS_COUNT];
-extern int16_t axisPID[XYZ_AXIS_COUNT];
-
-extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
-
-extern int16_t heading, magHold;
-
-extern int32_t AltHold;
-extern int32_t vario;
-
-void setPIDController(int type);
-void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
-void resetErrorAngle(void);
-void resetErrorGyro(void);
-
-
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index afea1727e..23a3805ea 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -26,11 +26,8 @@
 #include <platform.h>
 
 #include "common/axis.h"
-#include "flight/flight.h"
 
 #include "drivers/system.h"
-
-
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 #include "drivers/compass.h"
@@ -42,15 +39,18 @@
 #include "sensors/barometer.h"
 #include "sensors/sonar.h"
 
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+
 #include "config/runtime_config.h"
 
-#include "flight/mixer.h"
-#include "flight/imu.h"
 
 extern int16_t debug[4];
 
-int16_t gyroADC[XYZ_AXIS_COUNT], accADC[XYZ_AXIS_COUNT], accSmooth[XYZ_AXIS_COUNT];
+int16_t accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
+int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 uint32_t accTimeSum = 0;        // keep track for integration of acc
 int accSumCount = 0;
@@ -64,11 +64,6 @@ float fc_acc;
 float magneticDeclination = 0.0f;       // calculated at startup from config
 float gyroScaleRad;
 
-// **************
-// gyro+acc IMU
-// **************
-int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
-int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 rollAndPitchInclination_t inclination = { { 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 float anglerad[2] = { 0.0f, 0.0f };    // absolute angle inclination in radians
@@ -77,12 +72,11 @@ imuRuntimeConfig_t *imuRuntimeConfig;
 pidProfile_t *pidProfile;
 accDeadband_t *accDeadband;
 
-void configureIMU(
+void imuConfigure(
     imuRuntimeConfig_t *initialImuRuntimeConfig,
     pidProfile_t *initialPidProfile,
     accDeadband_t *initialAccDeadband,
     float accz_lpf_cutoff,
-    //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
     uint16_t throttle_correction_angle
 )
 {
@@ -90,18 +84,16 @@ void configureIMU(
     pidProfile = initialPidProfile;
     accDeadband = initialAccDeadband;
     fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
-    //TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
     throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
 }
 
-void initIMU()
+void imuInit()
 {
     smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
     accVelScale = 9.80665f / acc_1G / 10000.0f;
     gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
 }
 
-//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
 float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 {
     return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
@@ -132,7 +124,7 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
 
 t_fp_vector EstG;
 
-void accSum_reset(void)
+void imuResetAccelerationSum(void)
 {
     accSum[0] = 0;
     accSum[1] = 0;
@@ -142,7 +134,7 @@ void accSum_reset(void)
 }
 
 // rotate acc into Earth frame and calculate acceleration in it
-void acc_calc(uint32_t deltaT)
+void imuCalculateAcceleration(uint32_t deltaT)
 {
     static int32_t accZoffset = 0;
     static float accz_smooth = 0;
@@ -215,7 +207,7 @@ void acc_calc(uint32_t deltaT)
 *
 * //TODO: Add explanation for how it uses the Z dimension.
 */
-int16_t calculateHeading(t_fp_vector *vec)
+int16_t imuCalculateHeading(t_fp_vector *vec)
 {
     int16_t head;
 
@@ -237,7 +229,7 @@ int16_t calculateHeading(t_fp_vector *vec)
     return head;
 }
 
-static void getEstimatedAttitude(void)
+static void imuCalculateEstimatedAttitude(void)
 {
     int32_t axis;
     int32_t accMag = 0;
@@ -298,24 +290,24 @@ static void getEstimatedAttitude(void)
         for (axis = 0; axis < 3; axis++) {
             EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
         }
-        heading = calculateHeading(&EstM);
+        heading = imuCalculateHeading(&EstM);
     } else {
         rotateV(&EstN.V, &deltaGyroAngle);
         normalizeV(&EstN.V, &EstN.V);
-        heading = calculateHeading(&EstN);
+        heading = imuCalculateHeading(&EstN);
     }
 
-    acc_calc(deltaT); // rotate acc vector into earth frame
+    imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
 }
 
-void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
 {
     static int16_t gyroYawSmooth = 0;
 
-    gyroGetADC();
+    gyroUpdate();
     if (sensors(SENSOR_ACC)) {
-        updateAccelerationReadings(accelerometerTrims);
-        getEstimatedAttitude();
+        updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
+        imuCalculateEstimatedAttitude();
     } else {
         accADC[X] = 0;
         accADC[Y] = 0;
@@ -333,7 +325,6 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
     }
 }
 
-//TODO: Move throttle angle correction code into flight/throttle_angle_correction.c
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
 {
     float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h
index 45949c4e1..9d1c83110 100644
--- a/src/main/flight/imu.h
+++ b/src/main/flight/imu.h
@@ -22,6 +22,29 @@ extern uint32_t accTimeSum;
 extern int accSumCount;
 extern float accVelScale;
 
+extern int16_t accSmooth[XYZ_AXIS_COUNT];
+extern int32_t accSum[XYZ_AXIS_COUNT];
+extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT];
+
+
+typedef struct rollAndPitchInclination_s {
+    // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
+    int16_t rollDeciDegrees;
+    int16_t pitchDeciDegrees;
+} rollAndPitchInclination_t_def;
+
+typedef union {
+    int16_t raw[ANGLE_INDEX_COUNT];
+    rollAndPitchInclination_t_def values;
+} rollAndPitchInclination_t;
+
+extern rollAndPitchInclination_t inclination;
+
+typedef struct accDeadband_s {
+    uint8_t xy;                 // set the acc deadband for xy-Axis
+    uint8_t z;                  // set the acc deadband for z-Axis, this ignores small accelerations
+} accDeadband_t;
+
 typedef struct imuRuntimeConfig_s {
     uint8_t acc_lpf_factor;
     uint8_t acc_unarmedcal;
@@ -30,20 +53,22 @@ typedef struct imuRuntimeConfig_s {
     int8_t small_angle;
 } imuRuntimeConfig_t;
 
-void configureIMU(
-	imuRuntimeConfig_t *initialImuRuntimeConfig,
-	pidProfile_t *initialPidProfile,
-	accDeadband_t *initialAccDeadband,
-	float accz_lpf_cutoff,
-	uint16_t throttle_correction_angle
+void imuConfigure(
+    imuRuntimeConfig_t *initialImuRuntimeConfig,
+    pidProfile_t *initialPidProfile,
+    accDeadband_t *initialAccDeadband,
+    float accz_lpf_cutoff,
+    uint16_t throttle_correction_angle
 );
 
 void calculateEstimatedAltitude(uint32_t currentTime);
-void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
+void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
 float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
 float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
 
-int16_t calculateHeading(t_fp_vector *vec);
+int16_t imuCalculateHeading(t_fp_vector *vec);
+
+void imuResetAccelerationSum(void);
+
 
-void accSum_reset(void);
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index f32237474..9a66157d9 100644
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -30,14 +30,21 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_output.h"
 #include "drivers/pwm_mapping.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
 
 #include "rx/rx.h"
+
 #include "io/gimbal.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
 
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+
 #include "flight/mixer.h"
-#include "flight/flight.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c
index 9c1e919cc..6153f121d 100644
--- a/src/main/flight/navigation.c
+++ b/src/main/flight/navigation.c
@@ -25,31 +25,31 @@
 #include "platform.h"
 
 #include "common/maths.h"
+#include "common/axis.h"
 
 #include "drivers/system.h"
-
 #include "drivers/serial.h"
 #include "drivers/serial_uart.h"
-#include "io/serial.h"
-
 #include "drivers/gpio.h"
 #include "drivers/light_led.h"
 
-#include "common/axis.h"
-#include "flight/flight.h"
-
 #include "sensors/sensors.h"
 
+#include "io/serial.h"
+#include "io/gps.h"
+#include "io/rc_controls.h"
+
+#include "flight/pid.h"
+#include "flight/navigation.h"
+#include "flight/gps_conversion.h"
+
+#include "rx/rx.h"
+
+
 #include "config/config.h"
 #include "config/runtime_config.h"
 
-#include "flight/gps_conversion.h"
-#include "io/gps.h"
-
-#include "rx/rx.h"
-#include "io/rc_controls.h"
-
-#include "flight/navigation.h"
+extern int16_t magHold;
 
 #ifdef GPS
 
@@ -57,7 +57,6 @@ extern int16_t debug[4];
 
 bool areSticksInApModePosition(uint16_t ap_mode);
 
-
 // **********************
 // GPS
 // **********************
diff --git a/src/main/flight/flight.c b/src/main/flight/pid.c
similarity index 99%
rename from src/main/flight/flight.c
rename to src/main/flight/pid.c
index 26e18f618..fccab9ab5 100644
--- a/src/main/flight/flight.c
+++ b/src/main/flight/pid.c
@@ -26,25 +26,29 @@
 #include "common/axis.h"
 #include "common/maths.h"
 
-#include "config/runtime_config.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+
+#include "sensors/sensors.h"
+#include "sensors/gyro.h"
+#include "sensors/acceleration.h"
 
 #include "rx/rx.h"
 
-#include "drivers/sensor.h"
-#include "drivers/accgyro.h"
-#include "sensors/sensors.h"
-#include "sensors/gyro.h"
-
 #include "io/rc_controls.h"
-#include "flight/flight.h"
+#include "io/gps.h"
+
+#include "flight/pid.h"
+#include "flight/imu.h"
 #include "flight/navigation.h"
 #include "flight/autotune.h"
-#include "io/gps.h"
+
+#include "config/runtime_config.h"
 
 extern uint16_t cycleTime;
 extern uint8_t motorCount;
 
-int16_t heading, magHold;
+int16_t heading;
 int16_t axisPID[3];
 
 #ifdef BLACKBOX
@@ -65,12 +69,6 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
 
 pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
 
-void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
-{
-    rollAndPitchTrims->values.roll = 0;
-    rollAndPitchTrims->values.pitch = 0;
-}
-
 void resetErrorAngle(void)
 {
     errorAngleI[ROLL] = 0;
@@ -201,8 +199,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
 
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm - DTerm), -1000, 1000);
-    }
 
+#ifdef BLACKBOX
+        axisPID_P[axis] = PTerm;
+        axisPID_I[axis] = ITerm;
+        axisPID_D[axis] = -DTerm;
+#endif
+    }
 }
 
 static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
new file mode 100644
index 000000000..b70273416
--- /dev/null
+++ b/src/main/flight/pid.h
@@ -0,0 +1,62 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+
+typedef enum {
+    PIDROLL,
+    PIDPITCH,
+    PIDYAW,
+    PIDALT,
+    PIDPOS,
+    PIDPOSR,
+    PIDNAVR,
+    PIDLEVEL,
+    PIDMAG,
+    PIDVEL,
+    PID_ITEM_COUNT
+} pidIndex_e;
+
+#define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
+
+typedef struct pidProfile_s {
+    uint8_t pidController;                  // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671, 1, 2 = Luggi09s new baseflight pid
+
+    uint8_t P8[PID_ITEM_COUNT];
+    uint8_t I8[PID_ITEM_COUNT];
+    uint8_t D8[PID_ITEM_COUNT];
+
+    float P_f[3];                           // float p i and d factors for lux float pid controller
+    float I_f[3];
+    float D_f[3];
+    float A_level;
+    float H_level;
+    uint8_t H_sensitivity;
+} pidProfile_t;
+
+#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
+#define DECIDEGREES_TO_DEGREES(angle) (angle / 10.0f)
+
+extern int16_t axisPID[XYZ_AXIS_COUNT];
+extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
+
+void setPIDController(int type);
+void resetErrorAngle(void);
+void resetErrorGyro(void);
+
+
diff --git a/src/main/io/display.c b/src/main/io/display.c
index ef103fd44..849560f8d 100644
--- a/src/main/io/display.c
+++ b/src/main/io/display.c
@@ -26,32 +26,36 @@
 #include "build_config.h"
 
 #include "drivers/serial.h"
+#include "drivers/system.h"
+#include "drivers/display_ug2864hsweg01.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+#include "drivers/compass.h"
+
 #include "common/printf.h"
 #include "common/maths.h"
+#include "common/axis.h"
 
 #ifdef DISPLAY
 
-#include "drivers/system.h"
-#include "drivers/display_ug2864hsweg01.h"
-
-#include "drivers/sensor.h"
-#include "drivers/compass.h"
-
 #include "sensors/battery.h"
-
-#include "common/axis.h"
-#include "flight/flight.h"
 #include "sensors/sensors.h"
 #include "sensors/compass.h"
+#include "sensors/acceleration.h"
+#include "sensors/gyro.h"
+
+#include "rx/rx.h"
+
+#include "io/rc_controls.h"
+
+#include "flight/pid.h"
+#include "flight/imu.h"
 
 #ifdef GPS
 #include "io/gps.h"
 #include "flight/navigation.h"
 #endif
 
-#include "rx/rx.h"
-#include "io/rc_controls.h"
-
 #include "config/runtime_config.h"
 
 #include "config/config.h"
@@ -236,12 +240,14 @@ void showRxPage(void)
 
 void showWelcomePage(void)
 {
-    tfp_sprintf(lineBuffer, "Rev: %s", shortGitRevision);
-    i2c_OLED_set_line(PAGE_TITLE_LINE_COUNT + 0);
+    uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
+
+    tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
+    i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 
     tfp_sprintf(lineBuffer, "Target: %s", targetName);
-    i2c_OLED_set_line(PAGE_TITLE_LINE_COUNT + 1);
+    i2c_OLED_set_line(rowIndex++);
     i2c_OLED_send_string(lineBuffer);
 }
 
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index db0f726d3..fd923b2e9 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -27,30 +27,26 @@
 #include "build_config.h"
 
 #include "common/maths.h"
+#include "common/axis.h"
 
 #include "drivers/system.h"
-
 #include "drivers/serial.h"
 #include "drivers/serial_uart.h"
-#include "io/serial.h"
-
 #include "drivers/gpio.h"
 #include "drivers/light_led.h"
 
-#include "common/axis.h"
-#include "flight/flight.h"
-
 #include "sensors/sensors.h"
 
-#include "config/config.h"
-#include "config/runtime_config.h"
-
-#include "flight/gps_conversion.h"
-#include "flight/navigation.h"
-
+#include "io/serial.h"
 #include "io/display.h"
 #include "io/gps.h"
 
+#include "flight/gps_conversion.h"
+#include "flight/pid.h"
+#include "flight/navigation.h"
+
+#include "config/config.h"
+#include "config/runtime_config.h"
 
 
 #ifdef GPS
diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c
index 34adfbf50..5d40671a5 100644
--- a/src/main/io/ledstrip.c
+++ b/src/main/io/ledstrip.c
@@ -28,24 +28,26 @@
 #ifdef LED_STRIP
 
 #include <common/color.h>
+#include <common/maths.h>
+#include <common/typeconversion.h>
 
 #include "drivers/light_ws2811strip.h"
 #include "drivers/system.h"
 #include "drivers/serial.h"
 
-#include <common/maths.h>
 #include <common/printf.h>
-#include <common/typeconversion.h>
 
 #include "sensors/battery.h"
 
-#include "config/runtime_config.h"
-#include "config/config.h"
-#include "rx/rx.h"
 #include "io/rc_controls.h"
+#include "io/ledstrip.h"
+
+#include "rx/rx.h"
+
 #include "flight/failsafe.h"
 
-#include "io/ledstrip.h"
+#include "config/runtime_config.h"
+#include "config/config.h"
 
 static bool ledStripInitialised = false;
 static bool ledStripEnabled = true;
@@ -769,7 +771,7 @@ void applyLedThrottleLayer()
 
         int scaled = scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, -60, +60);
         scaled += HSV_HUE_MAX;
-        color.h = scaled % HSV_HUE_MAX;
+        color.h = (color.h + scaled) % HSV_HUE_MAX;
         setLedHsv(ledIndex, &color);
     }
 }
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 64a70ba40..54ecb963b 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -21,6 +21,8 @@
 
 #include <math.h>
 
+#include "platform.h"
+
 #include "common/axis.h"
 #include "common/maths.h"
 
@@ -28,26 +30,29 @@
 #include "config/runtime_config.h"
 
 #include "drivers/system.h"
-
-#include "flight/flight.h"
-
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 
+#include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"
 
+#include "rx/rx.h"
+
 #include "io/gps.h"
 #include "io/beeper.h"
-#include "mw.h"
-
-#include "rx/rx.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
 #include "io/rc_curves.h"
 
+#include "flight/pid.h"
+#include "flight/navigation.h"
+
+#include "mw.h"
+
+
 static escAndServoConfig_t *escAndServoConfig;
 static pidProfile_t *pidProfile;
 
@@ -360,6 +365,7 @@ void configureAdjustment(uint8_t index, uint8_t auxSwitchChannelIndex, const adj
 
 void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) {
     int newValue;
+    float newFloatValue;
 
     if (delta > 0) {
         queueConfirmationBeep(2);
@@ -391,34 +397,70 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
             controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
             break;
         case ADJUSTMENT_PITCH_ROLL_P:
-            newValue = (int)pidProfile->P8[PIDPITCH] + delta;
-            pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            newValue = (int)pidProfile->P8[PIDROLL] + delta;
-            pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
+                newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
+                pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
+                pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->P8[PIDPITCH] + delta;
+                pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newValue = (int)pidProfile->P8[PIDROLL] + delta;
+                pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_PITCH_ROLL_I:
-            newValue = (int)pidProfile->I8[PIDPITCH] + delta;
-            pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            newValue = (int)pidProfile->I8[PIDROLL] + delta;
-            pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
+                newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 100.0f);
+                pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 100.0f);
+                pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->I8[PIDPITCH] + delta;
+                pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newValue = (int)pidProfile->I8[PIDROLL] + delta;
+                pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_PITCH_ROLL_D:
-            newValue = (int)pidProfile->D8[PIDPITCH] + delta;
-            pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
-            newValue = (int)pidProfile->D8[PIDROLL] + delta;
-            pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
+                newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 1000.0f);
+                pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+                newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 1000.0f);
+                pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->D8[PIDPITCH] + delta;
+                pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+                newValue = (int)pidProfile->D8[PIDROLL] + delta;
+                pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_YAW_P:
-            newValue = (int)pidProfile->P8[PIDYAW] + delta;
-            pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
+                newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
+                pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->P8[PIDYAW] + delta;
+                pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_YAW_I:
-            newValue = (int)pidProfile->I8[PIDYAW] + delta;
-            pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
+                newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 100.0f);
+                pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->I8[PIDYAW] + delta;
+                pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         case ADJUSTMENT_YAW_D:
-            newValue = (int)pidProfile->D8[PIDYAW] + delta;
-            pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
+                newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 1000.0f);
+                pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
+            } else {
+                newValue = (int)pidProfile->D8[PIDYAW] + delta;
+                pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c
+            }
             break;
         default:
             break;
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index a1c2a10a5..dde0536f0 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -207,7 +207,10 @@ typedef struct adjustmentState_s {
     uint32_t timeoutAt;
 } adjustmentState_t;
 
+
+#ifndef MAX_SIMULTANEOUS_ADJUSTMENT_COUNT
 #define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 4 // enough for 4 x 3position switches / 4 aux channel
+#endif
 
 #define MAX_ADJUSTMENT_RANGE_COUNT 12 // enough for 2 * 6pos switches.
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 00a01c6c5..22d52baa3 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -44,11 +44,8 @@
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
-#include "flight/flight.h"
-#include "flight/mixer.h"
-#include "flight/navigation.h"
-#include "flight/failsafe.h"
-#include "rx/rx.h"
+
+
 #include "io/escservo.h"
 #include "io/gps.h"
 #include "io/gimbal.h"
@@ -56,7 +53,10 @@
 #include "io/serial.h"
 #include "io/ledstrip.h"
 #include "io/flashfs.h"
+
+#include "rx/rx.h"
 #include "rx/spektrum.h"
+
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
@@ -64,6 +64,13 @@
 #include "sensors/gyro.h"
 #include "sensors/compass.h"
 #include "sensors/barometer.h"
+
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/navigation.h"
+#include "flight/failsafe.h"
+
 #include "telemetry/telemetry.h"
 
 #include "config/runtime_config.h"
@@ -171,7 +178,7 @@ const clicmd_t cmdTable[] = {
     { "color", "configure colors", cliColor },
 #endif
     { "defaults", "reset to defaults and reboot", cliDefaults },
-    { "dump", "print configurable settings in a pastable form", cliDump },
+    { "dump", "dump configuration", cliDump },
     { "exit", "", cliExit },
     { "feature", "list or -val or val", cliFeature },
 #ifdef USE_FLASHFS
@@ -263,7 +270,7 @@ const clivalue_t valueTable[] = {
     { "serial_port_2_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[1], 0, SERIAL_PORT_SCENARIO_MAX },
 #if (SERIAL_PORT_COUNT > 2)
     { "serial_port_3_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[2], 0, SERIAL_PORT_SCENARIO_MAX },
-#if !defined(CC3D)
+#if (SERIAL_PORT_COUNT > 3)
     { "serial_port_4_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[3], 0, SERIAL_PORT_SCENARIO_MAX },
 #if (SERIAL_PORT_COUNT > 4)
     { "serial_port_5_scenario",     VAR_UINT8  | MASTER_VALUE,  &masterConfig.serialConfig.serial_port_scenario[4], 0, SERIAL_PORT_SCENARIO_MAX },
@@ -384,7 +391,7 @@ const clivalue_t valueTable[] = {
     { "mag_hardware",               VAR_UINT8  | MASTER_VALUE,  &masterConfig.mag_hardware, 0, MAG_NONE },
     { "mag_declination",            VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].mag_declination, -18000, 18000 },
 
-    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidController, 0, 5 },
+    { "pid_controller",             VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidController, 0, 5 },
 
     { "p_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], 0, 200 },
     { "i_pitch",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], 0, 200 },
@@ -422,9 +429,11 @@ const clivalue_t valueTable[] = {
     { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], 0, 200 },
     { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], 0, 200 },
 
-    { "blackbox_device",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_device, 0, 1 },
+#ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, 1, 32 },
     { "blackbox_rate_denom",        VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_denom, 1, 32 },
+    { "blackbox_device",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_device, 0, 1 },
+#endif
 };
 
 #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t))
@@ -1476,7 +1485,13 @@ static void cliVersion(char *cmdline)
 {
     UNUSED(cmdline);
 
-    printf("Cleanflight/%s %s / %s (%s)", targetName, buildDate, buildTime, shortGitRevision);
+    printf("Cleanflight/%s %s %s / %s (%s)",
+        targetName,
+        FC_VERSION_STRING,
+        buildDate,
+        buildTime,
+        shortGitRevision
+    );
 }
 
 void cliProcess(void)
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 94df8ffa7..96ede62e8 100644
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -41,14 +41,9 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 
-#include "flight/flight.h"
-#include "flight/mixer.h"
-#include "flight/failsafe.h"
-#include "flight/navigation.h"
-#include "flight/altitudehold.h"
-
 #include "rx/rx.h"
 #include "rx/msp.h"
+
 #include "io/escservo.h"
 #include "io/rc_controls.h"
 #include "io/gps.h"
@@ -56,6 +51,7 @@
 #include "io/serial.h"
 #include "io/ledstrip.h"
 #include "io/flashfs.h"
+
 #include "telemetry/telemetry.h"
 
 #include "sensors/boardalignment.h"
@@ -67,6 +63,15 @@
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
 
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/failsafe.h"
+#include "flight/navigation.h"
+#include "flight/altitudehold.h"
+
+#include "mw.h"
+
 #include "config/runtime_config.h"
 #include "config/config.h"
 #include "config/config_profile.h"
@@ -910,7 +915,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_PID:
         headSerialReply(3 * PID_ITEM_COUNT);
-        if (currentProfile->pidController == 2) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
+        if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { // convert float stuff into uint8_t to keep backwards compatability with all 8-bit shit with new pid
             for (i = 0; i < 3; i++) {
                 serialize8(constrain(lrintf(currentProfile->pidProfile.P_f[i] * 10.0f), 0, 250));
                 serialize8(constrain(lrintf(currentProfile->pidProfile.I_f[i] * 100.0f), 0, 250));
@@ -941,7 +946,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_PID_CONTROLLER:
         headSerialReply(1);
-        serialize8(currentProfile->pidController);
+        serialize8(currentProfile->pidProfile.pidController);
         break;
     case MSP_MODE_RANGES:
         headSerialReply(4 * MAX_MODE_ACTIVATION_CONDITION_COUNT);
@@ -1265,11 +1270,11 @@ static bool processInCommand(void)
         currentProfile->accelerometerTrims.values.roll  = read16();
         break;
     case MSP_SET_PID_CONTROLLER:
-        currentProfile->pidController = read8();
-        setPIDController(currentProfile->pidController);
+        currentProfile->pidProfile.pidController = read8();
+        setPIDController(currentProfile->pidProfile.pidController);
         break;
     case MSP_SET_PID:
-        if (currentProfile->pidController == 2) {
+        if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) {
             for (i = 0; i < 3; i++) {
                 currentProfile->pidProfile.P_f[i] = (float)read8() / 10.0f;
                 currentProfile->pidProfile.I_f[i] = (float)read8() / 100.0f;
diff --git a/src/main/main.c b/src/main/main.c
index 814c4de72..9768f61f3 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -25,6 +25,8 @@
 #include "common/axis.h"
 #include "common/color.h"
 #include "common/atomic.h"
+#include "common/maths.h"
+
 #include "drivers/nvic.h"
 
 #include "drivers/sensor.h"
@@ -46,31 +48,36 @@
 #include "drivers/inverter.h"
 #include "drivers/flash_m25p16.h"
 
-#include "flight/flight.h"
-#include "flight/mixer.h"
+#include "rx/rx.h"
 
 #include "io/serial.h"
 #include "io/flashfs.h"
-#include "flight/failsafe.h"
-#include "flight/navigation.h"
 
-#include "rx/rx.h"
 #include "io/gps.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
 #include "io/gimbal.h"
 #include "io/ledstrip.h"
 #include "io/display.h"
+
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
 #include "sensors/acceleration.h"
 #include "sensors/gyro.h"
-#include "telemetry/telemetry.h"
-#include "blackbox/blackbox.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
+
+#include "telemetry/telemetry.h"
+#include "blackbox/blackbox.h"
+
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/failsafe.h"
+#include "flight/navigation.h"
+
 #include "config/runtime_config.h"
 #include "config/config.h"
 #include "config/config_profile.h"
@@ -93,9 +100,9 @@ serialPort_t *loopbackPort;
 
 failsafe_t *failsafe;
 
-void initPrintfSupport(void);
+void printfSupportInit(void);
 void timerInit(void);
-void initTelemetry(void);
+void telemetryInit(void);
 void serialInit(serialConfig_t *initialSerialConfig);
 failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
 pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
@@ -106,7 +113,7 @@ void beepcodeInit(failsafe_t *initialFailsafe);
 void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
 void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
 bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig);
-void initIMU(void);
+void imuInit(void);
 void displayInit(rxConfig_t *intialRxConfig);
 void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
 void loop(void);
@@ -127,7 +134,7 @@ void init(void)
     drv_pwm_config_t pwm_params;
     bool sensorsOK = false;
 
-    initPrintfSupport();
+    printfSupportInit();
 
     initEEPROM();
 
@@ -176,11 +183,16 @@ void init(void)
 
 #ifdef BEEPER
     beeperConfig_t beeperConfig = {
-        .gpioMode = Mode_Out_OD,
         .gpioPin = BEEP_PIN,
         .gpioPort = BEEP_GPIO,
         .gpioPeripheral = BEEP_PERIPHERAL,
+#ifdef BEEPER_INVERTED
+        .gpioMode = Mode_Out_PP,
+        .isInverted = true
+#else
+        .gpioMode = Mode_Out_OD,
         .isInverted = false
+#endif
     };
 #ifdef NAZE
     if (hardwareRevision >= NAZE32_REV5) {
@@ -269,7 +281,8 @@ void init(void)
     LED0_OFF;
     LED1_OFF;
 
-    initIMU();
+
+    imuInit();
     mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
 
 #ifdef MAG
@@ -285,7 +298,7 @@ void init(void)
         pwm_params.airplane = true;
     else
         pwm_params.airplane = false;
-#if defined(SERIAL_PORT_USART2) && defined(STM32F10X)
+#if defined(USE_USART2) && defined(STM32F10X)
     pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2);
 #endif
     pwm_params.useVbat = feature(FEATURE_VBAT);
@@ -348,7 +361,7 @@ void init(void)
 
 #ifdef TELEMETRY
     if (feature(FEATURE_TELEMETRY))
-        initTelemetry();
+        telemetryInit();
 #endif
 
 #ifdef USE_FLASHFS
@@ -391,8 +404,7 @@ void init(void)
 
     // Now that everything has powered up the voltage and cell count be determined.
 
-    // Check battery type/voltage
-    if (feature(FEATURE_VBAT))
+    if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER))
         batteryInit(&masterConfig.batteryConfig);
 
 #ifdef DISPLAY
diff --git a/src/main/mw.c b/src/main/mw.c
index 0d542311c..4291bd4a4 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -37,7 +37,6 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_rx.h"
 
-#include "flight/flight.h"
 #include "sensors/sensors.h"
 #include "sensors/boardalignment.h"
 #include "sensors/sonar.h"
@@ -46,18 +45,12 @@
 #include "sensors/barometer.h"
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
+
 #include "io/beeper.h"
 #include "io/display.h"
 #include "io/escservo.h"
-#include "rx/rx.h"
 #include "io/rc_controls.h"
 #include "io/rc_curves.h"
-#include "flight/mixer.h"
-#include "flight/altitudehold.h"
-#include "flight/failsafe.h"
-#include "flight/imu.h"
-#include "flight/autotune.h"
-#include "flight/navigation.h"
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
@@ -65,10 +58,22 @@
 #include "io/serial_cli.h"
 #include "io/serial_msp.h"
 #include "io/statusindicator.h"
+
+#include "rx/rx.h"
 #include "rx/msp.h"
+
 #include "telemetry/telemetry.h"
 #include "blackbox/blackbox.h"
 
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/altitudehold.h"
+#include "flight/failsafe.h"
+#include "flight/autotune.h"
+#include "flight/navigation.h"
+
+
 #include "config/runtime_config.h"
 #include "config/config.h"
 #include "config/config_profile.h"
@@ -89,6 +94,8 @@ int16_t debug[4];
 uint32_t currentTime = 0;
 uint32_t previousTime = 0;
 uint16_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
+
+int16_t magHold;
 int16_t headFreeModeHold;
 
 int16_t telemTemperature1;      // gyro sensor temperature
@@ -124,7 +131,7 @@ void updateAutotuneState(void)
                     autotuneReset();
                     landedAfterAutoTuning = false;
                 }
-                autotuneBeginNextPhase(&currentProfile->pidProfile, currentProfile->pidController);
+                autotuneBeginNextPhase(&currentProfile->pidProfile);
                 ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
                 autoTuneWasUsed = true;
             } else {
@@ -663,7 +670,7 @@ void loop(void)
     if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
         loopTime = currentTime + masterConfig.looptime;
 
-        computeIMU(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
+        imuUpdate(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
 
         // Measure loop rate just after reading the sensors
         currentTime = micros();
diff --git a/src/main/mw.h b/src/main/mw.h
index aebee40be..d1d49bdf7 100644
--- a/src/main/mw.h
+++ b/src/main/mw.h
@@ -17,6 +17,8 @@
 
 #pragma once
 
+extern int16_t magHold;
+
 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta);
 void handleInflightCalibrationStickPosition();
 
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 17ce1f7a1..07ea188d6 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -21,6 +21,8 @@
 
 #include <string.h>
 
+#include "build_config.h"
+
 #include "platform.h"
 
 #include "common/maths.h"
@@ -59,7 +61,7 @@ bool rxMspInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadR
 
 const char rcChannelLetters[] = "AERT12345678abcdefgh";
 
-uint16_t rssi;                  // range: [0;1023]
+uint16_t rssi = 0;                  // range: [0;1023]
 
 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];     // interval [1000;2000]
 
@@ -377,6 +379,9 @@ void updateRSSIPWM(void)
 
 void updateRSSIADC(uint32_t currentTime)
 {
+#ifndef USE_ADC
+    UNUSED(currentTime);
+#else
     static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT];
     static uint8_t adcRssiSampleIndex = 0;
     static uint32_t rssiUpdateAt = 0;
@@ -403,6 +408,7 @@ void updateRSSIADC(uint32_t currentTime)
     adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT;
 
     rssi = (uint16_t)((constrain(adcRssiMean, 0, 100) / 100.0f) * 1023.0f);
+#endif
 }
 
 void updateRSSI(uint32_t currentTime)
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 303a2148a..f02e0e992 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -25,8 +25,6 @@
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
 
-#include "flight/flight.h"
-
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
 #include "io/beeper.h"
@@ -36,6 +34,8 @@
 
 #include "sensors/acceleration.h"
 
+int16_t accADC[XYZ_AXIS_COUNT];
+
 acc_t acc;                       // acc access functions
 uint8_t accHardware = ACC_DEFAULT;  // which accel chip is used/detected
 sensor_align_e accAlign = 0;
@@ -71,6 +71,12 @@ bool isOnFirstAccelerationCalibrationCycle(void)
     return calibratingA == CALIBRATING_ACC_CYCLES;
 }
 
+void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    rollAndPitchTrims->values.roll = 0;
+    rollAndPitchTrims->values.pitch = 0;
+}
+
 void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
 {
     static int32_t a[3];
@@ -92,9 +98,9 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
 
     if (isOnFinalAccelerationCalibrationCycle()) {
         // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
-        accelerationTrims->raw[FD_ROLL] = (a[FD_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-        accelerationTrims->raw[FD_PITCH] = (a[FD_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-        accelerationTrims->raw[FD_YAW] = (a[FD_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
+        accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
+        accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
+        accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
 
         resetRollAndPitchTrims(rollAndPitchTrims);
 
@@ -113,9 +119,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
 
     // Saving old zeropoints before measurement
     if (InflightcalibratingA == 50) {
-        accZero_saved[FD_ROLL] = accelerationTrims->raw[FD_ROLL];
-        accZero_saved[FD_PITCH] = accelerationTrims->raw[FD_PITCH];
-        accZero_saved[FD_YAW] = accelerationTrims->raw[FD_YAW];
+        accZero_saved[X] = accelerationTrims->raw[X];
+        accZero_saved[Y] = accelerationTrims->raw[Y];
+        accZero_saved[Z] = accelerationTrims->raw[Z];
         angleTrim_saved.values.roll = rollAndPitchTrims->values.roll;
         angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
     }
@@ -136,9 +142,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
             AccInflightCalibrationMeasurementDone = true;
             queueConfirmationBeep(5); // beeper to indicating the end of calibration
             // recover saved values to maintain current flight behaviour until new values are transferred
-            accelerationTrims->raw[FD_ROLL] = accZero_saved[FD_ROLL];
-            accelerationTrims->raw[FD_PITCH] = accZero_saved[FD_PITCH];
-            accelerationTrims->raw[FD_YAW] = accZero_saved[FD_YAW];
+            accelerationTrims->raw[X] = accZero_saved[X];
+            accelerationTrims->raw[Y] = accZero_saved[Y];
+            accelerationTrims->raw[Z] = accZero_saved[Z];
             rollAndPitchTrims->values.roll = angleTrim_saved.values.roll;
             rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch;
         }
@@ -147,9 +153,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
     // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
     if (AccInflightCalibrationSavetoEEProm) {      // the aircraft is landed, disarmed and the combo has been done again
         AccInflightCalibrationSavetoEEProm = false;
-        accelerationTrims->raw[FD_ROLL] = b[FD_ROLL] / 50;
-        accelerationTrims->raw[FD_PITCH] = b[FD_PITCH] / 50;
-        accelerationTrims->raw[FD_YAW] = b[FD_YAW] / 50 - acc_1G;    // for nunchuck 200=1G
+        accelerationTrims->raw[X] = b[X] / 50;
+        accelerationTrims->raw[Y] = b[Y] / 50;
+        accelerationTrims->raw[Z] = b[Z] / 50 - acc_1G;    // for nunchuck 200=1G
 
         resetRollAndPitchTrims(rollAndPitchTrims);
 
@@ -159,9 +165,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
 
 void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
 {
-    accADC[FD_ROLL] -= accelerationTrims->raw[FD_ROLL];
-    accADC[FD_PITCH] -= accelerationTrims->raw[FD_PITCH];
-    accADC[FD_YAW] -= accelerationTrims->raw[FD_YAW];
+    accADC[X] -= accelerationTrims->raw[X];
+    accADC[Y] -= accelerationTrims->raw[Y];
+    accADC[Z] -= accelerationTrims->raw[Z];
 }
 
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index b1cd83d75..159393aa1 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -36,12 +36,20 @@ extern sensor_align_e accAlign;
 extern acc_t acc;
 extern uint16_t acc_1G;
 
-typedef struct accDeadband_s {
-    uint8_t xy;                 // set the acc deadband for xy-Axis
-    uint8_t z;                  // set the acc deadband for z-Axis, this ignores small accelerations
-} accDeadband_t;
+extern int16_t accADC[XYZ_AXIS_COUNT];
+
+typedef struct rollAndPitchTrims_s {
+    int16_t roll;
+    int16_t pitch;
+} rollAndPitchTrims_t_def;
+
+typedef union {
+    int16_t raw[2];
+    rollAndPitchTrims_t_def values;
+} rollAndPitchTrims_t;
 
 bool isAccelerationCalibrationComplete(void);
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
+void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
 void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse);
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index 343c9d535..e3c18036a 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -28,7 +28,6 @@
 #include "drivers/gpio.h"
 #include "drivers/light_led.h"
 
-#include "flight/flight.h"
 #include "sensors/boardalignment.h"
 #include "config/runtime_config.h"
 #include "config/config.h"
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index d490a91f5..71ac374bb 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -25,7 +25,6 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
-#include "flight/flight.h"
 #include "sensors/sensors.h"
 #include "io/statusindicator.h"
 #include "sensors/boardalignment.h"
@@ -33,6 +32,8 @@
 #include "sensors/gyro.h"
 
 uint16_t calibratingG = 0;
+int16_t gyroADC[XYZ_AXIS_COUNT];
+int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
 
@@ -108,7 +109,7 @@ static void applyGyroZero(void)
     }
 }
 
-void gyroGetADC(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.
 
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 7acd2eb2a..caa34281d 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -20,12 +20,15 @@
 extern gyro_t gyro;
 extern sensor_align_e gyroAlign;
 
+extern int16_t gyroADC[XYZ_AXIS_COUNT];
+extern int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
+
 typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
 void useGyroConfig(gyroConfig_t *gyroConfigToUse);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
-void gyroGetADC(void);
+void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);
 
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 099e6fe00..f278893e7 100644
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -52,7 +52,6 @@
 #include "drivers/gpio.h"
 #include "drivers/system.h"
 
-#include "flight/flight.h"
 #include "config/runtime_config.h"
 
 #include "sensors/sensors.h"
diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h
index 81d82a9f8..805043d5c 100644
--- a/src/main/sensors/sensors.h
+++ b/src/main/sensors/sensors.h
@@ -17,6 +17,18 @@
 
 #pragma once
 
+
+typedef struct int16_flightDynamicsTrims_s {
+    int16_t roll;
+    int16_t pitch;
+    int16_t yaw;
+} flightDynamicsTrims_def_t;
+
+typedef union {
+    int16_t raw[3];
+    flightDynamicsTrims_def_t values;
+} flightDynamicsTrims_t;
+
 #define CALIBRATING_GYRO_CYCLES             1000
 #define CALIBRATING_ACC_CYCLES              400
 #define CALIBRATING_BARO_CYCLES             200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c
index 842df6404..b3a5438b9 100644
--- a/src/main/sensors/sonar.c
+++ b/src/main/sensors/sonar.c
@@ -28,8 +28,6 @@
 #include "config/runtime_config.h"
 #include "config/config.h"
 
-#include "flight/flight.h"
-
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index cc699343c..e27951b9f 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -70,6 +70,27 @@
 
 #define USE_ADC
 
+#define ADC_INSTANCE                ADC1
+#define ADC_AHB_PERIPHERAL          RCC_AHBPeriph_DMA1
+#define ADC_DMA_CHANNEL             DMA1_Channel1
+
+#define VBAT_ADC_GPIO               GPIOC
+#define VBAT_ADC_GPIO_PIN           GPIO_Pin_0
+#define VBAT_ADC_CHANNEL            ADC_Channel_6
+
+#define CURRENT_METER_ADC_GPIO      GPIOC
+#define CURRENT_METER_ADC_GPIO_PIN  GPIO_Pin_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_Channel_7
+
+#define RSSI_ADC_GPIO               GPIOC
+#define RSSI_ADC_GPIO_PIN           GPIO_Pin_2
+#define RSSI_ADC_CHANNEL            ADC_Channel_8
+
+#define EXTERNAL1_ADC_GPIO          GPIOC
+#define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_3
+#define EXTERNAL1_ADC_CHANNEL       ADC_Channel_9
+
+
 #define SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
 
 #define GPS
diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h
index 34ec32a7d..87dd9aadd 100644
--- a/src/main/target/OLIMEXINO/target.h
+++ b/src/main/target/OLIMEXINO/target.h
@@ -113,4 +113,4 @@
 #define TELEMETRY
 #define SERIAL_RX
 #define AUTOTUNE
-
+#define BLACKBOX
diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h
index aee9257cd..d9bfe31e3 100644
--- a/src/main/target/SPARKY/target.h
+++ b/src/main/target/SPARKY/target.h
@@ -39,8 +39,8 @@
 
 #define ACC_MPU6050_ALIGN CW270_DEG
 
-//#define BARO
-//#define USE_BARO_MS5611
+#define BARO
+#define USE_BARO_MS5611
 
 #define MAG
 #define USE_MAG_AK8975
diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h
index af35c8a18..5f4ce6693 100644
--- a/src/main/target/SPRACINGF3/target.h
+++ b/src/main/target/SPRACINGF3/target.h
@@ -26,20 +26,24 @@
 #define BEEP_GPIO   GPIOC
 #define BEEP_PIN    Pin_15
 #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC
+#define BEEPER_INVERTED
 
 #define USABLE_TIMER_CHANNEL_COUNT 17
 
 #define GYRO
 #define USE_GYRO_MPU6050
+#define GYRO_MPU6050_ALIGN CW270_DEG
 
 #define ACC
 #define USE_ACC_MPU6050
+#define ACC_MPU6050_ALIGN CW270_DEG
 
 #define BARO
 #define USE_BARO_MS5611
 
 #define MAG
 #define USE_MAG_HMC5883
+#define MAG_HMC5883_ALIGN CW270_DEG
 
 #define BEEPER
 #define LED0
@@ -49,6 +53,15 @@
 #define USE_USART3
 #define SERIAL_PORT_COUNT 3
 
+#ifndef UART1_GPIO
+#define UART1_TX_PIN        GPIO_Pin_9  // PA9
+#define UART1_RX_PIN        GPIO_Pin_10 // PA10
+#define UART1_GPIO          GPIOA
+#define UART1_GPIO_AF       GPIO_AF_7
+#define UART1_TX_PINSOURCE  GPIO_PinSource9
+#define UART1_RX_PINSOURCE  GPIO_PinSource10
+#endif
+
 #define UART2_TX_PIN        GPIO_Pin_14 // PA14 / SWCLK
 #define UART2_RX_PIN        GPIO_Pin_15 // PA15
 #define UART2_GPIO          GPIOA
@@ -56,12 +69,43 @@
 #define UART2_TX_PINSOURCE  GPIO_PinSource14
 #define UART2_RX_PINSOURCE  GPIO_PinSource15
 
+#ifndef UART3_GPIO
+#define UART3_TX_PIN        GPIO_Pin_10 // PB10 (AF7)
+#define UART3_RX_PIN        GPIO_Pin_11 // PB11 (AF7)
+#define UART2_GPIO_AF       GPIO_AF_7
+#define UART3_GPIO          GPIOB
+#define UART3_TX_PINSOURCE  GPIO_PinSource10
+#define UART3_RX_PINSOURCE  GPIO_PinSource11
+#endif
+
 #define USE_I2C
 #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA
 
 //#define USE_SPI
 //#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5
 
+#define USE_ADC
+
+#define ADC_INSTANCE                ADC2
+#define ADC_DMA_CHANNEL             DMA2_Channel1
+#define ADC_AHB_PERIPHERAL          RCC_AHBPeriph_DMA2
+
+#define VBAT_ADC_GPIO               GPIOA
+#define VBAT_ADC_GPIO_PIN           GPIO_Pin_4
+#define VBAT_ADC_CHANNEL            ADC_Channel_1
+
+#define CURRENT_METER_ADC_GPIO      GPIOA
+#define CURRENT_METER_ADC_GPIO_PIN  GPIO_Pin_5
+#define CURRENT_METER_ADC_CHANNEL   ADC_Channel_2
+
+#define RSSI_ADC_GPIO               GPIOB
+#define RSSI_ADC_GPIO_PIN           GPIO_Pin_2
+#define RSSI_ADC_CHANNEL            ADC_Channel_12
+
+#define RSSI_ADC_GPIO               GPIOB
+#define RSSI_ADC_GPIO_PIN           GPIO_Pin_2
+#define RSSI_ADC_CHANNEL            ADC_Channel_12
+
 #define LED_STRIP
 #define LED_STRIP_TIMER TIM1
 
@@ -83,4 +127,4 @@
 #define TELEMETRY
 #define SERIAL_RX
 #define AUTOTUNE
-
+#define DISPLAY
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index ab840213f..29f5f39e8 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -61,6 +61,26 @@
 
 #define USE_ADC
 
+#define ADC_INSTANCE                ADC1
+#define ADC_AHB_PERIPHERAL          RCC_AHBPeriph_DMA1
+#define ADC_DMA_CHANNEL             DMA1_Channel1
+
+#define VBAT_ADC_GPIO               GPIOC
+#define VBAT_ADC_GPIO_PIN           GPIO_Pin_0
+#define VBAT_ADC_CHANNEL            ADC_Channel_6
+
+#define CURRENT_METER_ADC_GPIO      GPIOC
+#define CURRENT_METER_ADC_GPIO_PIN  GPIO_Pin_1
+#define CURRENT_METER_ADC_CHANNEL   ADC_Channel_7
+
+#define RSSI_ADC_GPIO               GPIOC
+#define RSSI_ADC_GPIO_PIN           GPIO_Pin_2
+#define RSSI_ADC_CHANNEL            ADC_Channel_8
+
+#define EXTERNAL1_ADC_GPIO          GPIOC
+#define EXTERNAL1_ADC_GPIO_PIN      GPIO_Pin_3
+#define EXTERNAL1_ADC_CHANNEL       ADC_Channel_9
+
 #define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
 
 #define BLACKBOX
diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c
index f30f961eb..25691fb47 100644
--- a/src/main/telemetry/frsky.c
+++ b/src/main/telemetry/frsky.c
@@ -36,20 +36,28 @@
 #include "drivers/gpio.h"
 #include "drivers/timer.h"
 #include "drivers/serial.h"
-#include "io/serial.h"
-#include "rx/rx.h"
-#include "io/rc_controls.h"
 
-#include "config/runtime_config.h"
-#include "config/config.h"
 
 #include "sensors/sensors.h"
+#include "sensors/acceleration.h"
 #include "sensors/gyro.h"
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
-#include "flight/flight.h"
+
+#include "io/serial.h"
+#include "io/rc_controls.h"
 #include "io/gps.h"
 
+#include "rx/rx.h"
+
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/altitudehold.h"
+
+#include "config/runtime_config.h"
+#include "config/config.h"
+
 #include "telemetry/telemetry.h"
 #include "telemetry/frsky.h"
 
diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c
index 356c1f2b6..d0bfd56bf 100644
--- a/src/main/telemetry/hott.c
+++ b/src/main/telemetry/hott.c
@@ -74,7 +74,7 @@
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
 
-#include "flight/flight.h"
+#include "flight/pid.h"
 #include "flight/navigation.h"
 #include "io/gps.h"
 
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 4451fd856..01c8d74a6 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -27,18 +27,16 @@
 #include "drivers/adc.h"
 #include "drivers/light_led.h"
 
-#include "flight/flight.h"
-#include "flight/mixer.h"
-#include "flight/failsafe.h"
-#include "flight/navigation.h"
 #include "rx/rx.h"
 #include "rx/msp.h"
+
 #include "io/escservo.h"
 #include "io/rc_controls.h"
 #include "io/gps.h"
 #include "io/gimbal.h"
 #include "io/serial.h"
 #include "io/ledstrip.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
@@ -47,6 +45,13 @@
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
 
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/mixer.h"
+#include "flight/failsafe.h"
+#include "flight/navigation.h"
+#include "flight/altitudehold.h"
+
 #include "telemetry/telemetry.h"
 #include "telemetry/smartport.h"
 
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index ad3e45cf1..8d840aca2 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -95,7 +95,7 @@ bool canUseTelemetryWithCurrentConfiguration(void)
     return true;
 }
 
-void initTelemetry()
+void telemetryInit()
 {
     if (isTelemetryProviderSmartPort()) {
         telemetryPortIsShared = isSerialPortFunctionShared(FUNCTION_SMARTPORT_TELEMETRY, FUNCTION_MSP);
diff --git a/src/main/version.h b/src/main/version.h
index cb3706b0c..eb315aaae 100644
--- a/src/main/version.h
+++ b/src/main/version.h
@@ -17,7 +17,11 @@
 
 #define FC_VERSION_MAJOR            1  // increment when a major release is made (big new feature, etc)
 #define FC_VERSION_MINOR            7  // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      0  // increment when a bug is fixed
+#define FC_VERSION_PATCH_LEVEL      2  // increment when a bug is fixed
+
+#define STR_HELPER(x) #x
+#define STR(x) STR_HELPER(x)
+#define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)
 
 #define MW_VERSION              231
 
diff --git a/src/test/Makefile b/src/test/Makefile
index 6642d98b8..9f3c5db21 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -263,6 +263,7 @@ $(OBJECT_DIR)/rc_controls_unittest.o : \
 	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@
 
 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
diff --git a/src/test/unit/altitude_hold_unittest.cc b/src/test/unit/altitude_hold_unittest.cc
index bc2b7e467..b895a1b1b 100644
--- a/src/test/unit/altitude_hold_unittest.cc
+++ b/src/test/unit/altitude_hold_unittest.cc
@@ -24,25 +24,27 @@
 
 extern "C" {
     #include "common/axis.h"
-    #include "flight/flight.h"
+    #include "common/maths.h"
 
-    #include "sensors/sensors.h"
     #include "drivers/sensor.h"
     #include "drivers/accgyro.h"
+
+    #include "sensors/sensors.h"
     #include "sensors/acceleration.h"
     #include "sensors/barometer.h"
 
-    #include "flight/mixer.h"
-    #include "flight/mixer.h"
-
     #include "io/escservo.h"
-    #include "rx/rx.h"
     #include "io/rc_controls.h"
 
-    #include "config/runtime_config.h"
+    #include "rx/rx.h"
 
+    #include "flight/mixer.h"
+    #include "flight/pid.h"
+    #include "flight/imu.h"
     #include "flight/altitudehold.h"
 
+    #include "config/runtime_config.h"
+
 }
 
 #include "unittest_macros.h"
@@ -148,7 +150,7 @@ uint8_t armingFlags;
 int32_t sonarAlt;
 
 
-void gyroGetADC(void) {};
+void gyroUpdate(void) {};
 bool sensors(uint32_t mask)
 {
     UNUSED(mask);
@@ -159,7 +161,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
     UNUSED(rollAndPitchTrims);
 }
 
-void accSum_reset(void) {};
+void imuResetAccelerationSum(void) {};
 
 int32_t applyDeadband(int32_t, int32_t) { return 0; }
 uint32_t micros(void) { return 0; }
diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc
index d82d12fb1..379b2cfaf 100644
--- a/src/test/unit/flight_imu_unittest.cc
+++ b/src/test/unit/flight_imu_unittest.cc
@@ -25,7 +25,6 @@
 extern "C" {
     #include "common/axis.h"
     #include "common/maths.h"
-    #include "flight/flight.h"
 
     #include "sensors/sensors.h"
     #include "drivers/sensor.h"
@@ -39,6 +38,7 @@ extern "C" {
     #include "config/runtime_config.h"
 
     #include "flight/mixer.h"
+    #include "flight/pid.h"
     #include "flight/imu.h"
 }
 
@@ -53,19 +53,19 @@ TEST(FlightImuTest, TestCalculateHeading)
 {
     //TODO: Add test cases using the Z dimension.
     t_fp_vector north = {.A={1.0f, 0.0f, 0.0f}};
-    EXPECT_EQ(calculateHeading(&north), 0);
+    EXPECT_EQ(imuCalculateHeading(&north), 0);
 
     t_fp_vector east = {.A={0.0f, 1.0f, 0.0f}};
-    EXPECT_EQ(calculateHeading(&east), 90);
+    EXPECT_EQ(imuCalculateHeading(&east), 90);
 
     t_fp_vector south = {.A={-1.0f, 0.0f, 0.0f}};
-    EXPECT_EQ(calculateHeading(&south), 180);
+    EXPECT_EQ(imuCalculateHeading(&south), 180);
 
     t_fp_vector west = {.A={0.0f, -1.0f, 0.0f}};
-    EXPECT_EQ(calculateHeading(&west), 270);
+    EXPECT_EQ(imuCalculateHeading(&west), 270);
 
     t_fp_vector north_east = {.A={1.0f, 1.0f, 0.0f}};
-    EXPECT_EQ(calculateHeading(&north_east), 45);
+    EXPECT_EQ(imuCalculateHeading(&north_east), 45);
 }
 
 // STUBS
@@ -86,9 +86,12 @@ uint16_t flightModeFlags;
 uint8_t armingFlags;
 
 int32_t sonarAlt;
+int16_t accADC[XYZ_AXIS_COUNT];
+int16_t gyroADC[XYZ_AXIS_COUNT];
 
 
-void gyroGetADC(void) {};
+
+void gyroUpdate(void) {};
 bool sensors(uint32_t mask)
 {
     UNUSED(mask);
diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc
index 8ad6e3144..6cd1bf9dc 100644
--- a/src/test/unit/ledstrip_unittest.cc
+++ b/src/test/unit/ledstrip_unittest.cc
@@ -24,7 +24,6 @@ extern "C" {
 
     #include "common/color.h"
     #include "common/axis.h"
-    #include "flight/flight.h"
 
     #include "sensors/battery.h"
     #include "config/runtime_config.h"
diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h
index 39c809316..f74b9be29 100644
--- a/src/test/unit/platform.h
+++ b/src/test/unit/platform.h
@@ -25,3 +25,4 @@
 
 #define SERIAL_PORT_COUNT 4
 
+#define MAX_SIMULTANEOUS_ADJUSTMENT_COUNT 6
diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc
index 54a830278..0c3580f06 100644
--- a/src/test/unit/rc_controls_unittest.cc
+++ b/src/test/unit/rc_controls_unittest.cc
@@ -17,27 +17,32 @@
 #include <stdint.h>
 
 #include <limits.h>
-extern "C" {
-    #include "common/axis.h"
-    #include "flight/flight.h"
 
-    #include "rx/rx.h"
+extern "C" {
+    #include "platform.h"
+
+    #include "common/maths.h"
+    #include "common/axis.h"
+
+    #include "drivers/sensor.h"
+    #include "drivers/accgyro.h"
+
+    #include "sensors/sensors.h"
+    #include "sensors/acceleration.h"
+
     #include "io/escservo.h"
     #include "io/rc_controls.h"
+
+    #include "rx/rx.h"
+
+    #include "flight/pid.h"
 }
+
 #include "unittest_macros.h"
 #include "gtest/gtest.h"
 
 extern "C" {
-int constrain(int amt, int low, int high)
-{
-    if (amt < low)
-        return low;
-    else if (amt > high)
-        return high;
-    else
-        return amt;
-}
+extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile);
 }
 
 TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
@@ -479,6 +484,215 @@ TEST(RcControlsTest, processRcRateProfileAdjustments)
     EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
 }
 
+static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = {
+    .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
+    .mode = ADJUSTMENT_MODE_STEP,
+    .data = { { 1 } }
+};
+
+static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = {
+    .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
+    .mode = ADJUSTMENT_MODE_STEP,
+    .data = { { 1 } }
+};
+
+static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = {
+    .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
+    .mode = ADJUSTMENT_MODE_STEP,
+    .data = { { 1 } }
+};
+
+static const adjustmentConfig_t pidYawPAdjustmentConfig = {
+    .adjustmentFunction = ADJUSTMENT_YAW_P,
+    .mode = ADJUSTMENT_MODE_STEP,
+    .data = { { 1 } }
+};
+
+static const adjustmentConfig_t pidYawIAdjustmentConfig = {
+    .adjustmentFunction = ADJUSTMENT_YAW_I,
+    .mode = ADJUSTMENT_MODE_STEP,
+    .data = { { 1 } }
+};
+
+static const adjustmentConfig_t pidYawDAdjustmentConfig = {
+    .adjustmentFunction = ADJUSTMENT_YAW_D,
+    .mode = ADJUSTMENT_MODE_STEP,
+    .data = { { 1 } }
+};
+
+TEST(RcControlsTest, processPIDIncreasePidController0)
+{
+    // given
+    modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
+    memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
+
+    escAndServoConfig_t escAndServoConfig;
+    memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
+
+    pidProfile_t pidProfile;
+    memset(&pidProfile, 0, sizeof (pidProfile));
+    pidProfile.pidController = 0;
+    pidProfile.P8[PIDPITCH] = 0;
+    pidProfile.P8[PIDROLL] = 5;
+    pidProfile.P8[YAW] = 7;
+    pidProfile.I8[PIDPITCH] = 10;
+    pidProfile.I8[PIDROLL] = 15;
+    pidProfile.I8[YAW] = 17;
+    pidProfile.D8[PIDPITCH] = 20;
+    pidProfile.D8[PIDROLL] = 25;
+    pidProfile.D8[YAW] = 27;
+
+    // and
+    controlRateConfig_t controlRateConfig;
+    memset(&controlRateConfig, 0, sizeof (controlRateConfig));
+
+    // and
+    memset(&rxConfig, 0, sizeof (rxConfig));
+    rxConfig.mincheck = DEFAULT_MIN_CHECK;
+    rxConfig.maxcheck = DEFAULT_MAX_CHECK;
+    rxConfig.midrc = 1500;
+
+    adjustmentStateMask = 0;
+    memset(&adjustmentStates, 0, sizeof(adjustmentStates));
+
+    configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
+    configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
+    configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
+    configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
+    configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
+    configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
+
+    // and
+    uint8_t index;
+    for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
+        rcData[index] = PWM_RANGE_MIDDLE;
+    }
+
+    // and
+    resetCallCounters();
+    resetMillis();
+
+    // and
+    rcData[AUX1] = PWM_RANGE_MAX;
+    rcData[AUX2] = PWM_RANGE_MAX;
+    rcData[AUX3] = PWM_RANGE_MAX;
+
+    // and
+    uint8_t expectedAdjustmentStateMask =
+            (1 << 0) |
+            (1 << 1) |
+            (1 << 2) |
+            (1 << 3) |
+            (1 << 4) |
+            (1 << 5);
+
+    // when
+    useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
+    processRcAdjustments(&controlRateConfig, &rxConfig);
+
+    // then
+    EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
+    EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
+
+    // and
+    EXPECT_EQ(1, pidProfile.P8[PIDPITCH]);
+    EXPECT_EQ(6, pidProfile.P8[PIDROLL]);
+    EXPECT_EQ(8, pidProfile.P8[YAW]);
+    EXPECT_EQ(11, pidProfile.I8[PIDPITCH]);
+    EXPECT_EQ(16, pidProfile.I8[PIDROLL]);
+    EXPECT_EQ(18, pidProfile.I8[YAW]);
+    EXPECT_EQ(21, pidProfile.D8[PIDPITCH]);
+    EXPECT_EQ(26, pidProfile.D8[PIDROLL]);
+    EXPECT_EQ(28, pidProfile.D8[YAW]);
+}
+
+TEST(RcControlsTest, processPIDIncreasePidController2)
+{
+    // given
+    modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
+    memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
+
+    escAndServoConfig_t escAndServoConfig;
+    memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
+
+    pidProfile_t pidProfile;
+    memset(&pidProfile, 0, sizeof (pidProfile));
+    pidProfile.pidController = 2;
+    pidProfile.P_f[PIDPITCH] = 0.0f;
+    pidProfile.P_f[PIDROLL] = 5.0f;
+    pidProfile.P_f[PIDYAW] = 7.0f;
+    pidProfile.I_f[PIDPITCH] = 10.0f;
+    pidProfile.I_f[PIDROLL] = 15.0f;
+    pidProfile.I_f[PIDYAW] = 17.0f;
+    pidProfile.D_f[PIDPITCH] = 20.0f;
+    pidProfile.D_f[PIDROLL] = 25.0f;
+    pidProfile.D_f[PIDYAW] = 27.0f;
+
+    // and
+    controlRateConfig_t controlRateConfig;
+    memset(&controlRateConfig, 0, sizeof (controlRateConfig));
+
+    // and
+    memset(&rxConfig, 0, sizeof (rxConfig));
+    rxConfig.mincheck = DEFAULT_MIN_CHECK;
+    rxConfig.maxcheck = DEFAULT_MAX_CHECK;
+    rxConfig.midrc = 1500;
+
+    adjustmentStateMask = 0;
+    memset(&adjustmentStates, 0, sizeof(adjustmentStates));
+
+    configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
+    configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
+    configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
+    configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
+    configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
+    configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
+
+    // and
+    uint8_t index;
+    for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
+        rcData[index] = PWM_RANGE_MIDDLE;
+    }
+
+    // and
+    resetCallCounters();
+    resetMillis();
+
+    // and
+    rcData[AUX1] = PWM_RANGE_MAX;
+    rcData[AUX2] = PWM_RANGE_MAX;
+    rcData[AUX3] = PWM_RANGE_MAX;
+
+    // and
+    uint8_t expectedAdjustmentStateMask =
+            (1 << 0) |
+            (1 << 1) |
+            (1 << 2) |
+            (1 << 3) |
+            (1 << 4) |
+            (1 << 5);
+
+    // when
+    useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
+    processRcAdjustments(&controlRateConfig, &rxConfig);
+
+    // then
+    EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
+    EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
+
+    // and
+    EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]);
+    EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]);
+    EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]);
+    EXPECT_EQ(10.01f, pidProfile.I_f[PIDPITCH]);
+    EXPECT_EQ(15.01f, pidProfile.I_f[PIDROLL]);
+    EXPECT_EQ(17.01f, pidProfile.I_f[PIDYAW]);
+    EXPECT_EQ(20.001f, pidProfile.D_f[PIDPITCH]);
+    EXPECT_EQ(25.001f, pidProfile.D_f[PIDROLL]);
+    EXPECT_EQ(27.001f, pidProfile.D_f[PIDYAW]);
+
+}
+
 extern "C" {
 void saveConfigAndNotify(void) {}
 void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {}
@@ -494,6 +708,8 @@ void mwDisarm(void) {}
 uint8_t getCurrentControlRateProfile(void) {
     return 0;
 }
+void GPS_reset_home_position(void) {}
+void baroSetCalibrationCycles(uint16_t) {}
 
 uint8_t armingFlags = 0;
 int16_t heading;
diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc
index ad277ba00..51fca8d5a 100644
--- a/src/test/unit/telemetry_hott_unittest.cc
+++ b/src/test/unit/telemetry_hott_unittest.cc
@@ -27,22 +27,21 @@ extern "C" {
     #include "common/axis.h"
 
     #include "drivers/system.h"
-
     #include "drivers/serial.h"
-    #include "io/serial.h"
-
-    #include "config/runtime_config.h"
 
     #include "sensors/sensors.h"
-
-    #include "flight/flight.h"
-    #include "io/gps.h"
     #include "sensors/battery.h"
 
+    #include "io/serial.h"
+    #include "io/gps.h"
+
     #include "telemetry/telemetry.h"
     #include "telemetry/hott.h"
 
+    #include "flight/pid.h"
     #include "flight/gps_conversion.h"
+
+    #include "config/runtime_config.h"
 }
 
 #include "unittest_macros.h"