diff --git a/src/boardalignment.c b/src/boardalignment.c
index bb1369c5ac..0c3f2e9af3 100644
--- a/src/boardalignment.c
+++ b/src/boardalignment.c
@@ -1,6 +1,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <math.h>
+#include <string.h>
 
 #include "common/maths.h"
 #include "common/axis.h"
@@ -73,46 +74,49 @@ static void alignBoard(int16_t *vec)
 
 void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
 {
+    static uint16_t swap[3];
+    memcpy(swap, src, sizeof(swap));
+
     switch (rotation) {
         case CW0_DEG:
-            dest[X] = src[X];
-            dest[Y] = src[Y];
-            dest[Z] = src[Z];
+            dest[X] = swap[X];
+            dest[Y] = swap[Y];
+            dest[Z] = swap[Z];
             break;
         case CW90_DEG:
-            dest[X] = src[Y];
-            dest[Y] = -src[X];
-            dest[Z] = src[Z];
+            dest[X] = swap[Y];
+            dest[Y] = -swap[X];
+            dest[Z] = swap[Z];
             break;
         case CW180_DEG:
-            dest[X] = -src[X];
-            dest[Y] = -src[Y];
-            dest[Z] = src[Z];
+            dest[X] = -swap[X];
+            dest[Y] = -swap[Y];
+            dest[Z] = swap[Z];
             break;
         case CW270_DEG:
-            dest[X] = -src[Y];
-            dest[Y] = src[X];
-            dest[Z] = src[Z];
+            dest[X] = -swap[Y];
+            dest[Y] = swap[X];
+            dest[Z] = swap[Z];
             break;
         case CW0_DEG_FLIP:
-            dest[X] = -src[X];
-            dest[Y] = src[Y];
-            dest[Z] = -src[Z];
+            dest[X] = -swap[X];
+            dest[Y] = swap[Y];
+            dest[Z] = -swap[Z];
             break;
         case CW90_DEG_FLIP:
-            dest[X] = src[Y];
-            dest[Y] = src[X];
-            dest[Z] = -src[Z];
+            dest[X] = swap[Y];
+            dest[Y] = swap[X];
+            dest[Z] = -swap[Z];
             break;
         case CW180_DEG_FLIP:
-            dest[X] = src[X];
-            dest[Y] = -src[Y];
-            dest[Z] = -src[Z];
+            dest[X] = swap[X];
+            dest[Y] = -swap[Y];
+            dest[Z] = -swap[Z];
             break;
         case CW270_DEG_FLIP:
-            dest[X] = -src[Y];
-            dest[Y] = -src[X];
-            dest[Z] = -src[Z];
+            dest[X] = -swap[Y];
+            dest[Y] = -swap[X];
+            dest[Z] = -swap[Z];
             break;
         default:
             break;
diff --git a/src/drivers/accgyro_adxl345.c b/src/drivers/accgyro_adxl345.c
index b2e5a2d6b1..0c8b4a01c2 100755
--- a/src/drivers/accgyro_adxl345.c
+++ b/src/drivers/accgyro_adxl345.c
@@ -3,15 +3,10 @@
 
 #include <platform.h>
 
-#include "sensors_common.h" // FIXME dependency into the main code
-
-#include "accgyro_common.h"
-
 #include "system_common.h"
 #include "bus_i2c.h"
 
-#include "boardalignment.h"
-
+#include "accgyro_common.h"
 #include "accgyro_adxl345.h"
 
 // ADXL345, Alternative address mode 0x53
@@ -43,11 +38,10 @@
 #define ADXL345_RANGE_16G   0x03
 #define ADXL345_FIFO_STREAM 0x80
 
-static void adxl345Init(sensor_align_e align);
+static void adxl345Init(void);
 static void adxl345Read(int16_t *accelData);
 
 static bool useFifo = false;
-static sensor_align_e accAlign = CW270_DEG;
 
 bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
 {
@@ -70,7 +64,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
     return true;
 }
 
-static void adxl345Init(sensor_align_e align)
+static void adxl345Init(void)
 {
    if (useFifo) {
         uint8_t fifoDepth = 16;
@@ -84,9 +78,6 @@ static void adxl345Init(sensor_align_e align)
         i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100);
     }
     acc_1G = 265; // 3.3V operation
-
-    if (align > 0)
-        accAlign = align;
 }
 
 uint8_t acc_samples = 0;
@@ -94,7 +85,6 @@ uint8_t acc_samples = 0;
 static void adxl345Read(int16_t *accelData)
 {
     uint8_t buf[8];
-    int16_t data[3];
 
     if (useFifo) {
         int32_t x = 0;
@@ -111,16 +101,14 @@ static void adxl345Read(int16_t *accelData)
             z += (int16_t)(buf[4] + (buf[5] << 8));
             samples_remaining = buf[7] & 0x7F;
         } while ((i < 32) && (samples_remaining > 0));
-        data[0] = x / i;
-        data[1] = y / i;
-        data[2] = z / i;
+        accelData[0] = x / i;
+        accelData[1] = y / i;
+        accelData[2] = z / i;
         acc_samples = i;
     } else {
         i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
-        data[0] = buf[0] + (buf[1] << 8);
-        data[1] = buf[2] + (buf[3] << 8);
-        data[2] = buf[4] + (buf[5] << 8);
+        accelData[0] = buf[0] + (buf[1] << 8);
+        accelData[1] = buf[2] + (buf[3] << 8);
+        accelData[2] = buf[4] + (buf[5] << 8);
     }
-
-    alignSensors(data, accelData, accAlign);
 }
diff --git a/src/drivers/accgyro_bma280.c b/src/drivers/accgyro_bma280.c
index b5cf4a41dd..6c12be0d24 100644
--- a/src/drivers/accgyro_bma280.c
+++ b/src/drivers/accgyro_bma280.c
@@ -3,15 +3,10 @@
 
 #include <platform.h>
 
-#include "sensors_common.h" // FIXME dependency into the main code
-#include "accgyro_common.h"
-
-#include "accgyro_bma280.h"
-
 #include "bus_i2c.h"
 
-#include "boardalignment.h"
-
+#include "accgyro_common.h"
+#include "accgyro_bma280.h"
 
 // BMA280, default I2C address mode 0x18
 #define BMA280_ADDRESS     0x18
@@ -19,11 +14,9 @@
 #define BMA280_PMU_BW      0x10
 #define BMA280_PMU_RANGE   0x0F
 
-static void bma280Init(sensor_align_e align);
+static void bma280Init(void);
 static void bma280Read(int16_t *accelData);
 
-static sensor_align_e accAlign = CW0_DEG;
-
 bool bma280Detect(acc_t *acc)
 {
     bool ack = false;
@@ -38,28 +31,22 @@ bool bma280Detect(acc_t *acc)
     return true;
 }
 
-static void bma280Init(sensor_align_e align)
+static void bma280Init(void)
 {
     i2cWrite(BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
     i2cWrite(BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
 
     acc_1G = 512 * 8;
-
-    if (align > 0)
-        accAlign = align;
 }
 
 static void bma280Read(int16_t *accelData)
 {
     uint8_t buf[6];
-    int16_t data[3];
 
     i2cRead(BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf);
 
     // Data format is lsb<5:0><crap><new_data_bit> | msb<13:6>
-    data[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
-    data[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
-    data[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
-
-    alignSensors(data, accelData, accAlign);
+    accelData[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
+    accelData[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
+    accelData[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
 }
diff --git a/src/drivers/accgyro_common.h b/src/drivers/accgyro_common.h
index aed14d5603..95daf2a76e 100644
--- a/src/drivers/accgyro_common.h
+++ b/src/drivers/accgyro_common.h
@@ -2,7 +2,7 @@
 
 extern uint16_t acc_1G;
 
-typedef void (* sensorInitFuncPtr)(sensor_align_e align);   // sensor init prototype
+typedef void (* sensorInitFuncPtr)(void);   // sensor init prototype
 typedef void (* sensorReadFuncPtr)(int16_t *data);          // sensor read and align prototype
 
 typedef struct gyro_s
diff --git a/src/drivers/accgyro_fy90q.c b/src/drivers/accgyro_fy90q.c
index ec816909a3..6303f9c11f 100644
--- a/src/drivers/accgyro_fy90q.c
+++ b/src/drivers/accgyro_fy90q.c
@@ -5,8 +5,6 @@
 
 #include "platform.h"
 
-#include "sensors_common.h" // FIXME dependency into the main code
-
 #include "accgyro_common.h"
 
 #include "system_common.h"
@@ -15,7 +13,7 @@
 
 static void adcAccRead(int16_t *accelData);
 static void adcGyroRead(int16_t *gyroData);
-static void adcDummyInit(sensor_align_e align);
+static void adcDummyInit(void);
 
 void adcSensorInit(acc_t *acc, gyro_t *gyro)
 {
@@ -57,7 +55,7 @@ static void adcGyroRead(int16_t *gyroData)
     gyroData[2] = adcData[2] * 2;
 }
 
-static void adcDummyInit(sensor_align_e align)
+static void adcDummyInit(void)
 {
     // nothing to init here
 }
diff --git a/src/drivers/accgyro_l3g4200d.c b/src/drivers/accgyro_l3g4200d.c
index 09dedaee46..cf58cdcb77 100644
--- a/src/drivers/accgyro_l3g4200d.c
+++ b/src/drivers/accgyro_l3g4200d.c
@@ -3,19 +3,15 @@
 
 #include <platform.h>
 
-#include "common/axis.h"
-
-#include "sensors_common.h" // FIXME dependency into the main code
-#include "accgyro_common.h"
-
-#include "accgyro_l3g4200d.h"
-
 #include "system_common.h"
 #include "bus_i2c.h"
 
-#include "boardalignment.h"
-
 #include "common/maths.h"
+#include "common/axis.h"
+
+#include "accgyro_common.h"
+#include "accgyro_l3g4200d.h"
+
 
 // L3G4200D, Standard address 0x68
 #define L3G4200D_ADDRESS         0x68
@@ -42,9 +38,8 @@
 #define L3G4200D_DLPF_93HZ       0xC0
 
 static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
-static sensor_align_e gyroAlign = CW0_DEG;
 
-static void l3g4200dInit(sensor_align_e align);
+static void l3g4200dInit(void);
 static void l3g4200dRead(int16_t *gyroData);
 
 bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
@@ -83,7 +78,7 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
     return true;
 }
 
-static void l3g4200dInit(sensor_align_e align)
+static void l3g4200dInit(void)
 {
     bool ack;
 
@@ -95,20 +90,15 @@ static void l3g4200dInit(sensor_align_e align)
 
     delay(5);
     i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
-    if (align > 0)
-        gyroAlign = align;
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
 static void l3g4200dRead(int16_t *gyroData)
 {
     uint8_t buf[6];
-    int16_t data[3];
 
     i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
-    data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
-
-    alignSensors(data, gyroData, gyroAlign);
+    gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
+    gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+    gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
 }
diff --git a/src/drivers/accgyro_mma845x.c b/src/drivers/accgyro_mma845x.c
index 9c72e4ec07..91513c95e7 100644
--- a/src/drivers/accgyro_mma845x.c
+++ b/src/drivers/accgyro_mma845x.c
@@ -3,17 +3,12 @@
 
 #include "platform.h"
 
-#include "sensors_common.h" // FIXME dependency into the main code
-
-#include "accgyro_common.h"
 #include "system_common.h"
 #include "gpio_common.h"
-
-#include "accgyro_mma845x.h"
-
 #include "bus_i2c.h"
 
-#include "boardalignment.h"
+#include "accgyro_common.h"
+#include "accgyro_mma845x.h"
 
 // MMA8452QT, Standard address 0x1C
 // ACC_INT2 routed to PA5
@@ -63,9 +58,8 @@
 #define MMA8452_CTRL_REG1_ACTIVE        0x01
 
 static uint8_t device_id;
-static sensor_align_e accAlign = CW90_DEG;
 
-static void mma8452Init(sensor_align_e align);
+static void mma8452Init(void);
 static void mma8452Read(int16_t *accelData);
 
 bool mma8452Detect(acc_t *acc)
@@ -87,7 +81,7 @@ bool mma8452Detect(acc_t *acc)
     return true;
 }
 
-static void mma8452Init(sensor_align_e align)
+static void mma8452Init(void)
 {
     gpio_config_t gpio;
 
@@ -108,20 +102,14 @@ static void mma8452Init(sensor_align_e align)
     i2cWrite(MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G.
 
     acc_1G = 256;
-
-    if (align > 0)
-        accAlign = align;
 }
 
 static void mma8452Read(int16_t *accelData)
 {
     uint8_t buf[6];
-    int16_t data[3];
 
     i2cRead(MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf);
-    data[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
-    data[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
-    data[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
-
-    alignSensors(data, accelData, accAlign);
+    accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
+    accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
+    accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
 }
diff --git a/src/drivers/accgyro_mpu3050.c b/src/drivers/accgyro_mpu3050.c
index c2d27f9dcd..ecbdf5d229 100755
--- a/src/drivers/accgyro_mpu3050.c
+++ b/src/drivers/accgyro_mpu3050.c
@@ -5,16 +5,12 @@
 
 #include "common/maths.h"
 
-#include "sensors_common.h" // FIXME dependency into the main code
-
-#include "accgyro_common.h"
 #include "system_common.h"
-
-#include "accgyro_mpu3050.h"
-
 #include "bus_i2c.h"
 
-#include "boardalignment.h"
+#include "accgyro_common.h"
+#include "accgyro_mpu3050.h"
+
 
 
 // MPU3050, Standard address 0x68
@@ -42,9 +38,8 @@
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
 static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
-static sensor_align_e gyroAlign = CW0_DEG;
 
-static void mpu3050Init(sensor_align_e align);
+static void mpu3050Init(void);
 static void mpu3050Read(int16_t *gyroData);
 static void mpu3050ReadTemp(int16_t *tempData);
 
@@ -90,7 +85,7 @@ bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
     return true;
 }
 
-static void mpu3050Init(sensor_align_e align)
+static void mpu3050Init(void)
 {
     bool ack;
 
@@ -104,23 +99,17 @@ static void mpu3050Init(sensor_align_e align)
     i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
     i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
     i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
-
-    if (align > 0)
-        gyroAlign = align;
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
 static void mpu3050Read(int16_t *gyroData)
 {
     uint8_t buf[6];
-    int16_t data[3];
 
     i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
-    data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
-
-    alignSensors(data, gyroData, gyroAlign);
+    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
+    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
 }
 
 static void mpu3050ReadTemp(int16_t *tempData)
diff --git a/src/drivers/accgyro_mpu6050.c b/src/drivers/accgyro_mpu6050.c
index 5c2ec2da4e..f8ceefc8a4 100644
--- a/src/drivers/accgyro_mpu6050.c
+++ b/src/drivers/accgyro_mpu6050.c
@@ -5,18 +5,12 @@
 
 #include "common/maths.h"
 
-#include "sensors_common.h" // FIXME dependency into the main code
-
-#include "accgyro_common.h"
 #include "system_common.h"
 #include "gpio_common.h"
-
-
-#include "accgyro_mpu6050.h"
-
 #include "bus_i2c.h"
 
-#include "boardalignment.h"
+#include "accgyro_common.h"
+#include "accgyro_mpu6050.h"
 
 // MPU6050, Standard address 0x68
 // MPU_INT on PB13 on rev4 hardware
@@ -125,12 +119,10 @@
 #define MPU6050_LPF_5HZ         6
 
 static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ;
-static sensor_align_e gyroAlign = CW0_DEG;
-static sensor_align_e accAlign = CW0_DEG;
 
-static void mpu6050AccInit(sensor_align_e align);
+static void mpu6050AccInit(void);
 static void mpu6050AccRead(int16_t *accData);
-static void mpu6050GyroInit(sensor_align_e align);
+static void mpu6050GyroInit(void);
 static void mpu6050GyroRead(int16_t *gyroData);
 
 typedef enum {
@@ -225,7 +217,7 @@ bool mpu6050Detect(acc_t *acc, gyro_t *gyro, uint16_t lpf)
     return true;
 }
 
-static void mpu6050AccInit(sensor_align_e align)
+static void mpu6050AccInit(void)
 {
     switch(mpuAccelTrim) {
         case MPU_6050_HALF_RESOLUTION:
@@ -235,25 +227,19 @@ static void mpu6050AccInit(sensor_align_e align)
             acc_1G = 512 * 8;
             break;
     }
-
-    if (align > 0)
-        accAlign = align;
 }
 
 static void mpu6050AccRead(int16_t *accData)
 {
     uint8_t buf[6];
-    int16_t data[3];
 
     i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
-    data[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    alignSensors(data, accData, accAlign);
+    accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
 }
 
-static void mpu6050GyroInit(sensor_align_e align)
+static void mpu6050GyroInit(void)
 {
     gpio_config_t gpio;
 
@@ -277,20 +263,14 @@ static void mpu6050GyroInit(sensor_align_e align)
     // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
     // Accel scale 8g (4096 LSB/g)
     i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
-
-    if (align > 0)
-        gyroAlign = align;
 }
 
 static void mpu6050GyroRead(int16_t *gyroData)
 {
     uint8_t buf[6];
-    int16_t data[3];
 
     i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
-    data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
-
-    alignSensors(data, gyroData, gyroAlign);
+    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
+    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
 }
diff --git a/src/drivers/compass_hmc5883l.c b/src/drivers/compass_hmc5883l.c
index f3c9f91c32..dd9e65a9e8 100755
--- a/src/drivers/compass_hmc5883l.c
+++ b/src/drivers/compass_hmc5883l.c
@@ -92,10 +92,9 @@
 #define HMC_POS_BIAS 1
 #define HMC_NEG_BIAS 2
 
-static sensor_align_e magAlign = CW180_DEG;
 static float magGain[3] = { 1.0f, 1.0f, 1.0f };
 
-bool hmc5883lDetect(sensor_align_e align)
+bool hmc5883lDetect(void)
 {
     bool ack = false;
     uint8_t sig = 0;
@@ -104,9 +103,6 @@ bool hmc5883lDetect(sensor_align_e align)
     if (!ack || sig != 'H')
         return false;
 
-    if (align > 0)
-        magAlign = align;
-
     return true;
 }
 
@@ -196,13 +192,11 @@ void hmc5883lInit(void)
 void hmc5883lRead(int16_t *magData)
 {
     uint8_t buf[6];
-    int16_t mag[3];
 
     i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
     // During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
     // After calibration is done, magGain is set to calculated gain values.
-    mag[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
-    mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
-    mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
-    alignSensors(mag, magData, magAlign);
+    magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
+    magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
+    magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
 }
diff --git a/src/drivers/compass_hmc5883l.h b/src/drivers/compass_hmc5883l.h
index 581a5d6d00..7332104915 100755
--- a/src/drivers/compass_hmc5883l.h
+++ b/src/drivers/compass_hmc5883l.h
@@ -1,5 +1,5 @@
 #pragma once
 
-bool hmc5883lDetect(sensor_align_e align);
+bool hmc5883lDetect();
 void hmc5883lInit(void);
 void hmc5883lRead(int16_t *magData);
diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c
index c9aaae23e0..50d21051f0 100644
--- a/src/sensors_acceleration.c
+++ b/src/sensors_acceleration.c
@@ -8,6 +8,7 @@
 
 acc_t acc;                       // acc access functions
 uint8_t accHardware = ACC_DEFAULT;  // which accel chip is used/detected
+sensor_align_e accAlign = 0;
 
 uint16_t calibratingA = 0;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
 
@@ -106,6 +107,8 @@ void ACC_Common(void)
 void ACC_getADC(void)
 {
     acc.read(accADC);
+    alignSensors(accADC, accADC, accAlign);
+
     ACC_Common();
 }
 
diff --git a/src/sensors_acceleration.h b/src/sensors_acceleration.h
index 600a770b87..609e59efca 100644
--- a/src/sensors_acceleration.h
+++ b/src/sensors_acceleration.h
@@ -11,6 +11,7 @@ typedef enum AccelSensors {
 } AccelSensors;
 
 extern uint8_t accHardware;
+extern sensor_align_e accAlign;
 extern acc_t acc;
 
 void ACC_Common(void);
diff --git a/src/sensors_compass.c b/src/sensors_compass.c
index 84ffb15896..b43b2d49bd 100644
--- a/src/sensors_compass.c
+++ b/src/sensors_compass.c
@@ -4,7 +4,7 @@
 #include "common/axis.h"
 
 int16_t magADC[XYZ_AXIS_COUNT];
-
+sensor_align_e magAlign = 0;
 #ifdef MAG
 static uint8_t magInit = 0;
 
@@ -30,6 +30,7 @@ int Mag_getADC(void)
 
     // Read mag sensor
     hmc5883lRead(magADC);
+    alignSensors(magADC, magADC, magAlign);
 
     if (f.CALIBRATE_MAG) {
         tCal = t;
diff --git a/src/sensors_compass.h b/src/sensors_compass.h
index 49fa047569..2aa7b44715 100644
--- a/src/sensors_compass.h
+++ b/src/sensors_compass.h
@@ -6,3 +6,4 @@ int Mag_getADC(void);
 #endif
 
 extern int16_t magADC[XYZ_AXIS_COUNT];
+extern sensor_align_e magAlign;
diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c
index aa8183b857..d2c44aadf4 100644
--- a/src/sensors_gyro.c
+++ b/src/sensors_gyro.c
@@ -9,6 +9,7 @@
 uint16_t calibratingG = 0;
 uint16_t acc_1G = 256;          // this is the 1G measured acceleration.
 gyro_t gyro;                      // gyro access functions
+sensor_align_e gyroAlign = 0;
 
 void GYRO_Common(void)
 {
@@ -54,5 +55,6 @@ void Gyro_getADC(void)
 {
     // range: +/- 8192; +/- 2000 deg/sec
     gyro.read(gyroADC);
+    alignSensors(gyroADC, gyroADC, gyroAlign);
     GYRO_Common();
 }
diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h
index 2cd0ba076c..9253897499 100644
--- a/src/sensors_gyro.h
+++ b/src/sensors_gyro.h
@@ -2,6 +2,7 @@
 
 extern uint16_t acc_1G;
 extern gyro_t gyro;
+extern sensor_align_e gyroAlign;
 
 void GYRO_Common(void);
 void Gyro_getADC(void);
diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c
index 503fd19e51..bb3e4499e7 100755
--- a/src/sensors_initialisation.c
+++ b/src/sensors_initialisation.c
@@ -4,6 +4,7 @@
 #include "sensors_acceleration.h"
 #include "sensors_barometer.h"
 #include "sensors_gyro.h"
+#include "sensors_compass.h"
 
 #include "sensors_common.h"
 
@@ -38,12 +39,13 @@ void sensorsAutodetect(void)
 
     // Autodetect gyro hardware. We have MPU3050 or MPU6050.
     if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf)) {
-        // this filled up  acc.* struct with init values
         haveMpu6k = true;
+        gyroAlign = CW0_DEG; // default NAZE alignment
     } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) {
-        // well, we found our gyro
-        ;
-    } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
+        gyroAlign = CW0_DEG;
+    } else if (mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
+        gyroAlign = CW0_DEG;
+    } else {
         // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
         failureMode(3);
     }
@@ -58,8 +60,10 @@ retry:
         case ACC_ADXL345: // ADXL345
             acc_params.useFifo = false;
             acc_params.dataRate = 800; // unused currently
-            if (adxl345Detect(&acc_params, &acc))
+            if (adxl345Detect(&acc_params, &acc)) {
                 accHardware = ACC_ADXL345;
+                accAlign = CW270_DEG; // default NAZE alignment
+            }
             if (mcfg.acc_hardware == ACC_ADXL345)
                 break;
             ; // fallthrough
@@ -67,6 +71,7 @@ retry:
             if (haveMpu6k) {
                 mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf); // yes, i'm rerunning it again.  re-fill acc struct
                 accHardware = ACC_MPU6050;
+                accAlign = CW0_DEG; // default NAZE alignment
                 if (mcfg.acc_hardware == ACC_MPU6050)
                     break;
             }
@@ -75,6 +80,7 @@ retry:
         case ACC_MMA8452: // MMA8452
             if (mma8452Detect(&acc)) {
                 accHardware = ACC_MMA8452;
+                accAlign = CW90_DEG; // default NAZE alignment
                 if (mcfg.acc_hardware == ACC_MMA8452)
                     break;
             }
@@ -82,6 +88,7 @@ retry:
         case ACC_BMA280: // BMA280
             if (bma280Detect(&acc)) {
                 accHardware = ACC_BMA280;
+                accAlign = CW0_DEG; //
                 if (mcfg.acc_hardware == ACC_BMA280)
                     break;
             }
@@ -113,22 +120,27 @@ retry:
 
     // Now time to init things, acc first
     if (sensors(SENSOR_ACC))
-        acc.init(mcfg.acc_align);
+        acc.init();
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
-    gyro.init(mcfg.gyro_align);
+    gyro.init();
 
 #ifdef MAG
-    if (!hmc5883lDetect(mcfg.mag_align))
+    if (hmc5883lDetect()) {
+        magAlign = CW180_DEG; // default NAZE alignment
+    } else {
         sensorsClear(SENSOR_MAG);
+    }
 #endif
 
+    if (sensors(SENSOR_MAG)) {
     // calculate magnetic declination
-    deg = cfg.mag_declination / 100;
-    min = cfg.mag_declination % 100;
-    if (sensors(SENSOR_MAG))
+        deg = cfg.mag_declination / 100;
+        min = cfg.mag_declination % 100;
+
         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
-    else
-        magneticDeclination = 0.0f;
+    } else {
+        magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
+    }
 }
 #endif