diff --git a/Makefile b/Makefile
index 0e83a6cb7..9e279d38f 100644
--- a/Makefile
+++ b/Makefile
@@ -73,6 +73,7 @@ COMMON_SRC	 = startup_stm32f10x_md_gcc.S \
 		   flight_mixer.c \
 		   gps_common.c \
 		   runtime_config.c \
+		   rc_controls.c \
 		   rx_common.c \
 		   rx_pwm.c \
 		   rx_sbus.c \
diff --git a/src/config.c b/src/config.c
index 6b163798c..ff9e157b1 100755
--- a/src/config.c
+++ b/src/config.c
@@ -23,6 +23,7 @@
 #include "boardalignment.h"
 #include "battery.h"
 #include "gimbal.h"
+#include "rc_controls.h"
 #include "rx_common.h"
 #include "gps_common.h"
 #include "serial_common.h"
diff --git a/src/drivers/light_ledring.c b/src/drivers/light_ledring.c
index 4245bc94d..9830266eb 100644
--- a/src/drivers/light_ledring.c
+++ b/src/drivers/light_ledring.c
@@ -8,9 +8,13 @@
 #include "common/maths.h"
 
 #include "bus_i2c.h"
+
+// FIXME there should be no dependencies on the main source code
+#include "rc_controls.h"
 #include "sensors_common.h"
 #include "flight_common.h"
 
+#include "light_ledring.h"
 
 // Driver for DFRobot I2C Led Ring
 #define LED_RING_ADDRESS    0x6D
diff --git a/src/failsafe.c b/src/failsafe.c
index 1f6eec2e7..b55be2bc1 100644
--- a/src/failsafe.c
+++ b/src/failsafe.c
@@ -1,10 +1,10 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include "rx_common.h"
-
 #include "common/axis.h"
-#include "flight_common.h"
+
+#include "rc_controls.h"
+#include "rx_common.h"
 #include "runtime_config.h"
 
 #include "failsafe.h"
diff --git a/src/flight_common.c b/src/flight_common.c
index 2e0c1e6fc..aea504e86 100644
--- a/src/flight_common.c
+++ b/src/flight_common.c
@@ -1,13 +1,31 @@
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include "common/axis.h"
+#include "common/maths.h"
 
 #include "runtime_config.h"
 
+#include "rc_controls.h"
 #include "flight_common.h"
+#include "gps_common.h"
+
+extern uint16_t cycleTime;
 
 int16_t heading, magHold;
+int16_t axisPID[3];
+uint8_t dynP8[3], dynI8[3], dynD8[3];
+
+
+static int32_t errorGyroI[3] = { 0, 0, 0 };
+static int32_t errorAngleI[2] = { 0, 0 };
+
+static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);
+
+typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);                // pid controller function prototype
+
+pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
 
 
 void mwDisarm(void)
@@ -15,3 +33,158 @@ void mwDisarm(void)
     if (f.ARMED)
         f.ARMED = 0;
 }
+
+void resetErrorAngle(void)
+{
+    errorAngleI[ROLL] = 0;
+    errorAngleI[PITCH] = 0;
+}
+
+void resetErrorGyro(void)
+{
+    errorGyroI[ROLL] = 0;
+    errorGyroI[PITCH] = 0;
+    errorGyroI[YAW] = 0;
+}
+
+static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
+{
+    int axis, prop;
+    int32_t error, errorAngle;
+    int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
+    static int16_t lastGyro[3] = { 0, 0, 0 };
+    static int32_t delta1[3], delta2[3];
+    int32_t deltaSum;
+    int32_t delta;
+
+    // **** PITCH & ROLL & YAW PID ****
+    prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
+    for (axis = 0; axis < 3; axis++) {
+        if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
+            // 50 degrees max inclination
+            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim[axis];
+            PTermACC = errorAngle * pidProfile->P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
+            PTermACC = constrain(PTermACC, -pidProfile->D8[PIDLEVEL] * 5, +pidProfile->D8[PIDLEVEL] * 5);
+
+            errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
+            ITermACC = (errorAngleI[axis] * pidProfile->I8[PIDLEVEL]) >> 12;
+        }
+        if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
+            error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
+            error -= gyroData[axis];
+
+            PTermGYRO = rcCommand[axis];
+
+            errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
+            if (abs(gyroData[axis]) > 640)
+                errorGyroI[axis] = 0;
+            ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) >> 6;
+        }
+        if (f.HORIZON_MODE && axis < 2) {
+            PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
+            ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
+        } else {
+            if (f.ANGLE_MODE && axis < 2) {
+                PTerm = PTermACC;
+                ITerm = ITermACC;
+            } else {
+                PTerm = PTermGYRO;
+                ITerm = ITermGYRO;
+            }
+        }
+
+        PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
+        delta = gyroData[axis] - lastGyro[axis];
+        lastGyro[axis] = gyroData[axis];
+        deltaSum = delta1[axis] + delta2[axis] + delta;
+        delta2[axis] = delta1[axis];
+        delta1[axis] = delta;
+        DTerm = (deltaSum * dynD8[axis]) / 32;
+        axisPID[axis] = PTerm + ITerm - DTerm;
+    }
+}
+
+#define GYRO_I_MAX 256
+
+static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
+{
+    int32_t errorAngle = 0;
+    int axis;
+    int32_t delta, deltaSum;
+    static int32_t delta1[3], delta2[3];
+    int32_t PTerm, ITerm, DTerm;
+    static int32_t lastError[3] = { 0, 0, 0 };
+    int32_t AngleRateTmp, RateError;
+
+    // ----------PID controller----------
+    for (axis = 0; axis < 3; axis++) {
+        // -----Get the desired angle rate depending on flight mode
+        if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
+            // calculate error and limit the angle to max configured inclination
+            errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim[axis]; // 16 bits is ok here
+        }
+        if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
+            AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[2]) >> 5);
+         } else {
+            if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
+                AngleRateTmp = ((int32_t) (controlRateConfig->rollPitchRate + 27) * rcCommand[axis]) >> 4;
+                if (f.HORIZON_MODE) {
+                    // mix up angle error to desired AngleRateTmp to add a little auto-level feel
+                    AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL]) >> 8;
+                }
+            } else { // it's the ANGLE mode - control is angle based, so control loop is needed
+                AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
+            }
+        }
+
+        // --------low-level gyro-based PID. ----------
+        // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
+        // -----calculate scaled error.AngleRates
+        // multiplication of rcCommand corresponds to changing the sticks scaling here
+        RateError = AngleRateTmp - gyroData[axis];
+
+        // -----calculate P component
+        PTerm = (RateError * pidProfile->P8[axis]) >> 7;
+        // -----calculate I component
+        // there should be no division before accumulating the error to integrator, because the precision would be reduced.
+        // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
+        // Time correction (to avoid different I scaling for different builds based on average cycle time)
+        // is normalized to cycle time = 2048.
+        errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis];
+
+        // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
+        // I coefficient (I8) moved before integration to make limiting independent from PID settings
+        errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
+        ITerm = errorGyroI[axis] >> 13;
+
+        //-----calculate D-term
+        delta = RateError - lastError[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+        lastError[axis] = RateError;
+
+        // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
+        // would be scaled by different dt each time. Division by dT fixes that.
+        delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
+        // add moving average here to reduce noise
+        deltaSum = delta1[axis] + delta2[axis] + delta;
+        delta2[axis] = delta1[axis];
+        delta1[axis] = delta;
+        DTerm = (deltaSum * pidProfile->D8[axis]) >> 8;
+
+        // -----calculate total PID output
+        axisPID[axis] = PTerm + ITerm + DTerm;
+    }
+}
+
+void setPIDController(int type)
+{
+    switch (type) {
+        case 0:
+        default:
+            pid_controller = pidMultiWii;
+            break;
+        case 1:
+            pid_controller = pidRewrite;
+            break;
+    }
+}
+
diff --git a/src/flight_common.h b/src/flight_common.h
index bf660589c..05da52f78 100644
--- a/src/flight_common.h
+++ b/src/flight_common.h
@@ -43,8 +43,13 @@ extern int16_t gyroZero[GYRO_INDEX_COUNT]; // see gyro_index_t
 
 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 int16_t heading, magHold;
 
 void mwDisarm(void);
+void setPIDController(int type);
+void resetErrorAngle(void);
+void resetErrorGyro(void);
+
 
diff --git a/src/flight_imu.c b/src/flight_imu.c
index 270b89876..01675a149 100644
--- a/src/flight_imu.c
+++ b/src/flight_imu.c
@@ -24,6 +24,7 @@
 
 #include "boardalignment.h"
 #include "battery.h"
+#include "rc_controls.h"
 #include "rx_common.h"
 #include "drivers/serial_common.h"
 #include "serial_common.h"
@@ -53,7 +54,6 @@ int32_t vario = 0;                      // variometer in cm/s
 int16_t throttleAngleCorrection = 0;    // correction of throttle in lateral wind,
 float throttleAngleScale;
 
-uint16_t calibratingB = 0;      // baro calibration = get new ground pressure value
 int32_t baroPressure = 0;
 int32_t baroTemperature = 0;
 uint32_t baroPressureSum = 0;
diff --git a/src/flight_mixer.h b/src/flight_mixer.h
index b2c0677a8..59b35d2d2 100644
--- a/src/flight_mixer.h
+++ b/src/flight_mixer.h
@@ -57,3 +57,5 @@ typedef struct servoParam_t {
 } servoParam_t;
 
 bool isMixerUsingServos(void);
+void mixerInit(void);
+void writeAllMotors(int16_t mc);
diff --git a/src/main.c b/src/main.c
index 2776719cb..9728510a3 100755
--- a/src/main.c
+++ b/src/main.c
@@ -1,23 +1,51 @@
-#include "board.h"
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "common/axis.h"
+
+#include "drivers/system_common.h"
+#include "drivers/gpio_common.h"
+#include "drivers/light_led.h"
+#include "drivers/sound_beeper.h"
+#include "drivers/timer_common.h"
+#include "drivers/serial_common.h"
+#include "drivers/serial_softserial.h"
+#include "drivers/serial_uart.h"
+#include "drivers/accgyro_common.h"
+#include "drivers/pwm_common.h"
+#include "drivers/adc_common.h"
+
 #include "flight_common.h"
 #include "flight_mixer.h"
+
 #include "serial_common.h"
 #include "failsafe.h"
-#include "mw.h"
 
 #include "gps_common.h"
+#include "rc_controls.h"
 #include "rx_common.h"
+#include "gimbal.h"
 #include "sensors_common.h"
-#include "drivers/accgyro_common.h"
-#include "drivers/serial_common.h"
+#include "sensors_sonar.h"
+#include "sensors_barometer.h"
+#include "sensors_acceleration.h"
+#include "sensors_gyro.h"
 #include "telemetry_common.h"
+#include "battery.h"
 #include "boardalignment.h"
+#include "runtime_config.h"
 #include "config.h"
+#include "config_profile.h"
+#include "config_master.h"
 
 #include "build_config.h"
 
 extern rcReadRawDataPtr rcReadRawFunc;
 
+extern uint32_t previousTime;
+
 failsafe_t *failsafe;
 
 void initTelemetry(serialPorts_t *serialPorts);
@@ -27,6 +55,10 @@ void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe);
 void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
 void buzzerInit(failsafe_t *initialFailsafe);
 void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
+void sensorsAutodetect(void);
+void imuInit(void);
+
+void loop(void);
 
 int main(void)
 {
@@ -159,10 +191,13 @@ int main(void)
         initTelemetry(&serialPorts);
 
     previousTime = micros();
-    if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL)
-        calibratingA = CALIBRATING_ACC_CYCLES;
-    calibratingG = CALIBRATING_GYRO_CYCLES;
-    calibratingB = CALIBRATING_BARO_CYCLES;             // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
+
+    if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) {
+        ACC_SetCalibrationCycles(CALIBRATING_ACC_CYCLES);
+    }
+    GYRO_SetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+    Baro_SetCalibrationCycles(CALIBRATING_BARO_CYCLES);
+
     f.SMALL_ANGLE = 1;
 
     // loopy
diff --git a/src/mw.c b/src/mw.c
index 93cf3aae0..de65195ed 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -4,6 +4,7 @@
 #include "common/maths.h"
 #include "common/typeconversion.h"
 
+#include "sensors_sonar.h"
 #include "sensors_gyro.h"
 #include "flight_common.h"
 #include "serial_cli.h"
@@ -26,19 +27,15 @@ int16_t headFreeModeHold;
 
 int16_t telemTemperature1;      // gyro sensor temperature
 
-int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 uint16_t rssi;                  // range: [0;1023]
 
-static void pidMultiWii(void);
-static void pidRewrite(void);
-pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
-
-uint8_t dynP8[3], dynI8[3], dynD8[3];
-
-int16_t axisPID[3];
-
+extern uint8_t dynP8[3], dynI8[3], dynD8[3];
 extern failsafe_t *failsafe;
 
+typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);
+
+extern pidControllerFuncPtr pid_controller;
+
 // Automatic ACC Offset Calibration
 bool AccInflightCalibrationArmed = false;
 bool AccInflightCalibrationMeasurementDone = false;
@@ -46,11 +43,6 @@ bool AccInflightCalibrationSavetoEEProm = false;
 bool AccInflightCalibrationActive = false;
 uint16_t InflightcalibratingA = 0;
 
-bool areSticksInApModePosition(uint16_t ap_mode) // FIXME should probably live in rc_sticks.h
-{
-    return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
-}
-
 void annexCode(void)
 {
     static uint32_t calibratedAccTime;
@@ -198,150 +190,6 @@ static void mwVario(void)
 
 }
 
-static int32_t errorGyroI[3] = { 0, 0, 0 };
-static int32_t errorAngleI[2] = { 0, 0 };
-
-static void pidMultiWii(void)
-{
-    int axis, prop;
-    int32_t error, errorAngle;
-    int32_t PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
-    static int16_t lastGyro[3] = { 0, 0, 0 };
-    static int32_t delta1[3], delta2[3];
-    int32_t deltaSum;
-    int32_t delta;
-
-    // **** PITCH & ROLL & YAW PID ****
-    prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
-    for (axis = 0; axis < 3; axis++) {
-        if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
-            // 50 degrees max inclination
-            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis];
-            PTermACC = errorAngle * currentProfile.pidProfile.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
-            PTermACC = constrain(PTermACC, -currentProfile.pidProfile.D8[PIDLEVEL] * 5, +currentProfile.pidProfile.D8[PIDLEVEL] * 5);
-
-            errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
-            ITermACC = (errorAngleI[axis] * currentProfile.pidProfile.I8[PIDLEVEL]) >> 12;
-        }
-        if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
-            error = (int32_t)rcCommand[axis] * 10 * 8 / currentProfile.pidProfile.P8[axis];
-            error -= gyroData[axis];
-
-            PTermGYRO = rcCommand[axis];
-
-            errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
-            if (abs(gyroData[axis]) > 640)
-                errorGyroI[axis] = 0;
-            ITermGYRO = (errorGyroI[axis] / 125 * currentProfile.pidProfile.I8[axis]) >> 6;
-        }
-        if (f.HORIZON_MODE && axis < 2) {
-            PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
-            ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
-        } else {
-            if (f.ANGLE_MODE && axis < 2) {
-                PTerm = PTermACC;
-                ITerm = ITermACC;
-            } else {
-                PTerm = PTermGYRO;
-                ITerm = ITermGYRO;
-            }
-        }
-
-        PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
-        delta = gyroData[axis] - lastGyro[axis];
-        lastGyro[axis] = gyroData[axis];
-        deltaSum = delta1[axis] + delta2[axis] + delta;
-        delta2[axis] = delta1[axis];
-        delta1[axis] = delta;
-        DTerm = (deltaSum * dynD8[axis]) / 32;
-        axisPID[axis] = PTerm + ITerm - DTerm;
-    }
-}
-
-#define GYRO_I_MAX 256
-
-static void pidRewrite(void)
-{
-    int32_t errorAngle = 0;
-    int axis;
-    int32_t delta, deltaSum;
-    static int32_t delta1[3], delta2[3];
-    int32_t PTerm, ITerm, DTerm;
-    static int32_t lastError[3] = { 0, 0, 0 };
-    int32_t AngleRateTmp, RateError;
-
-    // ----------PID controller----------
-    for (axis = 0; axis < 3; axis++) {
-        // -----Get the desired angle rate depending on flight mode
-        if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
-            // calculate error and limit the angle to max configured inclination
-            errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)masterConfig.max_angle_inclination), +masterConfig.max_angle_inclination) - angle[axis] + currentProfile.angleTrim[axis]; // 16 bits is ok here
-        }
-        if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
-            AngleRateTmp = (((int32_t)(currentProfile.controlRateConfig.yawRate + 27) * rcCommand[2]) >> 5);
-         } else {
-            if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
-                AngleRateTmp = ((int32_t) (currentProfile.controlRateConfig.rollPitchRate + 27) * rcCommand[axis]) >> 4;
-                if (f.HORIZON_MODE) {
-                    // mix up angle error to desired AngleRateTmp to add a little auto-level feel
-                    AngleRateTmp += (errorAngle * currentProfile.pidProfile.I8[PIDLEVEL]) >> 8;
-                }
-            } else { // it's the ANGLE mode - control is angle based, so control loop is needed
-                AngleRateTmp = (errorAngle * currentProfile.pidProfile.P8[PIDLEVEL]) >> 4;
-            }
-        }
-
-        // --------low-level gyro-based PID. ----------
-        // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
-        // -----calculate scaled error.AngleRates
-        // multiplication of rcCommand corresponds to changing the sticks scaling here
-        RateError = AngleRateTmp - gyroData[axis];
-
-        // -----calculate P component
-        PTerm = (RateError * currentProfile.pidProfile.P8[axis]) >> 7;
-        // -----calculate I component
-        // there should be no division before accumulating the error to integrator, because the precision would be reduced.
-        // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
-        // Time correction (to avoid different I scaling for different builds based on average cycle time)
-        // is normalized to cycle time = 2048.
-        errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * currentProfile.pidProfile.I8[axis];
-
-        // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
-        // I coefficient (I8) moved before integration to make limiting independent from PID settings
-        errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
-        ITerm = errorGyroI[axis] >> 13;
-
-        //-----calculate D-term
-        delta = RateError - lastError[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
-        lastError[axis] = RateError;
-
-        // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
-        // would be scaled by different dt each time. Division by dT fixes that.
-        delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
-        // add moving average here to reduce noise
-        deltaSum = delta1[axis] + delta2[axis] + delta;
-        delta2[axis] = delta1[axis];
-        delta1[axis] = delta;
-        DTerm = (deltaSum * currentProfile.pidProfile.D8[axis]) >> 8;
-
-        // -----calculate total PID output
-        axisPID[axis] = PTerm + ITerm + DTerm;
-    }
-}
-
-void setPIDController(int type)
-{
-    switch (type) {
-        case 0:
-        default:
-            pid_controller = pidMultiWii;
-            break;
-        case 1:
-            pid_controller = pidRewrite;
-            break;
-    }
-}
-
 void loop(void)
 {
     static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
@@ -418,11 +266,8 @@ void loop(void)
         else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < masterConfig.rxConfig.mincheck))
             isThrottleLow = true;
         if (isThrottleLow) {
-            errorGyroI[ROLL] = 0;
-            errorGyroI[PITCH] = 0;
-            errorGyroI[YAW] = 0;
-            errorAngleI[ROLL] = 0;
-            errorAngleI[PITCH] = 0;
+            resetErrorAngle();
+            resetErrorGyro();
             if (currentProfile.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
                 if (rcOptions[BOXARM] && f.OK_TO_ARM)
                     mwArm();
@@ -540,8 +385,7 @@ void loop(void)
         if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
             // bumpless transfer to Level mode
             if (!f.ANGLE_MODE) {
-                errorAngleI[ROLL] = 0;
-                errorAngleI[PITCH] = 0;
+                resetErrorAngle();
                 f.ANGLE_MODE = 1;
             }
         } else {
@@ -551,8 +395,7 @@ void loop(void)
         if (rcOptions[BOXHORIZON]) {
             f.ANGLE_MODE = 0;
             if (!f.HORIZON_MODE) {
-                errorAngleI[ROLL] = 0;
-                errorAngleI[PITCH] = 0;
+                resetErrorAngle();
                 f.HORIZON_MODE = 1;
             }
         } else {
@@ -757,7 +600,7 @@ void loop(void)
         }
 
         // PID - note this is function pointer set by setPIDController()
-        pid_controller();
+        pid_controller(&currentProfile.pidProfile, &currentProfile.controlRateConfig, masterConfig.max_angle_inclination, currentProfile.angleTrim);
 
         mixTable();
         writeServos();
diff --git a/src/mw.h b/src/mw.h
index c82ab730f..077cdaa75 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include "rc_controls.h"
 #include "runtime_config.h"
 #include "flight_common.h"
 #include "failsafe.h"
@@ -19,26 +20,19 @@ enum {
     ALIGN_MAG = 2
 };
 
-#define CALIBRATING_GYRO_CYCLES             1000
-#define CALIBRATING_ACC_CYCLES              400
-#define CALIBRATING_BARO_CYCLES             200
-
 #include "serial_common.h"
+#include "rc_controls.h"
 #include "rx_common.h"
 #include "gps_common.h"
 #include "config.h"
 #include "config_profile.h"
 #include "config_master.h"
 
-extern int16_t axisPID[3];
-extern int16_t rcCommand[4];
-
 extern int16_t debug[4];
 extern uint16_t acc_1G;
 extern uint32_t accTimeSum;
 extern int accSumCount;
 extern uint32_t currentTime;
-extern uint32_t previousTime;
 extern uint16_t cycleTime;
 extern uint16_t calibratingA;
 extern uint16_t calibratingB;
@@ -65,28 +59,19 @@ extern uint8_t toggleBeep;
 
 extern flags_t f;
 
-// main
-void setPIDController(int type);
-void loop(void);
-
 // IMU
-void imuInit(void);
 void annexCode(void);
 void computeIMU(void);
 int getEstimatedAltitude(void);
 
 // Sensors
-void sensorsAutodetect(void);
 void ACC_getADC(void);
 int Baro_update(void);
 void Gyro_getADC(void);
 void Mag_init(void);
 int Mag_getADC(void);
-void Sonar_init(void);
-void Sonar_update(void);
 
 // Output
-void mixerInit(void);
 void mixerResetMotors(void);
 void mixerLoadMix(int index);
 void writeServos(void);
diff --git a/src/rc_controls.c b/src/rc_controls.c
new file mode 100644
index 000000000..813581877
--- /dev/null
+++ b/src/rc_controls.c
@@ -0,0 +1,15 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "common/maths.h"
+
+#include "rc_controls.h"
+
+int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
+
+bool areSticksInApModePosition(uint16_t ap_mode)
+{
+    return abs(rcCommand[ROLL]) < ap_mode && abs(rcCommand[PITCH]) < ap_mode;
+}
diff --git a/src/rc_controls.h b/src/rc_controls.h
new file mode 100644
index 000000000..736c0ef7a
--- /dev/null
+++ b/src/rc_controls.h
@@ -0,0 +1,38 @@
+#pragma once
+
+typedef enum rc_alias {
+    ROLL = 0,
+    PITCH,
+    YAW,
+    THROTTLE,
+    AUX1,
+    AUX2,
+    AUX3,
+    AUX4
+} rc_alias_e;
+
+#define ROL_LO (1 << (2 * ROLL))
+#define ROL_CE (3 << (2 * ROLL))
+#define ROL_HI (2 << (2 * ROLL))
+#define PIT_LO (1 << (2 * PITCH))
+#define PIT_CE (3 << (2 * PITCH))
+#define PIT_HI (2 << (2 * PITCH))
+#define YAW_LO (1 << (2 * YAW))
+#define YAW_CE (3 << (2 * YAW))
+#define YAW_HI (2 << (2 * YAW))
+#define THR_LO (1 << (2 * THROTTLE))
+#define THR_CE (3 << (2 * THROTTLE))
+#define THR_HI (2 << (2 * THROTTLE))
+
+typedef struct controlRateConfig_s {
+    uint8_t rcRate8;
+    uint8_t rcExpo8;
+    uint8_t thrMid8;
+    uint8_t thrExpo8;
+    uint8_t rollPitchRate;
+    uint8_t yawRate;
+} controlRateConfig_t;
+
+extern int16_t rcCommand[4];
+
+bool areSticksInApModePosition(uint16_t ap_mode);
diff --git a/src/runtime_config.h b/src/runtime_config.h
index afdceecb5..17dd99701 100644
--- a/src/runtime_config.h
+++ b/src/runtime_config.h
@@ -47,8 +47,6 @@ typedef struct flags_t {
     uint8_t FIXED_WING;                     // set when in flying_wing or airplane mode. currently used by althold selection code
 } flags_t;
 
-typedef void (* pidControllerFuncPtr)(void);                // pid controller function prototype
-
 extern flags_t f;
 
 bool sensors(uint32_t mask);
diff --git a/src/rx_common.c b/src/rx_common.c
index 8f487271f..3c3a93b1d 100644
--- a/src/rx_common.c
+++ b/src/rx_common.c
@@ -9,6 +9,7 @@
 #include "config.h"
 
 #include "failsafe.h"
+#include "rc_controls.h"
 
 #include "rx_pwm.h"
 #include "rx_sbus.h"
diff --git a/src/rx_common.h b/src/rx_common.h
index db2d57df3..e27f61e2b 100644
--- a/src/rx_common.h
+++ b/src/rx_common.h
@@ -1,29 +1,5 @@
 #pragma once
 
-typedef enum rc_alias {
-    ROLL = 0,
-    PITCH,
-    YAW,
-    THROTTLE,
-    AUX1,
-    AUX2,
-    AUX3,
-    AUX4
-} rc_alias_e;
-
-#define ROL_LO (1 << (2 * ROLL))
-#define ROL_CE (3 << (2 * ROLL))
-#define ROL_HI (2 << (2 * ROLL))
-#define PIT_LO (1 << (2 * PITCH))
-#define PIT_CE (3 << (2 * PITCH))
-#define PIT_HI (2 << (2 * PITCH))
-#define YAW_LO (1 << (2 * YAW))
-#define YAW_CE (3 << (2 * YAW))
-#define YAW_HI (2 << (2 * YAW))
-#define THR_LO (1 << (2 * THROTTLE))
-#define THR_CE (3 << (2 * THROTTLE))
-#define THR_HI (2 << (2 * THROTTLE))
-
 #define PWM_RANGE_ZERO 0 // FIXME should all usages of this be changed to use PWM_RANGE_MIN?
 #define PWM_RANGE_MIN 1000
 #define PWM_RANGE_MAX 2000
@@ -52,15 +28,6 @@ typedef struct rxConfig_s {
     uint16_t maxcheck;                      // maximum rc end
 } rxConfig_t;
 
-typedef struct controlRateConfig_s {
-    uint8_t rcRate8;
-    uint8_t rcExpo8;
-    uint8_t thrMid8;
-    uint8_t thrExpo8;
-    uint8_t rollPitchRate;
-    uint8_t yawRate;
-} controlRateConfig_t;
-
 typedef struct rxRuntimeConfig_s {
     uint8_t channelCount;                  // number of rc channels as reported by current input driver
 } rxRuntimeConfig_t;
diff --git a/src/rx_pwm.c b/src/rx_pwm.c
index 58251725c..c17d711e6 100644
--- a/src/rx_pwm.c
+++ b/src/rx_pwm.c
@@ -6,6 +6,7 @@
 
 #include "platform.h"
 
+#include "rc_controls.h"
 #include "rx_common.h"
 #include "config.h"
 
diff --git a/src/rx_sbus.c b/src/rx_sbus.c
index 618b999ae..f31a689ab 100644
--- a/src/rx_sbus.c
+++ b/src/rx_sbus.c
@@ -11,6 +11,7 @@
 
 
 #include "failsafe.h"
+#include "rc_controls.h"
 
 #include "rx_common.h"
 #include "rx_sbus.h"
diff --git a/src/rx_spektrum.c b/src/rx_spektrum.c
index d710a8676..504d044a5 100644
--- a/src/rx_spektrum.c
+++ b/src/rx_spektrum.c
@@ -10,6 +10,7 @@
 #include "serial_common.h"
 
 #include "failsafe.h"
+#include "rc_controls.h"
 
 #include "rx_common.h"
 #include "rx_spektrum.h"
diff --git a/src/rx_sumd.c b/src/rx_sumd.c
index 2be2e42e7..6452b01c0 100644
--- a/src/rx_sumd.c
+++ b/src/rx_sumd.c
@@ -10,6 +10,7 @@
 #include "serial_common.h"
 
 #include "failsafe.h"
+#include "rc_controls.h"
 
 #include "rx_common.h"
 #include "rx_sumd.h"
diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c
index 84bbd3e7d..bacea401b 100644
--- a/src/sensors_acceleration.c
+++ b/src/sensors_acceleration.c
@@ -18,6 +18,11 @@ extern bool AccInflightCalibrationMeasurementDone;
 extern bool AccInflightCalibrationSavetoEEProm;
 extern bool AccInflightCalibrationActive;
 
+void ACC_SetCalibrationCycles(uint16_t calibrationCyclesRequired)
+{
+    calibratingA = calibrationCyclesRequired;
+}
+
 void ACC_Common(void)
 {
     static int32_t a[3];
diff --git a/src/sensors_acceleration.h b/src/sensors_acceleration.h
index 609e59efc..0663cc59c 100644
--- a/src/sensors_acceleration.h
+++ b/src/sensors_acceleration.h
@@ -13,7 +13,9 @@ typedef enum AccelSensors {
 extern uint8_t accHardware;
 extern sensor_align_e accAlign;
 extern acc_t acc;
+extern uint16_t calibratingA;
 
+void ACC_SetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void ACC_Common(void);
 void ACC_getADC(void);
 
diff --git a/src/sensors_barometer.c b/src/sensors_barometer.c
index 9002f3f42..7ea33ecf6 100644
--- a/src/sensors_barometer.c
+++ b/src/sensors_barometer.c
@@ -6,9 +6,16 @@
 baro_t baro;                        // barometer access functions
 
 #ifdef BARO
+uint16_t calibratingB = 0;      // baro calibration = get new ground pressure value
+
 static int32_t baroGroundAltitude = 0;
 static int32_t baroGroundPressure = 0;
 
+void Baro_SetCalibrationCycles(uint16_t calibrationCyclesRequired)
+{
+    calibratingB = calibrationCyclesRequired;
+}
+
 void Baro_Common(void)
 {
     static int32_t baroHistTab[BARO_TAB_SIZE_MAX];
diff --git a/src/sensors_barometer.h b/src/sensors_barometer.h
index c00756099..9cb4ed68a 100644
--- a/src/sensors_barometer.h
+++ b/src/sensors_barometer.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #ifdef BARO
+void Baro_SetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void Baro_Common(void);
 int Baro_update(void);
 int32_t Baro_calculateAltitude(void);
diff --git a/src/sensors_common.h b/src/sensors_common.h
index 073bbd6c2..64287cf17 100644
--- a/src/sensors_common.h
+++ b/src/sensors_common.h
@@ -1,5 +1,9 @@
 #pragma once
 
+#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
+
 typedef enum {
     SENSOR_GYRO = 1 << 0, // always present
     SENSOR_ACC = 1 << 1,
diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c
index 72ca46451..c866fadf5 100644
--- a/src/sensors_gyro.c
+++ b/src/sensors_gyro.c
@@ -11,6 +11,11 @@ uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
+void GYRO_SetCalibrationCycles(uint16_t calibrationCyclesRequired)
+{
+    calibratingG = calibrationCyclesRequired;
+}
+
 void GYRO_Common(void)
 {
     int axis;
diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h
index 925389749..f092492b1 100644
--- a/src/sensors_gyro.h
+++ b/src/sensors_gyro.h
@@ -4,6 +4,7 @@ extern uint16_t acc_1G;
 extern gyro_t gyro;
 extern sensor_align_e gyroAlign;
 
+void GYRO_SetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void GYRO_Common(void);
 void Gyro_getADC(void);
 
diff --git a/src/sensors_sonar.h b/src/sensors_sonar.h
new file mode 100644
index 000000000..20076cb2e
--- /dev/null
+++ b/src/sensors_sonar.h
@@ -0,0 +1,4 @@
+#pragma once
+
+void Sonar_init(void);
+void Sonar_update(void);