diff --git a/src/main/drivers/compass_ak8963.c b/src/main/drivers/compass_ak8963.c
index b90abf1bd0..fb280f8ae3 100644
--- a/src/main/drivers/compass_ak8963.c
+++ b/src/main/drivers/compass_ak8963.c
@@ -42,6 +42,9 @@
 #include "accgyro_spi_mpu9250.h"
 #include "compass_ak8963.h"
 
+void ak8963Init(void);
+bool ak8963Read(int16_t *magData);
+
 // This sensor is available in MPU-9250.
 
 // AK8963, mag sensor address
diff --git a/src/main/drivers/compass_ak8963.h b/src/main/drivers/compass_ak8963.h
index 34a4922257..9a13b17943 100644
--- a/src/main/drivers/compass_ak8963.h
+++ b/src/main/drivers/compass_ak8963.h
@@ -18,5 +18,3 @@
 #pragma once
 
 bool ak8963Detect(magDev_t *mag);
-void ak8963Init(void);
-bool ak8963Read(int16_t *magData);
diff --git a/src/main/drivers/compass_ak8975.c b/src/main/drivers/compass_ak8975.c
index b86218f8a9..579c7118cb 100644
--- a/src/main/drivers/compass_ak8975.c
+++ b/src/main/drivers/compass_ak8975.c
@@ -37,6 +37,9 @@
 
 #include "compass_ak8975.h"
 
+void ak8975Init(void);
+bool ak8975Read(int16_t *magData);
+
 // This sensor is available in MPU-9150.
 
 // AK8975, mag sensor address
diff --git a/src/main/drivers/compass_ak8975.h b/src/main/drivers/compass_ak8975.h
index 5a7d729bed..05f5367f7d 100644
--- a/src/main/drivers/compass_ak8975.h
+++ b/src/main/drivers/compass_ak8975.h
@@ -18,5 +18,3 @@
 #pragma once
 
 bool ak8975Detect(magDev_t *mag);
-void ak8975Init(void);
-bool ak8975Read(int16_t *magData);
diff --git a/src/main/drivers/compass_fake.c b/src/main/drivers/compass_fake.c
index cb8098c77d..20b58b1a16 100644
--- a/src/main/drivers/compass_fake.c
+++ b/src/main/drivers/compass_fake.c
@@ -32,13 +32,12 @@
 
 static int16_t fakeMagData[XYZ_AXIS_COUNT];
 
-static bool fakeMagInit(void)
+static void fakeMagInit(void)
 {
     // initially point north
     fakeMagData[X] = 4096;
     fakeMagData[Y] = 0;
     fakeMagData[Z] = 0;
-    return true;
 }
 
 void fakeMagSet(int16_t x, int16_t y, int16_t z)
diff --git a/src/main/drivers/compass_hmc5883l.c b/src/main/drivers/compass_hmc5883l.c
index 0514d3cad0..d4e802597b 100644
--- a/src/main/drivers/compass_hmc5883l.c
+++ b/src/main/drivers/compass_hmc5883l.c
@@ -41,6 +41,9 @@
 
 #include "compass_hmc5883l.h"
 
+void hmc5883lInit(void);
+bool hmc5883lRead(int16_t *magData);
+
 //#define DEBUG_MAG_DATA_READY_INTERRUPT
 
 // HMC5883L, default address 0x1E
diff --git a/src/main/drivers/compass_hmc5883l.h b/src/main/drivers/compass_hmc5883l.h
index 215bc84afa..9028143afc 100644
--- a/src/main/drivers/compass_hmc5883l.h
+++ b/src/main/drivers/compass_hmc5883l.h
@@ -24,5 +24,3 @@ typedef struct hmc5883Config_s {
 } hmc5883Config_t;
 
 bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse);
-void hmc5883lInit(void);
-bool hmc5883lRead(int16_t *magData);
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 36b8c47d16..6061341630 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -84,7 +84,7 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 #endif
 
 retry:
-    acc.dev.accAlign = ALIGN_DEFAULT;
+    dev->accAlign = ALIGN_DEFAULT;
 
     switch (accHardwareToUse) {
     case ACC_DEFAULT:
@@ -99,7 +99,7 @@ retry:
         if (adxl345Detect(&acc_params, dev)) {
 #endif
 #ifdef ACC_ADXL345_ALIGN
-            acc.dev.accAlign = ACC_ADXL345_ALIGN;
+            dev->accAlign = ACC_ADXL345_ALIGN;
 #endif
             accHardware = ACC_ADXL345;
             break;
@@ -110,7 +110,7 @@ retry:
 #ifdef USE_ACC_LSM303DLHC
         if (lsm303dlhcAccDetect(dev)) {
 #ifdef ACC_LSM303DLHC_ALIGN
-            acc.dev.accAlign = ACC_LSM303DLHC_ALIGN;
+            dev->accAlign = ACC_LSM303DLHC_ALIGN;
 #endif
             accHardware = ACC_LSM303DLHC;
             break;
@@ -121,7 +121,7 @@ retry:
 #ifdef USE_ACC_MPU6050
         if (mpu6050AccDetect(dev)) {
 #ifdef ACC_MPU6050_ALIGN
-            acc.dev.accAlign = ACC_MPU6050_ALIGN;
+            dev->accAlign = ACC_MPU6050_ALIGN;
 #endif
             accHardware = ACC_MPU6050;
             break;
@@ -137,7 +137,7 @@ retry:
         if (mma8452Detect(dev)) {
 #endif
 #ifdef ACC_MMA8452_ALIGN
-            acc.dev.accAlign = ACC_MMA8452_ALIGN;
+            dev->accAlign = ACC_MMA8452_ALIGN;
 #endif
             accHardware = ACC_MMA8452;
             break;
@@ -148,7 +148,7 @@ retry:
 #ifdef USE_ACC_BMA280
         if (bma280Detect(dev)) {
 #ifdef ACC_BMA280_ALIGN
-            acc.dev.accAlign = ACC_BMA280_ALIGN;
+            dev->accAlign = ACC_BMA280_ALIGN;
 #endif
             accHardware = ACC_BMA280;
             break;
@@ -159,7 +159,7 @@ retry:
 #ifdef USE_ACC_SPI_MPU6000
         if (mpu6000SpiAccDetect(dev)) {
 #ifdef ACC_MPU6000_ALIGN
-            acc.dev.accAlign = ACC_MPU6000_ALIGN;
+            dev->accAlign = ACC_MPU6000_ALIGN;
 #endif
             accHardware = ACC_MPU6000;
             break;
@@ -175,7 +175,7 @@ retry:
 #endif
         {
 #ifdef ACC_MPU6500_ALIGN
-            acc.dev.accAlign = ACC_MPU6500_ALIGN;
+            dev->accAlign = ACC_MPU6500_ALIGN;
 #endif
             accHardware = ACC_MPU6500;
             break;
@@ -188,7 +188,7 @@ retry:
         if (icm20689SpiAccDetect(dev))
         {
 #ifdef ACC_ICM20689_ALIGN
-            acc.dev.accAlign = ACC_ICM20689_ALIGN;
+            dev->accAlign = ACC_ICM20689_ALIGN;
 #endif
             accHardware = ACC_ICM20689;
             break;
@@ -380,7 +380,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
+        DEBUG_SET(DEBUG_ACCELEROMETER, axis, accADCRaw[axis]);
         acc.accSmooth[axis] = accADCRaw[axis];
     }
 
diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
index b0ee1a098f..9cc1e29fc3 100644
--- a/src/main/sensors/compass.c
+++ b/src/main/sensors/compass.c
@@ -81,7 +81,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
 
 retry:
 
-    mag.dev.magAlign = ALIGN_DEFAULT;
+    dev->magAlign = ALIGN_DEFAULT;
 
     switch(magHardwareToUse) {
     case MAG_DEFAULT:
@@ -91,7 +91,7 @@ retry:
 #ifdef USE_MAG_HMC5883
         if (hmc5883lDetect(dev, hmc5883Config)) {
 #ifdef MAG_HMC5883_ALIGN
-            mag.dev.magAlign = MAG_HMC5883_ALIGN;
+            dev->magAlign = MAG_HMC5883_ALIGN;
 #endif
             magHardware = MAG_HMC5883;
             break;
@@ -103,7 +103,7 @@ retry:
 #ifdef USE_MAG_AK8975
         if (ak8975Detect(dev)) {
 #ifdef MAG_AK8975_ALIGN
-            mag.dev.magAlign = MAG_AK8975_ALIGN;
+            dev->magAlign = MAG_AK8975_ALIGN;
 #endif
             magHardware = MAG_AK8975;
             break;
@@ -115,7 +115,7 @@ retry:
 #ifdef USE_MAG_AK8963
         if (ak8963Detect(dev)) {
 #ifdef MAG_AK8963_ALIGN
-            mag.dev.magAlign = MAG_AK8963_ALIGN;
+            dev->magAlign = MAG_AK8963_ALIGN;
 #endif
             magHardware = MAG_AK8963;
             break;
@@ -161,15 +161,16 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
     static uint32_t tCal = 0;
     static flightDynamicsTrims_t magZeroTempMin;
     static flightDynamicsTrims_t magZeroTempMax;
-    uint32_t axis;
 
     mag.dev.read(magADCRaw);
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) mag.magADC[axis] = magADCRaw[axis];  // int32_t copy to work with
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        mag.magADC[axis] = magADCRaw[axis];
+    }
     alignSensors(mag.magADC, mag.dev.magAlign);
 
     if (STATE(CALIBRATE_MAG)) {
         tCal = currentTime;
-        for (axis = 0; axis < 3; axis++) {
+        for (int axis = 0; axis < 3; axis++) {
             magZero->raw[axis] = 0;
             magZeroTempMin.raw[axis] = mag.magADC[axis];
             magZeroTempMax.raw[axis] = mag.magADC[axis];
@@ -186,7 +187,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
     if (tCal != 0) {
         if ((currentTime - tCal) < 30000000) {    // 30s: you have 30s to turn the multi in all directions
             LED0_TOGGLE;
-            for (axis = 0; axis < 3; axis++) {
+            for (int axis = 0; axis < 3; axis++) {
                 if (mag.magADC[axis] < magZeroTempMin.raw[axis])
                     magZeroTempMin.raw[axis] = mag.magADC[axis];
                 if (mag.magADC[axis] > magZeroTempMax.raw[axis])
@@ -194,7 +195,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
             }
         } else {
             tCal = 0;
-            for (axis = 0; axis < 3; axis++) {
+            for (int axis = 0; axis < 3; axis++) {
                 magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
             }
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index f99ca3cf5f..98c78bd7eb 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -75,7 +75,7 @@ bool gyroDetect(gyroDev_t *dev)
 {
     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 
-    gyro.dev.gyroAlign = ALIGN_DEFAULT;
+    dev->gyroAlign = ALIGN_DEFAULT;
 
     switch(gyroHardware) {
     case GYRO_DEFAULT:
@@ -85,7 +85,7 @@ bool gyroDetect(gyroDev_t *dev)
         if (mpu6050GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6050;
 #ifdef GYRO_MPU6050_ALIGN
-            gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN;
+            dev->gyroAlign = GYRO_MPU6050_ALIGN;
 #endif
             break;
         }
@@ -96,7 +96,7 @@ bool gyroDetect(gyroDev_t *dev)
         if (l3g4200dDetect(dev)) {
             gyroHardware = GYRO_L3G4200D;
 #ifdef GYRO_L3G4200D_ALIGN
-            gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN;
+            dev->gyroAlign = GYRO_L3G4200D_ALIGN;
 #endif
             break;
         }
@@ -108,7 +108,7 @@ bool gyroDetect(gyroDev_t *dev)
         if (mpu3050Detect(dev)) {
             gyroHardware = GYRO_MPU3050;
 #ifdef GYRO_MPU3050_ALIGN
-            gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN;
+            dev->gyroAlign = GYRO_MPU3050_ALIGN;
 #endif
             break;
         }
@@ -120,7 +120,7 @@ bool gyroDetect(gyroDev_t *dev)
         if (l3gd20Detect(dev)) {
             gyroHardware = GYRO_L3GD20;
 #ifdef GYRO_L3GD20_ALIGN
-            gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN;
+            dev->gyroAlign = GYRO_L3GD20_ALIGN;
 #endif
             break;
         }
@@ -132,7 +132,7 @@ bool gyroDetect(gyroDev_t *dev)
         if (mpu6000SpiGyroDetect(dev)) {
             gyroHardware = GYRO_MPU6000;
 #ifdef GYRO_MPU6000_ALIGN
-            gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN;
+            dev->gyroAlign = GYRO_MPU6000_ALIGN;
 #endif
             break;
         }
@@ -149,7 +149,7 @@ bool gyroDetect(gyroDev_t *dev)
         {
             gyroHardware = GYRO_MPU6500;
 #ifdef GYRO_MPU6500_ALIGN
-            gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN;
+            dev->gyroAlign = GYRO_MPU6500_ALIGN;
 #endif
 
             break;
@@ -164,7 +164,7 @@ case GYRO_MPU9250:
     {
         gyroHardware = GYRO_MPU9250;
 #ifdef GYRO_MPU9250_ALIGN
-        gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN;
+        dev->gyroAlign = GYRO_MPU9250_ALIGN;
 #endif
 
         break;
@@ -178,7 +178,7 @@ case GYRO_MPU9250:
         {
             gyroHardware = GYRO_ICM20689;
 #ifdef GYRO_ICM20689_ALIGN
-            gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN;
+            dev->gyroAlign = GYRO_ICM20689_ALIGN;
 #endif
 
             break;