diff --git a/src/buzzer.c b/src/buzzer.c
index d9eaa4ff56..81db27369d 100644
--- a/src/buzzer.c
+++ b/src/buzzer.c
@@ -11,14 +11,15 @@
 
 #include "buzzer.h"
 
-#define LONG_PAUSE_DELAY 50
+#define LONG_PAUSE_DURATION_MILLIS 50
+#define SHORT_CONFIRMATION_BEEP_DURATION_MILLIS 50
 
 static uint8_t buzzerIsOn = 0, beepDone = 0;
 static uint32_t buzzerLastToggleTime;
-static void beep(uint16_t pulse);
+static void beep(uint16_t pulseMillis);
 static void beep_code(char first, char second, char third, char pause);
 
-uint8_t toggleBeep = 0;
+static uint8_t toggleBeep = 0;
 
 typedef enum {
     FAILSAFE_IDLE = 0,
@@ -97,6 +98,11 @@ void buzzer(bool warn_vbat)
     }
 }
 
+// duration is specified in multiples of SHORT_CONFIRMATION_BEEP_DURATION_MILLIS
+void queueConfirmationBeep(uint8_t duration) {
+    toggleBeep = duration;
+}
+
 void beep_code(char first, char second, char third, char pause)
 {
     char patternChar[4];
@@ -121,7 +127,7 @@ void beep_code(char first, char second, char third, char pause)
             Duration = 0;
             break;
         default:
-            Duration = LONG_PAUSE_DELAY;
+            Duration = LONG_PAUSE_DURATION_MILLIS;
             break;
     }
 
@@ -140,18 +146,23 @@ void beep_code(char first, char second, char third, char pause)
     }
 }
 
-static void beep(uint16_t pulse)
+static void beep(uint16_t pulseMillis)
 {
-    if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + LONG_PAUSE_DELAY))) {         // Buzzer is off and long pause time is up -> turn it on
+    if (buzzerIsOn) {
+        if (millis() >= buzzerLastToggleTime + pulseMillis) {
+            buzzerIsOn = 0;
+            BEEP_OFF;
+            buzzerLastToggleTime = millis();
+            if (toggleBeep >0)
+                toggleBeep--;
+            beepDone = 1;
+        }
+        return;
+    }
+
+    if (millis() >= (buzzerLastToggleTime + LONG_PAUSE_DURATION_MILLIS)) {         // Buzzer is off and long pause time is up -> turn it on
         buzzerIsOn = 1;
         BEEP_ON;
         buzzerLastToggleTime = millis();      // save the time the buzer turned on
-    } else if (buzzerIsOn && (millis() >= buzzerLastToggleTime + pulse)) {         // Buzzer is on and time is up -> turn it off
-        buzzerIsOn = 0;
-        BEEP_OFF;
-        buzzerLastToggleTime = millis();
-        if (toggleBeep >0)
-            toggleBeep--;
-        beepDone = 1;
     }
 }
diff --git a/src/buzzer.h b/src/buzzer.h
index 6218b56fbe..8b9d4bb48f 100644
--- a/src/buzzer.h
+++ b/src/buzzer.h
@@ -1,3 +1,4 @@
 #pragma once
 
 void buzzer(bool warn_vbat);
+void queueConfirmationBeep(uint8_t duration);
diff --git a/src/config.c b/src/config.c
index ff9e157b1b..61dd3c5bf3 100755
--- a/src/config.c
+++ b/src/config.c
@@ -90,6 +90,8 @@ void activateConfig(void)
     gpsUseProfile(&currentProfile.gpsProfile);
     gpsUsePIDs(&currentProfile.pidProfile);
     useFailsafeConfig(&currentProfile.failsafeConfig);
+
+    setAccelerationTrims(&masterConfig.accZero);
 }
 
 void readEEPROM(void)
@@ -101,9 +103,9 @@ void readEEPROM(void)
     // Read flash
     memcpy(&masterConfig, (char *)FLASH_WRITE_ADDR, sizeof(master_t));
     // Copy current profile
-    if (masterConfig.current_profile > 2) // sanity check
-        masterConfig.current_profile = 0;
-    memcpy(&currentProfile, &masterConfig.profile[masterConfig.current_profile], sizeof(profile_t)); 
+    if (masterConfig.current_profile_index > 2) // sanity check
+        masterConfig.current_profile_index = 0;
+    memcpy(&currentProfile, &masterConfig.profile[masterConfig.current_profile_index], sizeof(profile_t)); 
 
     activateConfig();
 }
@@ -171,6 +173,13 @@ void resetEEPROM(void)
     writeEEPROM();
 }
 
+static void resetAccelerometerTrims(int16_flightDynamicsTrims_t *accelerometerTrims)
+{
+    accelerometerTrims->trims.pitch = 0;
+    accelerometerTrims->trims.roll = 0;
+    accelerometerTrims->trims.yaw = 0;
+}
+
 static void resetPidProfile(pidProfile_t *pidProfile)
 {
     pidProfile->P8[ROLL] = 40;
@@ -230,13 +239,11 @@ static void resetConf(void)
     featureSet(FEATURE_VBAT);
 
     // global settings
-    masterConfig.current_profile = 0;       // default profile
+    masterConfig.current_profile_index = 0;       // default profile
     masterConfig.gyro_cmpf_factor = 600;    // default MWC
     masterConfig.gyro_cmpfm_factor = 250;   // default MWC
     masterConfig.gyro_lpf = 42;             // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
-    masterConfig.accZero[0] = 0;
-    masterConfig.accZero[1] = 0;
-    masterConfig.accZero[2] = 0;
+    resetAccelerometerTrims(&masterConfig.accZero);
     masterConfig.gyro_align = ALIGN_DEFAULT;
     masterConfig.acc_align = ALIGN_DEFAULT;
     masterConfig.mag_align = ALIGN_DEFAULT;
@@ -299,8 +306,7 @@ static void resetConf(void)
     currentProfile.controlRateConfig.thrExpo8 = 0;
     // for (i = 0; i < CHECKBOXITEMS; i++)
     //     cfg.activate[i] = 0;
-    currentProfile.angleTrim[0] = 0;
-    currentProfile.angleTrim[1] = 0;
+    resetRollAndPitchTrims(&currentProfile.accelerometerTrims);
     currentProfile.mag_declination = 0;    // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
     currentProfile.acc_lpf_factor = 4;
     currentProfile.accz_deadband = 40;
@@ -352,6 +358,12 @@ static void resetConf(void)
         memcpy(&masterConfig.profile[i], &currentProfile, sizeof(profile_t));
 }
 
+void saveAndReloadCurrentProfileToCurrentProfileSlot(void)
+{
+    copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
+    writeEEPROM();
+    readEEPROMAndNotify();
+}
 
 bool feature(uint32_t mask)
 {
diff --git a/src/config.h b/src/config.h
index b8680710eb..b3267562bf 100644
--- a/src/config.h
+++ b/src/config.h
@@ -31,3 +31,4 @@ void readEEPROM(void);
 void readEEPROMAndNotify(void);
 void writeEEPROM();
 void ensureEEPROMContainsValidData(void);
+void saveAndReloadCurrentProfileToCurrentProfileSlot(void);
diff --git a/src/config_master.h b/src/config_master.h
index 2ce2aed513..664ac229ca 100644
--- a/src/config_master.h
+++ b/src/config_master.h
@@ -36,7 +36,7 @@ typedef struct master_t {
     uint16_t gyro_cmpfm_factor;             // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter
     uint8_t moron_threshold;                // 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.
     uint16_t max_angle_inclination;         // max inclination allowed in angle (level) mode. default 500 (50 degrees).
-    int16_t accZero[3];
+    int16_flightDynamicsTrims_t accZero;
     int16_t magZero[3];
 
     batteryConfig_t batteryConfig;
@@ -59,8 +59,8 @@ typedef struct master_t {
     uint8_t telemetry_provider;             // See TelemetryProvider enum.
     uint8_t telemetry_port;                 // See TelemetryPort enum.
     uint8_t telemetry_switch;               // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
-    profile_t profile[3];                    // 3 separate profiles
-    uint8_t current_profile;                // currently loaded profile
+    profile_t profile[3];                   // 3 separate profiles
+    uint8_t current_profile_index;          // currently loaded profile
 
     uint8_t magic_ef;                       // magic number, should be 0xEF
     uint8_t chk;                            // XOR checksum
diff --git a/src/config_profile.h b/src/config_profile.h
index 9a3333a33e..b7dc748b07 100644
--- a/src/config_profile.h
+++ b/src/config_profile.h
@@ -8,9 +8,9 @@ typedef struct profile_s {
     controlRateConfig_t controlRateConfig;
 
     uint8_t dynThrPID;
-    uint16_t tpaBreakPoint;                  // Breakpoint where TPA is activated
+    uint16_t tpaBreakPoint;                 // Breakpoint where TPA is activated
     int16_t mag_declination;                // Get your magnetic decliniation from here : http://magnetic-declination.com/
-    int16_t angleTrim[ANGLE_INDEX_COUNT];   // accelerometer trim
+    rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
 
     // sensor-related stuff
     uint8_t acc_lpf_factor;                 // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
diff --git a/src/flight_common.c b/src/flight_common.c
index aea504e86c..e9f2453e51 100644
--- a/src/flight_common.c
+++ b/src/flight_common.c
@@ -21,9 +21,9 @@ 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);
+static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);
 
-typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim);                // pid controller function prototype
+typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim);                // pid controller function prototype
 
 pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
 
@@ -34,6 +34,12 @@ void mwDisarm(void)
         f.ARMED = 0;
 }
 
+void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    rollAndPitchTrims->trims.roll = 0;
+    rollAndPitchTrims->trims.pitch = 0;
+}
+
 void resetErrorAngle(void)
 {
     errorAngleI[ROLL] = 0;
@@ -47,7 +53,7 @@ void resetErrorGyro(void)
     errorGyroI[YAW] = 0;
 }
 
-static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
+static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
 {
     int axis, prop;
     int32_t error, errorAngle;
@@ -60,16 +66,16 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     // **** 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
+        if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // 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];
+            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[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
+        if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == FD_YAW) { // MODE relying on GYRO or YAW axis
             error = (int32_t)rcCommand[axis] * 10 * 8 / pidProfile->P8[axis];
             error -= gyroData[axis];
 
@@ -80,11 +86,11 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
                 errorGyroI[axis] = 0;
             ITermGYRO = (errorGyroI[axis] / 125 * pidProfile->I8[axis]) >> 6;
         }
-        if (f.HORIZON_MODE && axis < 2) {
+        if (f.HORIZON_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
             PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
             ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
         } else {
-            if (f.ANGLE_MODE && axis < 2) {
+            if (f.ANGLE_MODE && (axis == FD_ROLL || axis == FD_PITCH)) {
                 PTerm = PTermACC;
                 ITerm = ITermACC;
             } else {
@@ -106,7 +112,7 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
 
 #define GYRO_I_MAX 256
 
-static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, int16_t *angleTrim)
+static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim)
 {
     int32_t errorAngle = 0;
     int axis;
@@ -119,11 +125,11 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     // ----------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
+        if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { // 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
+            errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)max_angle_inclination), +max_angle_inclination) - angle[axis] + angleTrim->raw[axis]; // 16 bits is ok here
         }
-        if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
+        if (axis == FD_YAW) { // 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
diff --git a/src/flight_common.h b/src/flight_common.h
index 05da52f789..7ef5d67cf4 100644
--- a/src/flight_common.h
+++ b/src/flight_common.h
@@ -30,16 +30,55 @@ enum {
 
 extern int16_t angle[ANGLE_INDEX_COUNT]; // see angle_index_t
 
+// See http://en.wikipedia.org/wiki/Flight_dynamics
 enum {
-    GI_ROLL = 0,
-    GI_PITCH,
-    GI_YAW
-} gyro_index_t;
+    FD_ROLL = 0,
+    FD_PITCH,
+    FD_YAW
+} flight_dynamics_index_t;
 
-#define GYRO_INDEX_COUNT 3
+#define FLIGHT_DYNAMICS_INDEX_COUNT 3
 
-extern int16_t gyroData[GYRO_INDEX_COUNT]; // see gyro_index_t
-extern int16_t gyroZero[GYRO_INDEX_COUNT]; // see gyro_index_t
+typedef struct fp_vector {
+    float X;
+    float Y;
+    float Z;
+} t_fp_vector_def;
+
+typedef union {
+    float A[3];
+    t_fp_vector_def V;
+} t_fp_vector;
+
+typedef struct fp_angles {
+    float roll;
+    float pitch;
+    float yaw;
+} fp_angles_t;
+
+typedef struct int16_flightDynamicsTrims_s {
+    int16_t roll;
+    int16_t pitch;
+    int16_t yaw;
+} int16_flightDynamicsTrims_def_t;
+
+typedef union {
+    int16_t raw[3];
+    int16_flightDynamicsTrims_def_t trims;
+} int16_flightDynamicsTrims_t;
+
+typedef struct rollAndPitchTrims_s {
+    int16_t roll;
+    int16_t pitch;
+} rollAndPitchTrims_t_def;
+
+typedef union {
+    int16_t raw[2];
+    rollAndPitchTrims_t_def trims;
+} rollAndPitchTrims_t;
+
+extern int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT]; // see gyro_index_t
+extern int16_t gyroZero[FLIGHT_DYNAMICS_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];
@@ -49,6 +88,7 @@ extern int16_t heading, magHold;
 
 void mwDisarm(void);
 void setPIDController(int type);
+void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
 void resetErrorAngle(void);
 void resetErrorGyro(void);
 
diff --git a/src/flight_imu.c b/src/flight_imu.c
index 94d022d912..8862986405 100644
--- a/src/flight_imu.c
+++ b/src/flight_imu.c
@@ -66,8 +66,8 @@ float magneticDeclination = 0.0f;       // calculated at startup from config
 // **************
 // gyro+acc IMU
 // **************
-int16_t gyroData[GYRO_INDEX_COUNT] = { 0, 0, 0 };
-int16_t gyroZero[GYRO_INDEX_COUNT] = { 0, 0, 0 };
+int16_t gyroData[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
+int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 int16_t angle[2] = { 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
@@ -94,7 +94,7 @@ void computeIMU(void)
 
     gyroGetADC();
     if (sensors(SENSOR_ACC)) {
-        accGetADC();
+        updateAccelerationReadings(&currentProfile.accelerometerTrims);
         getEstimatedAttitude();
     } else {
         accADC[X] = 0;
@@ -103,10 +103,10 @@ void computeIMU(void)
     }
 
     if (masterConfig.mixerConfiguration == MULTITYPE_TRI) {
-        gyroData[GI_YAW] = (gyroYawSmooth * 2 + gyroADC[GI_YAW]) / 3;
-        gyroYawSmooth = gyroData[GI_YAW];
-        gyroData[GI_ROLL] = gyroADC[GI_ROLL];
-        gyroData[GI_PITCH] = gyroADC[GI_PITCH];
+        gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
+        gyroYawSmooth = gyroData[FD_YAW];
+        gyroData[FD_ROLL] = gyroADC[FD_ROLL];
+        gyroData[FD_PITCH] = gyroADC[FD_PITCH];
     } else {
         for (axis = 0; axis < 3; axis++)
             gyroData[axis] = gyroADC[axis];
@@ -127,25 +127,9 @@ void computeIMU(void)
 //
 // **************************************************
 
-typedef struct fp_vector {
-    float X;
-    float Y;
-    float Z;
-} t_fp_vector_def;
-
-typedef union {
-    float A[3];
-    t_fp_vector_def V;
-} t_fp_vector;
 
 t_fp_vector EstG;
 
-typedef struct fp_angles {
-    float roll;
-    float pitch;
-    float yaw;
-} fp_angles_t;
-
 // Normalize a vector
 void normalizeV(struct fp_vector *src, struct fp_vector *dest)
 {
@@ -200,9 +184,9 @@ void rotateAnglesV(struct fp_vector *v, fp_angles_t *delta)
 // deprecated - it uses legacy indices for ROLL/PITCH/YAW, see rc_alias_e - use rotateAnglesV instead
 void rotateV(struct fp_vector *v, float *delta) {
     fp_angles_t temp;
-    temp.roll = delta[GI_ROLL];
-    temp.pitch = delta[GI_PITCH];
-    temp.yaw = delta[GI_YAW];
+    temp.roll = delta[FD_ROLL];
+    temp.pitch = delta[FD_PITCH];
+    temp.yaw = delta[FD_YAW];
 
     rotateAnglesV(v, &temp);
 }
diff --git a/src/mw.c b/src/mw.c
index e0d35b440f..19efbdd2b7 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -4,10 +4,12 @@
 #include "common/maths.h"
 #include "common/typeconversion.h"
 
+#include "buzzer.h"
 #include "sensors_sonar.h"
 #include "sensors_gyro.h"
 #include "sensors_compass.h"
 #include "sensors_barometer.h"
+#include "sensors_acceleration.h"
 #include "flight_common.h"
 #include "serial_cli.h"
 #include "telemetry_common.h"
@@ -34,7 +36,7 @@ uint16_t rssi;                  // range: [0;1023]
 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);
+typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
 
 extern pidControllerFuncPtr pid_controller;
 
@@ -305,9 +307,9 @@ void loop(void)
                     } else {
                         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
                         if (AccInflightCalibrationArmed) {
-                            toggleBeep = 2;
+                            queueConfirmationBeep(2);
                         } else {
-                            toggleBeep = 3;
+                            queueConfirmationBeep(3);
                         }
                     }
                 }
@@ -320,7 +322,7 @@ void loop(void)
                 else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
                     i = 3;
                 if (i) {
-                    masterConfig.current_profile = i - 1;
+                    masterConfig.current_profile_index = i - 1;
                     writeEEPROM();
                     readEEPROM();
                     blinkLedAndSoundBeeper(2, 40, i);
@@ -335,27 +337,27 @@ void loop(void)
                     mwArm();
                 // Calibrating Acc
                 else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
-                    calibratingA = CALIBRATING_ACC_CYCLES;
+                    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.angleTrim[PITCH] += 2;
+                    currentProfile.accelerometerTrims.trims.pitch += 2;
                     i = 1;
                 } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
-                    currentProfile.angleTrim[PITCH] -= 2;
+                    currentProfile.accelerometerTrims.trims.pitch -= 2;
                     i = 1;
                 } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
-                    currentProfile.angleTrim[ROLL] += 2;
+                    currentProfile.accelerometerTrims.trims.roll += 2;
                     i = 1;
                 } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
-                    currentProfile.angleTrim[ROLL] -= 2;
+                    currentProfile.accelerometerTrims.trims.roll -= 2;
                     i = 1;
                 }
                 if (i) {
-                    copyCurrentProfileToProfileSlot(masterConfig.current_profile);
+                    copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
                     writeEEPROM();
                     readEEPROMAndNotify();
                     rcDelayCommand = 0; // allow autorepetition
@@ -602,7 +604,7 @@ void loop(void)
         }
 
         // PID - note this is function pointer set by setPIDController()
-        pid_controller(&currentProfile.pidProfile, &currentProfile.controlRateConfig, masterConfig.max_angle_inclination, currentProfile.angleTrim);
+        pid_controller(&currentProfile.pidProfile, &currentProfile.controlRateConfig, masterConfig.max_angle_inclination, &currentProfile.accelerometerTrims);
 
         mixTable();
         writeServos();
diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c
index ad34de76ed..b49f6f6788 100644
--- a/src/sensors_acceleration.c
+++ b/src/sensors_acceleration.c
@@ -1,8 +1,17 @@
-#include "board.h"
+#include <stdbool.h>
+#include <stdint.h>
 
+#include "platform.h"
+
+#include "common/axis.h"
+
+#include "drivers/accgyro_common.h"
 #include "flight_common.h"
-
-#include "mw.h"
+#include "sensors_common.h"
+#include "buzzer.h"
+#include "boardalignment.h"
+#include "runtime_config.h"
+#include "config.h"
 
 #include "sensors_acceleration.h"
 
@@ -18,102 +27,139 @@ extern bool AccInflightCalibrationMeasurementDone;
 extern bool AccInflightCalibrationSavetoEEProm;
 extern bool AccInflightCalibrationActive;
 
+int16_flightDynamicsTrims_t *accelerationTrims;
+
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
     calibratingA = calibrationCyclesRequired;
 }
 
-void accCommon(void)
+bool isAccelerationCalibrationComplete(void)
 {
-    static int32_t a[3];
-    int axis;
-
-    if (calibratingA > 0) {
-        for (axis = 0; axis < 3; axis++) {
-            // Reset a[axis] at start of calibration
-            if (calibratingA == CALIBRATING_ACC_CYCLES)
-                a[axis] = 0;
-            // Sum up CALIBRATING_ACC_CYCLES readings
-            a[axis] += accADC[axis];
-            // Clear global variables for next reading
-            accADC[axis] = 0;
-            masterConfig.accZero[axis] = 0;
-        }
-        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
-        if (calibratingA == 1) {
-            masterConfig.accZero[GI_ROLL] = (a[GI_ROLL] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-            masterConfig.accZero[GI_PITCH] = (a[GI_PITCH] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-            masterConfig.accZero[GI_YAW] = (a[GI_YAW] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc_1G;
-            currentProfile.angleTrim[AI_ROLL] = 0;
-            currentProfile.angleTrim[AI_PITCH] = 0;
-            copyCurrentProfileToProfileSlot(masterConfig.current_profile);
-            writeEEPROM();      // write accZero in EEPROM
-            readEEPROMAndNotify();
-        }
-        calibratingA--;
-    }
-
-    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
-        static int32_t b[3];
-        static int16_t accZero_saved[3] = { 0, 0, 0 };
-        static int16_t angleTrim_saved[2] = { 0, 0 };
-        // Saving old zeropoints before measurement
-        if (InflightcalibratingA == 50) {
-            accZero_saved[GI_ROLL] = masterConfig.accZero[GI_ROLL];
-            accZero_saved[GI_PITCH] = masterConfig.accZero[GI_PITCH];
-            accZero_saved[GI_YAW] = masterConfig.accZero[GI_YAW];
-            angleTrim_saved[AI_ROLL] = currentProfile.angleTrim[AI_ROLL];
-            angleTrim_saved[AI_PITCH] = currentProfile.angleTrim[AI_PITCH];
-        }
-        if (InflightcalibratingA > 0) {
-            for (axis = 0; axis < 3; axis++) {
-                // Reset a[axis] at start of calibration
-                if (InflightcalibratingA == 50)
-                    b[axis] = 0;
-                // Sum up 50 readings
-                b[axis] += accADC[axis];
-                // Clear global variables for next reading
-                accADC[axis] = 0;
-                masterConfig.accZero[axis] = 0;
-            }
-            // all values are measured
-            if (InflightcalibratingA == 1) {
-                AccInflightCalibrationActive = false;
-                AccInflightCalibrationMeasurementDone = true;
-                toggleBeep = 2;      // buzzer for indicatiing the end of calibration
-                // recover saved values to maintain current flight behavior until new values are transferred
-                masterConfig.accZero[GI_ROLL] = accZero_saved[GI_ROLL];
-                masterConfig.accZero[GI_PITCH] = accZero_saved[GI_PITCH];
-                masterConfig.accZero[GI_YAW] = accZero_saved[GI_YAW];
-                currentProfile.angleTrim[AI_ROLL] = angleTrim_saved[AI_ROLL];
-                currentProfile.angleTrim[AI_PITCH] = angleTrim_saved[AI_PITCH];
-            }
-            InflightcalibratingA--;
-        }
-        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
-        if (AccInflightCalibrationSavetoEEProm) {      // the copter is landed, disarmed and the combo has been done again
-            AccInflightCalibrationSavetoEEProm = false;
-            masterConfig.accZero[GI_ROLL] = b[GI_ROLL] / 50;
-            masterConfig.accZero[GI_PITCH] = b[GI_PITCH] / 50;
-            masterConfig.accZero[GI_YAW] = b[GI_YAW] / 50 - acc_1G;    // for nunchuk 200=1G
-            currentProfile.angleTrim[AI_ROLL] = 0;
-            currentProfile.angleTrim[AI_PITCH] = 0;
-            copyCurrentProfileToProfileSlot(masterConfig.current_profile);
-            writeEEPROM();          // write accZero in EEPROM
-            readEEPROMAndNotify();
-        }
-    }
-
-    accADC[GI_ROLL] -= masterConfig.accZero[GI_ROLL];
-    accADC[GI_PITCH] -= masterConfig.accZero[GI_PITCH];
-    accADC[GI_YAW] -= masterConfig.accZero[GI_YAW];
+    return calibratingA == 0;
 }
 
-void accGetADC(void)
+bool isOnFinalAccelerationCalibrationCycle(void)
+{
+    return calibratingA == 1;
+}
+
+bool isOnFirstAccelerationCalibrationCycle(void)
+{
+    return calibratingA == CALIBRATING_ACC_CYCLES;
+}
+
+void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    static int32_t a[3];
+    uint8_t axis;
+
+    for (axis = 0; axis < 3; axis++) {
+
+        // Reset a[axis] at start of calibration
+        if (isOnFirstAccelerationCalibrationCycle())
+            a[axis] = 0;
+
+        // Sum up CALIBRATING_ACC_CYCLES readings
+        a[axis] += accADC[axis];
+
+        // Reset global variables to prevent other code from using un-calibrated data
+        accADC[axis] = 0;
+        accelerationTrims->raw[axis] = 0;
+    }
+
+    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;
+
+        resetRollAndPitchTrims(rollAndPitchTrims);
+
+        saveAndReloadCurrentProfileToCurrentProfileSlot();
+    }
+
+    calibratingA--;
+}
+
+void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    uint8_t axis;
+    static int32_t b[3];
+    static int16_t accZero_saved[3] = { 0, 0, 0 };
+    static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } };
+
+    // 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];
+        angleTrim_saved.trims.roll = rollAndPitchTrims->trims.roll;
+        angleTrim_saved.trims.pitch = rollAndPitchTrims->trims.pitch;
+    }
+    if (InflightcalibratingA > 0) {
+        for (axis = 0; axis < 3; axis++) {
+            // Reset a[axis] at start of calibration
+            if (InflightcalibratingA == 50)
+                b[axis] = 0;
+            // Sum up 50 readings
+            b[axis] += accADC[axis];
+            // Clear global variables for next reading
+            accADC[axis] = 0;
+            accelerationTrims->raw[axis] = 0;
+        }
+        // all values are measured
+        if (InflightcalibratingA == 1) {
+            AccInflightCalibrationActive = false;
+            AccInflightCalibrationMeasurementDone = true;
+            queueConfirmationBeep(2); // buzzer to indicatiing the end of calibration
+            // recover saved values to maintain current flight behavior 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];
+            rollAndPitchTrims->trims.roll = angleTrim_saved.trims.roll;
+            rollAndPitchTrims->trims.pitch = angleTrim_saved.trims.pitch;
+        }
+        InflightcalibratingA--;
+    }
+    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
+    if (AccInflightCalibrationSavetoEEProm) {      // the copter 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 nunchuk 200=1G
+
+        resetRollAndPitchTrims(rollAndPitchTrims);
+
+        saveAndReloadCurrentProfileToCurrentProfileSlot();
+    }
+
+}
+
+void applyAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrims)
+{
+    accADC[FD_ROLL] -= accelerationTrims->raw[FD_ROLL];
+    accADC[FD_PITCH] -= accelerationTrims->raw[FD_PITCH];
+    accADC[FD_YAW] -= accelerationTrims->raw[FD_YAW];
+}
+
+void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
 {
     acc.read(accADC);
     alignSensors(accADC, accADC, accAlign);
 
-    accCommon();
+    if (!isAccelerationCalibrationComplete()) {
+        performAcclerationCalibration(rollAndPitchTrims);
+    }
+
+    if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
+        performInflightAccelerationCalibration(rollAndPitchTrims);
+    }
+
+    applyAccelerationTrims(accelerationTrims);
 }
 
+void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse)
+{
+    accelerationTrims = accelerationTrimsToUse;
+}
diff --git a/src/sensors_acceleration.h b/src/sensors_acceleration.h
index dc3f752844..84b855feae 100644
--- a/src/sensors_acceleration.h
+++ b/src/sensors_acceleration.h
@@ -16,6 +16,5 @@ extern acc_t acc;
 extern uint16_t calibratingA;
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
-void accCommon(void);
-void accGetADC(void);
-
+void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
+void setAccelerationTrims(int16_flightDynamicsTrims_t *accelerationTrimsToUse);
diff --git a/src/sensors_compass.c b/src/sensors_compass.c
index a61c9bf60f..3b9b146b9a 100644
--- a/src/sensors_compass.c
+++ b/src/sensors_compass.c
@@ -62,7 +62,7 @@ int compassGetADC(void)
             for (axis = 0; axis < 3; axis++) {
                 masterConfig.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
             }
-            copyCurrentProfileToProfileSlot(masterConfig.current_profile);
+            copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
             writeEEPROM();
             readEEPROMAndNotify();
         }
diff --git a/src/serial_cli.c b/src/serial_cli.c
index a484ce668b..cb4dc39811 100644
--- a/src/serial_cli.c
+++ b/src/serial_cli.c
@@ -194,8 +194,8 @@ const clivalue_t valueTable[] = {
     { "accxy_deadband", VAR_UINT8, &currentProfile.accxy_deadband, 0, 100 },
     { "accz_deadband", VAR_UINT8, &currentProfile.accz_deadband, 0, 100 },
     { "acc_unarmedcal", VAR_UINT8, &currentProfile.acc_unarmedcal, 0, 1 },
-    { "acc_trim_pitch", VAR_INT16, &currentProfile.angleTrim[PITCH], -300, 300 },
-    { "acc_trim_roll", VAR_INT16, &currentProfile.angleTrim[ROLL], -300, 300 },
+    { "acc_trim_pitch", VAR_INT16, &currentProfile.accelerometerTrims.trims.pitch, -300, 300 },
+    { "acc_trim_roll", VAR_INT16, &currentProfile.accelerometerTrims.trims.roll, -300, 300 },
     { "baro_tab_size", VAR_UINT8, &currentProfile.baro_tab_size, 0, BARO_TAB_SIZE_MAX },
     { "baro_noise_lpf", VAR_FLOAT, &currentProfile.baro_noise_lpf, 0, 1 },
     { "baro_cf_vel", VAR_FLOAT, &currentProfile.baro_cf_vel, 0, 1 },
@@ -821,12 +821,12 @@ static void cliProfile(char *cmdline)
 
     len = strlen(cmdline);
     if (len == 0) {
-        printf("Current profile: %d\r\n", masterConfig.current_profile);
+        printf("Current profile: %d\r\n", masterConfig.current_profile_index);
         return;
     } else {
         i = atoi(cmdline);
         if (i >= 0 && i <= 2) {
-            masterConfig.current_profile = i;
+            masterConfig.current_profile_index = i;
             writeEEPROM();
             readEEPROM();
             cliProfile("");
@@ -843,7 +843,7 @@ static void cliReboot(void) {
 static void cliSave(char *cmdline)
 {
     cliPrint("Saving...");
-    copyCurrentProfileToProfileSlot(masterConfig.current_profile);
+    copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
     writeEEPROM();
     cliReboot();
 }
diff --git a/src/serial_msp.c b/src/serial_msp.c
index 1603879147..9fc8b500c6 100755
--- a/src/serial_msp.c
+++ b/src/serial_msp.c
@@ -8,6 +8,7 @@
 #include "telemetry_common.h"
 #include "flight_common.h"
 #include "sensors_compass.h"
+#include "sensors_acceleration.h"
 
 // Multiwii Serial Protocol 0
 #define MSP_VERSION              0
@@ -294,8 +295,8 @@ static void evaluateCommand(void)
         headSerialReply(0);
         break;
     case MSP_SET_ACC_TRIM:
-        currentProfile.angleTrim[PITCH] = read16();
-        currentProfile.angleTrim[ROLL]  = read16();
+        currentProfile.accelerometerTrims.trims.pitch = read16();
+        currentProfile.accelerometerTrims.trims.roll  = read16();
         headSerialReply(0);
         break;
     case MSP_SET_RAW_GPS:
@@ -353,9 +354,9 @@ static void evaluateCommand(void)
         break;
     case MSP_SELECT_SETTING:
         if (!f.ARMED) {
-            masterConfig.current_profile = read8();
-            if (masterConfig.current_profile > 2) {
-                masterConfig.current_profile = 0;
+            masterConfig.current_profile_index = read8();
+            if (masterConfig.current_profile_index > 2) {
+                masterConfig.current_profile_index = 0;
             }
             writeEEPROM();
             readEEPROM();
@@ -402,7 +403,7 @@ static void evaluateCommand(void)
                 junk |= 1 << i;
         }
         serialize32(junk);
-        serialize8(masterConfig.current_profile);
+        serialize8(masterConfig.current_profile_index);
         break;
     case MSP_RAW_IMU:
         headSerialReply(18);
@@ -609,7 +610,7 @@ static void evaluateCommand(void)
         break;
     case MSP_ACC_CALIBRATION:
         if (!f.ARMED)
-            calibratingA = CALIBRATING_ACC_CYCLES;
+            accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
         headSerialReply(0);
         break;
     case MSP_MAG_CALIBRATION:
@@ -621,7 +622,7 @@ static void evaluateCommand(void)
         if (f.ARMED) {
             headSerialError(0);
         } else {
-            copyCurrentProfileToProfileSlot(masterConfig.current_profile);
+            copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
             writeEEPROM();
             readEEPROM();
             headSerialReply(0);
@@ -638,8 +639,8 @@ static void evaluateCommand(void)
     // Additional commands that are not compatible with MultiWii
     case MSP_ACC_TRIM:
         headSerialReply(4);
-        serialize16(currentProfile.angleTrim[PITCH]);
-        serialize16(currentProfile.angleTrim[ROLL]);
+        serialize16(currentProfile.accelerometerTrims.trims.pitch);
+        serialize16(currentProfile.accelerometerTrims.trims.roll);
         break;
     case MSP_UID:
         headSerialReply(12);