diff --git a/src/main/config/config.c b/src/main/config/config.c
index 1ae2872c40..7124f4f224 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -26,9 +26,9 @@
 #include "sensors/battery.h"
 #include "io/gimbal.h"
 #include "io/escservo.h"
+#include "rx/rx.h"
 #include "io/rc_controls.h"
 #include "io/rc_curves.h"
-#include "rx/rx.h"
 #include "io/gps.h"
 #include "flight/failsafe.h"
 
@@ -538,6 +538,14 @@ void saveAndReloadCurrentProfileToCurrentProfileSlot(void)
     readEEPROMAndNotify();
 }
 
+void changeProfile(uint8_t profileIndex)
+{
+    masterConfig.current_profile_index = profileIndex;
+    writeEEPROM();
+    readEEPROM();
+    blinkLedAndSoundBeeper(2, 40, profileIndex);
+}
+
 bool feature(uint32_t mask)
 {
     return masterConfig.enabledFeatures & mask;
diff --git a/src/main/config/config.h b/src/main/config/config.h
index 61d33ad1bf..556292d44f 100644
--- a/src/main/config/config.h
+++ b/src/main/config/config.h
@@ -34,6 +34,7 @@ void readEEPROMAndNotify(void);
 void writeEEPROM();
 void ensureEEPROMContainsValidData(void);
 void saveAndReloadCurrentProfileToCurrentProfileSlot(void);
+void changeProfile(uint8_t profileIndex);
 
 bool canSoftwareSerialBeUsed(void);
 
diff --git a/src/main/drivers/light_ledring.c b/src/main/drivers/light_ledring.c
index b325584501..e9512c48ea 100644
--- a/src/main/drivers/light_ledring.c
+++ b/src/main/drivers/light_ledring.c
@@ -10,10 +10,10 @@
 #include "bus_i2c.h"
 
 // FIXME there should be no dependencies on the main source code
-#include "io/escservo.h"
-#include "io/rc_controls.h"
-#include "sensors/sensors.h"
-#include "flight/flight.h"
+//#include "io/escservo.h"
+//#include "io/rc_controls.h"
+//#include "sensors/sensors.h"
+//#include "flight/flight.h"
 
 #include "light_ledring.h"
 
@@ -32,7 +32,7 @@ bool ledringDetect(void)
 }
 
 // pitch/roll are absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
-void ledringState(bool armed, int16_t pitch, int16_t roll)
+void ledringState(bool armed, int16_t pitch, int16_t roll, int16_t heading)
 {
     uint8_t b[10];
     static uint8_t state;
diff --git a/src/main/drivers/light_ledring.h b/src/main/drivers/light_ledring.h
index bf247fe8b4..b7c123a9a6 100644
--- a/src/main/drivers/light_ledring.h
+++ b/src/main/drivers/light_ledring.h
@@ -1,5 +1,5 @@
 #pragma once
 
 bool ledringDetect(void);
-void ledringState(bool armed, int16_t pitch, int16_t roll);
+void ledringState(bool armed, int16_t pitch, int16_t roll, int16_t heading);
 void ledringBlink(void);
diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c
index 3b5f0dd60b..b9a3f0273d 100644
--- a/src/main/flight/failsafe.c
+++ b/src/main/flight/failsafe.c
@@ -3,9 +3,9 @@
 
 #include "common/axis.h"
 
+#include "rx/rx.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
-#include "rx/rx.h"
 #include "config/runtime_config.h"
 
 #include "flight/failsafe.h"
diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c
index 39e7e305cb..21ec119c40 100644
--- a/src/main/flight/flight.c
+++ b/src/main/flight/flight.c
@@ -7,6 +7,7 @@
 
 #include "config/runtime_config.h"
 
+#include "rx/rx.h"
 #include "drivers/accgyro.h"
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 13fc0e40a5..5194b39fa4 100755
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -28,9 +28,9 @@
 // FIXME remove dependency on config.h
 #include "sensors/boardalignment.h"
 #include "sensors/battery.h"
+#include "rx/rx.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
-#include "rx/rx.h"
 #include "drivers/serial.h"
 #include "io/serial.h"
 #include "telemetry/telemetry.h"
diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c
index 5c9346e649..6b665632ef 100755
--- a/src/main/flight/mixer.c
+++ b/src/main/flight/mixer.c
@@ -11,10 +11,10 @@
 #include "drivers/timer.h"
 #include "drivers/pwm_output.h"
 
+#include "rx/rx.h"
 #include "io/gimbal.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
-#include "rx/rx.h"
 
 #include "flight/mixer.h"
 #include "flight/flight.h"
diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c
index 6041aab67e..5fd471850a 100644
--- a/src/main/io/rc_controls.c
+++ b/src/main/io/rc_controls.c
@@ -1,15 +1,179 @@
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include <math.h>
 
+#include "common/axis.h"
 #include "common/maths.h"
 
+#include "config/config.h"
+#include "config/runtime_config.h"
+
+#include "flight/flight.h"
+
+#include "drivers/accgyro.h"
+
+#include "sensors/sensors.h"
+#include "sensors/gyro.h"
+#include "sensors/acceleration.h"
+
+#include "io/gps.h"
+#include "mw.h"
+
+#include "rx/rx.h"
 #include "io/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;
 }
+
+throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
+{
+    if (feature(FEATURE_3D) && (rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle)))
+        return THROTTLE_LOW;
+    else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->mincheck))
+        return THROTTLE_LOW;
+
+    return THROTTLE_HIGH;
+}
+
+void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, uint16_t *activate, bool retarded_arm)
+{
+    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
+    static uint8_t rcSticks;            // this hold sticks position for command combos
+    uint8_t stTmp = 0;
+    int i;
+
+    // ------------------ STICKS COMMAND HANDLER --------------------
+    // checking sticks positions
+    for (i = 0; i < 4; i++) {
+        stTmp >>= 2;
+        if (rcData[i] > rxConfig->mincheck)
+            stTmp |= 0x80;  // check for MIN
+        if (rcData[i] < rxConfig->maxcheck)
+            stTmp |= 0x40;  // check for MAX
+    }
+    if (stTmp == rcSticks) {
+        if (rcDelayCommand < 250)
+            rcDelayCommand++;
+    } else
+        rcDelayCommand = 0;
+    rcSticks = stTmp;
+
+    // perform actions
+    if (throttleStatus == THROTTLE_LOW) {
+        if (activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
+            if (rcOptions[BOXARM] && f.OK_TO_ARM)
+                mwArm();
+            else if (f.ARMED)
+                mwDisarm();
+        }
+    }
+
+    if (rcDelayCommand != 20) {
+        return;
+    }
+
+    if (f.ARMED) {      // actions during armed
+        // Disarm on throttle down + yaw
+        if (activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
+            mwDisarm();
+        // Disarm on roll (only when retarded_arm is enabled)
+        if (retarded_arm && activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
+            mwDisarm();
+
+        return;
+    }
+
+    // actions during not armed
+    i = 0;
+
+    if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
+        // GYRO calibration
+        gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+        if (feature(FEATURE_GPS))
+            GPS_reset_home_position();
+#ifdef BARO
+        if (sensors(SENSOR_BARO))
+            baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
+#endif
+        if (!sensors(SENSOR_MAG))
+            heading = 0; // reset heading to zero after gyro calibration
+
+        return;
+    }
+
+    if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
+        // Inflight ACC Calibration
+        handleInflightCalibrationStickPosition();
+        return;
+    }
+
+    // Multiple configuration profiles
+    if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
+        i = 1;
+    else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
+        i = 2;
+    else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
+        i = 3;
+    if (i) {
+        changeProfile(i - 1);
+        return;
+    }
+
+    if (activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) {
+        // Arm via YAW
+        mwArm();
+        return;
+    }
+
+    if (retarded_arm && activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) {
+        // Arm via ROLL
+        mwArm();
+        return;
+    }
+
+    if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
+        // Calibrating Acc
+        accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
+        return;
+    }
+
+
+    if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
+        // Calibrating Mag
+        f.CALIBRATE_MAG = 1;
+        return;
+    }
+
+
+    // Accelerometer Trim
+
+    rollAndPitchTrims_t accelerometerTrimsDelta;
+    memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
+
+    bool shouldApplyRollAndPitchTrimDelta = false;
+    if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
+        accelerometerTrimsDelta.values.pitch = 2;
+        shouldApplyRollAndPitchTrimDelta = true;
+    } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
+        accelerometerTrimsDelta.values.pitch = -2;
+        shouldApplyRollAndPitchTrimDelta = true;
+    } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
+        accelerometerTrimsDelta.values.roll = 2;
+        shouldApplyRollAndPitchTrimDelta = true;
+    } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
+        accelerometerTrimsDelta.values.roll = -2;
+        shouldApplyRollAndPitchTrimDelta = true;
+    }
+    if (shouldApplyRollAndPitchTrimDelta) {
+        applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
+        rcDelayCommand = 0; // allow autorepetition
+        return;
+    }
+}
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index a062e185c9..ea1d1652a8 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -11,6 +11,11 @@ typedef enum rc_alias {
     AUX4
 } rc_alias_e;
 
+typedef enum {
+    THROTTLE_LOW = 0,
+    THROTTLE_HIGH
+} throttleStatus_e;
+
 #define ROL_LO (1 << (2 * ROLL))
 #define ROL_CE (3 << (2 * ROLL))
 #define ROL_HI (2 << (2 * ROLL))
@@ -36,4 +41,7 @@ typedef struct controlRateConfig_s {
 extern int16_t rcCommand[4];
 
 bool areSticksInApModePosition(uint16_t ap_mode);
+throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
+void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, uint16_t *activate, bool retarded_arm);
+
 
diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c
index 4a44beef48..44dd9d4d62 100644
--- a/src/main/io/rc_curves.c
+++ b/src/main/io/rc_curves.c
@@ -1,6 +1,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include "rx/rx.h"
 #include "io/rc_controls.h"
 #include "io/escservo.h"
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index ac969c9248..44701e2c61 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -15,22 +15,22 @@
 #include "drivers/accgyro.h"
 #include "drivers/serial.h"
 #include "drivers/bus_i2c.h"
-#include "io/serial.h"
 #include "flight/flight.h"
 #include "flight/mixer.h"
-#include "io/escservo.h"
-#include "io/rc_controls.h"
-#include "sensors/boardalignment.h"
-#include "telemetry/telemetry.h"
-#include "io/gps.h"
+#include "flight/failsafe.h"
 #include "rx/rx.h"
-#include "sensors/battery.h"
+#include "io/escservo.h"
+#include "io/gps.h"
 #include "io/gimbal.h"
+#include "io/rc_controls.h"
+#include "io/serial.h"
+#include "sensors/battery.h"
+#include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
 #include "sensors/gyro.h"
 #include "sensors/barometer.h"
-#include "flight/failsafe.h"
+#include "telemetry/telemetry.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 2143b51b25..130605a557 100755
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -15,24 +15,24 @@
 #include "drivers/serial.h"
 #include "drivers/bus_i2c.h"
 
-#include "io/serial.h"
 #include "flight/flight.h"
 #include "flight/mixer.h"
-#include "io/escservo.h"
-#include "io/rc_controls.h"
-#include "sensors/boardalignment.h"
-#include "io/gps.h"
+#include "flight/failsafe.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 "telemetry/telemetry.h"
+#include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
-#include "flight/failsafe.h"
 
 #include "config/runtime_config.h"
 #include "config/config.h"
diff --git a/src/main/main.c b/src/main/main.c
index ed1a512129..503117a245 100755
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -24,10 +24,10 @@
 #include "io/serial.h"
 #include "flight/failsafe.h"
 
+#include "rx/rx.h"
 #include "io/gps.h"
 #include "io/escservo.h"
 #include "io/rc_controls.h"
-#include "rx/rx.h"
 #include "io/gimbal.h"
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
diff --git a/src/main/mw.c b/src/main/mw.c
index ebe6145a41..142ae901aa 100755
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -35,9 +35,9 @@
 #include "io/serial_cli.h"
 #include "io/serial.h"
 #include "io/statusindicator.h"
+#include "rx/rx.h"
 #include "io/rc_controls.h"
 #include "io/rc_curves.h"
-#include "rx/rx.h"
 #include "rx/msp.h"
 #include "telemetry/telemetry.h"
 
@@ -72,12 +72,13 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig
 
 extern pidControllerFuncPtr pid_controller;
 
-// Automatic ACC Offset Calibration
-bool AccInflightCalibrationArmed = false;
-bool AccInflightCalibrationMeasurementDone = false;
-bool AccInflightCalibrationSavetoEEProm = false;
-bool AccInflightCalibrationActive = false;
-uint16_t InflightcalibratingA = 0;
+void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
+{
+    currentProfile.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
+    currentProfile.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
+
+    saveAndReloadCurrentProfileToCurrentProfileSlot();
+}
 
 void updateAutotuneState(void)
 {
@@ -247,7 +248,7 @@ void annexCode(void)
         static uint32_t LEDTime;
         if ((int32_t)(currentTime - LEDTime) >= 0) {
             LEDTime = currentTime + 50000;
-            ledringState(f.ARMED, inclination.values.pitchDeciDegrees, inclination.values.rollDeciDegrees);
+            ledringState(f.ARMED, inclination.values.pitchDeciDegrees, inclination.values.rollDeciDegrees, heading);
         }
     }
 #endif
@@ -262,9 +263,6 @@ void annexCode(void)
     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
     if (gyro.temperature)
         gyro.temperature(&telemTemperature1);
-    else {
-        // TODO MCU temp
-    }
 }
 
 void mwDisarm(void)
@@ -291,18 +289,53 @@ void mwArm(void)
     }
 }
 
+// Automatic ACC Offset Calibration
+bool AccInflightCalibrationArmed = false;
+bool AccInflightCalibrationMeasurementDone = false;
+bool AccInflightCalibrationSavetoEEProm = false;
+bool AccInflightCalibrationActive = false;
+uint16_t InflightcalibratingA = 0;
+
+void handleInflightCalibrationStickPosition(void)
+{
+    if (AccInflightCalibrationMeasurementDone) {
+        // trigger saving into eeprom after landing
+        AccInflightCalibrationMeasurementDone = false;
+        AccInflightCalibrationSavetoEEProm = true;
+    } else {
+        AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
+        if (AccInflightCalibrationArmed) {
+            queueConfirmationBeep(2);
+        } else {
+            queueConfirmationBeep(3);
+        }
+    }
+}
+
+void updateInflightCalibrationState(void)
+{
+    if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) {   // Copter is airborne and you are turning it off via boxarm : start measurement
+        InflightcalibratingA = 50;
+        AccInflightCalibrationArmed = false;
+    }
+    if (rcOptions[BOXCALIB]) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
+        if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
+            InflightcalibratingA = 50;
+        AccInflightCalibrationActive = true;
+    } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
+        AccInflightCalibrationMeasurementDone = false;
+        AccInflightCalibrationSavetoEEProm = true;
+    }
+}
+
 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
-    static uint8_t rcSticks;            // this hold sticks position for command combos
-    uint8_t stTmp = 0;
     int i;
 #ifdef BARO
     static int16_t initialThrottleHold;
 #endif
     static uint32_t loopTime;
     uint16_t auxState = 0;
-    bool isThrottleLow = false;
 
     updateRx();
 
@@ -326,138 +359,17 @@ void loop(void)
             failsafe->vTable->updateState();
         }
 
-        // ------------------ STICKS COMMAND HANDLER --------------------
-        // checking sticks positions
-        for (i = 0; i < 4; i++) {
-            stTmp >>= 2;
-            if (rcData[i] > masterConfig.rxConfig.mincheck)
-                stTmp |= 0x80;  // check for MIN
-            if (rcData[i] < masterConfig.rxConfig.maxcheck)
-                stTmp |= 0x40;  // check for MAX
-        }
-        if (stTmp == rcSticks) {
-            if (rcDelayCommand < 250)
-                rcDelayCommand++;
-        } else
-            rcDelayCommand = 0;
-        rcSticks = stTmp;
+        throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 
-        // perform actions
-        if (feature(FEATURE_3D) && (rcData[THROTTLE] > (masterConfig.rxConfig.midrc - masterConfig.flight3DConfig.deadband3d_throttle) && rcData[THROTTLE] < (masterConfig.rxConfig.midrc + masterConfig.flight3DConfig.deadband3d_throttle)))
-            isThrottleLow = true;
-        else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < masterConfig.rxConfig.mincheck))
-            isThrottleLow = true;
-        if (isThrottleLow) {
+        if (throttleStatus == THROTTLE_LOW) {
             resetErrorAngle();
             resetErrorGyro();
-            if (currentProfile.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
-                if (rcOptions[BOXARM] && f.OK_TO_ARM)
-                    mwArm();
-                else if (f.ARMED)
-                    mwDisarm();
-            }
         }
 
-        if (rcDelayCommand == 20) {
-            if (f.ARMED) {      // actions during armed
-                // Disarm on throttle down + yaw
-                if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
-                    mwDisarm();
-                // Disarm on roll (only when retarded_arm is enabled)
-                if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
-                    mwDisarm();
-            } else {            // actions during not armed
-                i = 0;
-                // GYRO calibration
-                if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
-                    gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
-                    if (feature(FEATURE_GPS))
-                        GPS_reset_home_position();
-#ifdef BARO
-                    if (sensors(SENSOR_BARO))
-                        baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
-#endif
-                    if (!sensors(SENSOR_MAG))
-                        heading = 0; // reset heading to zero after gyro calibration
-                // Inflight ACC Calibration
-                } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
-                    if (AccInflightCalibrationMeasurementDone) {        // trigger saving into eeprom after landing
-                        AccInflightCalibrationMeasurementDone = false;
-                        AccInflightCalibrationSavetoEEProm = true;
-                    } else {
-                        AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
-                        if (AccInflightCalibrationArmed) {
-                            queueConfirmationBeep(2);
-                        } else {
-                            queueConfirmationBeep(3);
-                        }
-                    }
-                }
-
-                // Multiple configuration profiles
-                if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
-                    i = 1;
-                else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
-                    i = 2;
-                else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
-                    i = 3;
-                if (i) {
-                    masterConfig.current_profile_index = i - 1;
-                    writeEEPROM();
-                    readEEPROM();
-                    blinkLedAndSoundBeeper(2, 40, i);
-                    // TODO alarmArray[0] = i;
-                }
-
-                // Arm via YAW
-                if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
-                    mwArm();
-                // Arm via ROLL
-                else if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
-                    mwArm();
-                // Calibrating Acc
-                else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
-                    accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
-                // Calibrating Mag
-                else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
-                    f.CALIBRATE_MAG = 1;
-                i = 0;
-                // Acc Trim
-                if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
-                    currentProfile.accelerometerTrims.values.pitch += 2;
-                    i = 1;
-                } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
-                    currentProfile.accelerometerTrims.values.pitch -= 2;
-                    i = 1;
-                } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
-                    currentProfile.accelerometerTrims.values.roll += 2;
-                    i = 1;
-                } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
-                    currentProfile.accelerometerTrims.values.roll -= 2;
-                    i = 1;
-                }
-                if (i) {
-                    copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
-                    writeEEPROM();
-                    readEEPROMAndNotify();
-                    rcDelayCommand = 0; // allow autorepetition
-                }
-            }
-        }
+        processRcStickPositions(&masterConfig.rxConfig, throttleStatus, currentProfile.activate, masterConfig.retarded_arm);
 
         if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
-            if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) {   // Copter is airborne and you are turning it off via boxarm : start measurement
-                InflightcalibratingA = 50;
-                AccInflightCalibrationArmed = false;
-            }
-            if (rcOptions[BOXCALIB]) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
-                if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
-                    InflightcalibratingA = 50;
-                AccInflightCalibrationActive = true;
-            } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
-                AccInflightCalibrationMeasurementDone = false;
-                AccInflightCalibrationSavetoEEProm = true;
-            }
+            updateInflightCalibrationState();
         }
 
         // Check AUX switches
diff --git a/src/main/mw.h b/src/main/mw.h
new file mode 100644
index 0000000000..245d59533f
--- /dev/null
+++ b/src/main/mw.h
@@ -0,0 +1,7 @@
+#pragma once
+
+void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta);
+void handleInflightCalibrationStickPosition();
+
+void mwDisarm(void);
+void mwArm(void);