Decouple sensor alignment from all acc/gyro/mag drivers.

This resulted in the removal of duplicate logic, duplicate code and the
removal of a temporary buffer per-driver.

The NAZE specific sensor alignment is now contained within the core code
instead of in the drivers.
The sensor alignment is determines by the sensor initialisation code.
The alignment of sensor readings is now performed once and only by the
appropriate sensor code, see usages of alignSensors().

The acc/gyro/compass driver code is now more reusable since it has no
dependencies on the main code.

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[X] = swap[X];
-            dest[Y] = src[Y];
+            dest[Y] = swap[Y];
-            dest[Z] = src[Z];
+            dest[Z] = swap[Z];
             break;
         case CW90_DEG:
-            dest[X] = src[Y];
+            dest[X] = swap[Y];
-            dest[Y] = -src[X];
+            dest[Y] = -swap[X];
-            dest[Z] = src[Z];
+            dest[Z] = swap[Z];
             break;
         case CW180_DEG:
-            dest[X] = -src[X];
+            dest[X] = -swap[X];
-            dest[Y] = -src[Y];
+            dest[Y] = -swap[Y];
-            dest[Z] = src[Z];
+            dest[Z] = swap[Z];
             break;
         case CW270_DEG:
-            dest[X] = -src[Y];
+            dest[X] = -swap[Y];
-            dest[Y] = src[X];
+            dest[Y] = swap[X];
-            dest[Z] = src[Z];
+            dest[Z] = swap[Z];
             break;
         case CW0_DEG_FLIP:
-            dest[X] = -src[X];
+            dest[X] = -swap[X];
-            dest[Y] = src[Y];
+            dest[Y] = swap[Y];
-            dest[Z] = -src[Z];
+            dest[Z] = -swap[Z];
             break;
         case CW90_DEG_FLIP:
-            dest[X] = src[Y];
+            dest[X] = swap[Y];
-            dest[Y] = src[X];
+            dest[Y] = swap[X];
-            dest[Z] = -src[Z];
+            dest[Z] = -swap[Z];
             break;
         case CW180_DEG_FLIP:
-            dest[X] = src[X];
+            dest[X] = swap[X];
-            dest[Y] = -src[Y];
+            dest[Y] = -swap[Y];
-            dest[Z] = -src[Z];
+            dest[Z] = -swap[Z];
             break;
         case CW270_DEG_FLIP:
-            dest[X] = -src[Y];
+            dest[X] = -swap[Y];
-            dest[Y] = -src[X];
+            dest[Y] = -swap[X];
-            dest[Z] = -src[Z];
+            dest[Z] = -swap[Z];
             break;
         default:
             break;

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;
+        accelData[0] = x / i;
-        data[1] = y / i;
+        accelData[1] = y / i;
-        data[2] = z / i;
+        accelData[2] = z / i;
         acc_samples = i;
     } else {
         i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
-        data[0] = buf[0] + (buf[1] << 8);
+        accelData[0] = buf[0] + (buf[1] << 8);
-        data[1] = buf[2] + (buf[3] << 8);
+        accelData[1] = buf[2] + (buf[3] << 8);
-        data[2] = buf[4] + (buf[5] << 8);
+        accelData[2] = buf[4] + (buf[5] << 8);
     }
-
-    alignSensors(data, accelData, accAlign);
 }

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));
+    accelData[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8));
-    data[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
+    accelData[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8));
-    data[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
+    accelData[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8));
-
-    alignSensors(data, accelData, accAlign);
 }

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

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
 }

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;
+    gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+    gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+    gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
-
-    alignSensors(data, gyroData, gyroAlign);
 }

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;
+    accelData[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4;
-    data[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
+    accelData[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4;
-    data[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
+    accelData[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4;
-
-    alignSensors(data, accelData, accAlign);
 }

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;
+    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
-
-    alignSensors(data, gyroData, gyroAlign);
 }
 
 static void mpu3050ReadTemp(int16_t *tempData)

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]);
+    accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    alignSensors(data, accData, accAlign);
 }
 
-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;
+    gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+    gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
-
-    alignSensors(data, gyroData, gyroAlign);
 }

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];
+    magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
-    mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
+    magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
-    mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
+    magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
-    alignSensors(mag, magData, magAlign);
 }

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);

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();
 }
 

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);

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;

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;

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();
 }

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);

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
+        gyroAlign = CW0_DEG;
-        ;
+    } else if (mpu3050Detect(&gyro, mcfg.gyro_lpf)) {
-    } 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;
+        deg = cfg.mag_declination / 100;
-    min = cfg.mag_declination % 100;
+        min = cfg.mag_declination % 100;
-    if (sensors(SENSOR_MAG))
+
         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
-    else
+    } else {
-        magneticDeclination = 0.0f;
+        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