beginnings of the great sensor orientation unfucking. WORK IN PROGRESS DO NOT FLY.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@397 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/board.h

@@ -53,6 +53,7 @@ typedef enum AccelSensors {
     ACC_ADXL345 = 1,
     ACC_MPU6050 = 2,
     ACC_MMA8452 = 3,
+    ACC_NONE = 4
 } AccelSensors;
 
 typedef enum {
@@ -79,8 +80,35 @@ typedef enum {
     GPS_MTK,
 } GPSHardware;
 
-typedef void (* sensorInitFuncPtr)(void);                   // sensor init prototype
+typedef enum {
+    X = 0,
+    Y,
+    Z
+} sensor_axis_e;
+
+typedef enum {
+    ALIGN_DEFAULT = 0,                                      // driver-provided alignment
+    CW0_DEG = 1,
+    CW90_DEG = 2,
+    CW180_DEG = 3,
+    CW270_DEG = 4,
+    CW0_DEG_FLIP = 5,
+    CW90_DEG_FLIP = 6,
+    CW180_DEG_FLIP = 7,
+    CW270_DEG_FLIP = 8
+} sensor_align_e;
+
+typedef struct sensor_data_t
+{
+    int16_t x;
+    int16_t y;
+    int16_t z;
+    float temperature;
+} sensor_data_t;
+
+typedef void (* sensorInitFuncPtr)(sensor_align_e align);   // sensor init prototype
 typedef void (* sensorReadFuncPtr)(int16_t *data);          // sensor read and align prototype
+typedef void (* baroOpFuncPtr)(void);                       // baro start operation
 typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature);             // baro calculation (filled params are pressure and temperature)
 typedef void (* uartReceiveCallbackPtr)(uint16_t data);     // used by uart2 driver to return frames to app
 typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan);        // used by receiver driver to return channel data
@@ -90,7 +118,6 @@ typedef struct sensor_t
 {
     sensorInitFuncPtr init;                                 // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function
-    sensorReadFuncPtr align;                                // sensor align
     sensorReadFuncPtr temperature;                          // read temperature if available
     float scale;                                            // scalefactor (currently used for gyro only, todo for accel)
 } sensor_t;
@@ -99,10 +126,10 @@ typedef struct baro_t
 {
     uint16_t ut_delay;
     uint16_t up_delay;
-    sensorInitFuncPtr start_ut;
-    sensorInitFuncPtr get_ut;
-    sensorInitFuncPtr start_up;
-    sensorInitFuncPtr get_up;
+    baroOpFuncPtr start_ut;
+    baroOpFuncPtr get_ut;
+    baroOpFuncPtr start_up;
+    baroOpFuncPtr get_up;
     baroCalculateFuncPtr calculate;
 } baro_t;
 

src/cli.c

@@ -121,16 +121,10 @@ const clivalue_t valueTable[] = {
     { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 },
     { "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 },
     { "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 },
-    { "align_gyro_x", VAR_INT8, &mcfg.align[ALIGN_GYRO][0], -3, 3 },
-    { "align_gyro_y", VAR_INT8, &mcfg.align[ALIGN_GYRO][1], -3, 3 },
-    { "align_gyro_z", VAR_INT8, &mcfg.align[ALIGN_GYRO][2], -3, 3 },
-    { "align_acc_x", VAR_INT8, &mcfg.align[ALIGN_ACCEL][0], -3, 3 },
-    { "align_acc_y", VAR_INT8, &mcfg.align[ALIGN_ACCEL][1], -3, 3 },
-    { "align_acc_z", VAR_INT8, &mcfg.align[ALIGN_ACCEL][2], -3, 3 },
-    { "align_mag_x", VAR_INT8, &mcfg.align[ALIGN_MAG][0], -3, 3 },
-    { "align_mag_y", VAR_INT8, &mcfg.align[ALIGN_MAG][1], -3, 3 },
-    { "align_mag_z", VAR_INT8, &mcfg.align[ALIGN_MAG][2], -3, 3 },
-    { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 3 },
+    { "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 },
+    { "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 },
+    { "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 },
+    { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 4 },
     { "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
     { "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
     { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },

src/config.c

@@ -159,7 +159,6 @@ void checkFirstTime(bool reset)
 static void resetConf(void)
 {
     int i;
-    const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } };
 
     // Clear all configuration
     memset(&mcfg, 0, sizeof(master_t));
@@ -178,7 +177,9 @@ static void resetConf(void)
     mcfg.accZero[0] = 0;
     mcfg.accZero[1] = 0;
     mcfg.accZero[2] = 0;
-    memcpy(&mcfg.align, default_align, sizeof(mcfg.align));
+    mcfg.gyro_align = ALIGN_DEFAULT;
+    mcfg.acc_align = ALIGN_DEFAULT;
+    mcfg.mag_align = ALIGN_DEFAULT;
     mcfg.acc_hardware = ACC_DEFAULT;     // default/autodetect
     mcfg.moron_threshold = 32;
     mcfg.gyro_smoothing_factor = 0x00141403;     // default factors of 20, 20, 3 for R/P/Y

src/drv_adxl345.c

@@ -31,11 +31,11 @@
 
 extern uint16_t acc_1G;
 
-static void adxl345Init(void);
+static void adxl345Init(sensor_align_e align);
 static void adxl345Read(int16_t *accelData);
-static void adxl345Align(int16_t *accelData);
 
 static bool useFifo = false;
+static sensor_align_e accAlign = CW0_DEG;
 
 bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
 {
@@ -55,11 +55,10 @@ bool adxl345Detect(drv_adxl345_config_t *init, sensor_t *acc)
 
     acc->init = adxl345Init;
     acc->read = adxl345Read;
-    acc->align = adxl345Align;
     return true;
 }
 
-static void adxl345Init(void)
+static void adxl345Init(sensor_align_e align)
 {
    if (useFifo) {
         uint8_t fifoDepth = 16;
@@ -73,6 +72,9 @@ static void adxl345Init(void)
         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;
@@ -80,6 +82,7 @@ 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;
@@ -96,19 +99,16 @@ 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));
-        accelData[0] = x / i;
-        accelData[1] = y / i;
-        accelData[2] = z / i;
+        data[0] = x / i;
+        data[1] = y / i;
+        data[2] = z / i;
         acc_samples = i;
     } else {
         i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
-        accelData[0] = buf[0] + (buf[1] << 8);
-        accelData[1] = buf[2] + (buf[3] << 8);
-        accelData[2] = buf[4] + (buf[5] << 8);
+        data[0] = buf[0] + (buf[1] << 8);
+        data[1] = buf[2] + (buf[3] << 8);
+        data[2] = buf[4] + (buf[5] << 8);
     }
-}
 
-static void adxl345Align(int16_t *accData)
-{
-    // official direction is RPY, nothing to change here.
+    alignSensors(data, accelData, accAlign);
 }

src/drv_hmc5883l.c

@@ -73,9 +73,9 @@
 #define HMC_POS_BIAS 1
 #define HMC_NEG_BIAS 2
 
-static int8_t sensor_align[3];
+static sensor_align_e magAlign = CW180_DEG;
 
-bool hmc5883lDetect(int8_t *align)
+bool hmc5883lDetect(sensor_align_e align)
 {
     bool ack = false;
     uint8_t sig = 0;
@@ -84,7 +84,8 @@ bool hmc5883lDetect(int8_t *align)
     if (!ack || sig != 'H')
         return false;
 
-    memcpy(sensor_align, align, 3);
+    if (align > 0)
+        magAlign = align;
 
     return true;
 }
@@ -184,18 +185,11 @@ void hmc5883lRead(int16_t *magData)
 {
     uint8_t buf[6];
     int16_t mag[3];
-    int i;
 
     i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
-    mag[0] = buf[0] << 8 | buf[1];
-    mag[1] = buf[2] << 8 | buf[3];
-    mag[2] = buf[4] << 8 | buf[5];
-
-    for (i = 0; i < 3; i++) {
-        int8_t axis = sensor_align[i];
-        if (axis > 0)
-            magData[axis - 1] = mag[i];
-        else
-            magData[-axis - 1] = -mag[i];
-    }
+    mag[X] = buf[0] << 8 | buf[1];
+    mag[Z] = buf[2] << 8 | buf[3];
+    mag[Y] = buf[4] << 8 | buf[5];
+
+    alignSensors(mag, magData, magAlign);
 }

src/drv_hmc5883l.h

@@ -1,5 +1,5 @@
 #pragma once
 
-bool hmc5883lDetect(int8_t *align);
+bool hmc5883lDetect(sensor_align_e align);
 void hmc5883lInit(float *calibrationGain);
 void hmc5883lRead(int16_t *magData);

src/drv_l3g4200d.c

@@ -24,10 +24,10 @@
 #define L3G4200D_DLPF_93HZ       0xC0
 
 static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
+static sensor_align_e gyroAlign = CW0_DEG;
 
-static void l3g4200dInit(void);
+static void l3g4200dInit(sensor_align_e align);
 static void l3g4200dRead(int16_t *gyroData);
-static void l3g4200dAlign(int16_t *gyroData);
 
 bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
 {
@@ -41,7 +41,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
 
     gyro->init = l3g4200dInit;
     gyro->read = l3g4200dRead;
-    gyro->align = l3g4200dAlign;
+
     // 14.2857dps/lsb scalefactor
     gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
 
@@ -65,7 +65,7 @@ bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf)
     return true;
 }
 
-static void l3g4200dInit(void)
+static void l3g4200dInit(sensor_align_e align)
 {
     bool ack;
 
@@ -77,21 +77,20 @@ static void l3g4200dInit(void)
 
     delay(5);
     i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
-}
-
-static void l3g4200dAlign(int16_t *gyroData)
-{
-    gyroData[0] = -gyroData[0];
-    gyroData[1] = gyroData[1];
-    gyroData[2] = -gyroData[2];
+    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_GYRO_OUT, 6, buf);
-    gyroData[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
-    gyroData[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
-    gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+    data[1] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
+    data[0] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
+    data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
+
+    alignSensors(data, gyroData, gyroAlign);
 }

src/drv_mma845x.c

@@ -49,10 +49,10 @@
 
 extern uint16_t acc_1G;
 static uint8_t device_id;
+static sensor_align_e accAlign = CW180_DEG;
 
-static void mma8452Init(void);
+static void mma8452Init(sensor_align_e align);
 static void mma8452Read(int16_t *accelData);
-static void mma8452Align(int16_t *accelData);
 
 bool mma8452Detect(sensor_t *acc)
 {
@@ -69,12 +69,11 @@ bool mma8452Detect(sensor_t *acc)
 
     acc->init = mma8452Init;
     acc->read = mma8452Read;
-    acc->align = mma8452Align;
     device_id = sig;
     return true;
 }
 
-static void mma8452Init(void)
+static void mma8452Init(sensor_align_e align)
 {
     gpio_config_t gpio;
 
@@ -95,21 +94,20 @@ static void mma8452Init(void)
     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);
-    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;
-}
+    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;
 
-static void mma8452Align(int16_t *accelData)
-{
-    accelData[0] = -accelData[0];
-    accelData[1] = -accelData[1];
-    accelData[2] = accelData[2];
+    alignSensors(data, accelData, accAlign);
 }

src/drv_mpu3050.c

@@ -25,10 +25,10 @@
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
 static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
+static sensor_align_e gyroAlign = CW0_DEG;
 
-static void mpu3050Init(void);
+static void mpu3050Init(sensor_align_e align);
 static void mpu3050Read(int16_t *gyroData);
-static void mpu3050Align(int16_t *gyroData);
 static void mpu3050ReadTemp(int16_t *tempData);
 
 bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
@@ -43,7 +43,6 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
 
     gyro->init = mpu3050Init;
     gyro->read = mpu3050Read;
-    gyro->align = mpu3050Align;
     gyro->temperature = mpu3050ReadTemp;
     // 16.4 dps/lsb scalefactor
     gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
@@ -74,7 +73,7 @@ bool mpu3050Detect(sensor_t *gyro, uint16_t lpf)
     return true;
 }
 
-static void mpu3050Init(void)
+static void mpu3050Init(sensor_align_e align)
 {
     bool ack;
 
@@ -88,24 +87,23 @@ static void mpu3050Init(void)
     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);
-}
 
-static void mpu3050Align(int16_t *gyroData)
-{
-    // official direction is RPY
-    gyroData[0] = gyroData[0];
-    gyroData[1] = gyroData[1];
-    gyroData[2] = -gyroData[2];
+    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);
-    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;
+    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);
 }
 
 static void mpu3050ReadTemp(int16_t *tempData)

src/drv_mpu6050.c

@@ -4,26 +4,9 @@
 // MPU_INT on PB13 on rev4 hardware
 #define MPU6050_ADDRESS         0x68
 
-// Experimental DMP support
-// #define MPU6050_DMP
-
 #define DMP_MEM_START_ADDR 0x6E
 #define DMP_MEM_R_W 0x6F
 
-#define INV_MAX_NUM_ACCEL_SAMPLES      (8)
-#define DMP_REF_QUATERNION             (0)
-#define DMP_REF_GYROS                  (DMP_REF_QUATERNION + 4) // 4
-#define DMP_REF_CONTROL                (DMP_REF_GYROS + 3)      // 7
-#define DMP_REF_RAW                    (DMP_REF_CONTROL + 4)    // 11
-#define DMP_REF_RAW_EXTERNAL           (DMP_REF_RAW + 8)        // 19
-#define DMP_REF_ACCEL                  (DMP_REF_RAW_EXTERNAL + 6)       // 25
-#define DMP_REF_QUANT_ACCEL            (DMP_REF_ACCEL + 3)      // 28
-#define DMP_REF_QUATERNION_6AXIS       (DMP_REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES)        // 36
-#define DMP_REF_EIS                    (DMP_REF_QUATERNION_6AXIS + 4)   // 40
-#define DMP_REF_DMP_PACKET             (DMP_REF_EIS + 3)        // 43
-#define DMP_REF_GARBAGE                (DMP_REF_DMP_PACKET + 1) // 44
-#define DMP_REF_LAST                   (DMP_REF_GARBAGE + 1)    // 45
-
 #define MPU_RA_XG_OFFS_TC       0x00    //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
 #define MPU_RA_YG_OFFS_TC       0x01    //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
 #define MPU_RA_ZG_OFFS_TC       0x02    //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
@@ -113,7 +96,7 @@
 #define MPU_RA_FIFO_R_W         0x74
 #define MPU_RA_WHO_AM_I         0x75
 
-#define MPU6050_SMPLRT_DIV      0       //8000Hz
+#define MPU6050_SMPLRT_DIV      0       // 8000Hz
 
 #define MPU6050_LPF_256HZ       0
 #define MPU6050_LPF_188HZ       1
@@ -124,19 +107,13 @@
 #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(void);
+static void mpu6050AccInit(sensor_align_e align);
 static void mpu6050AccRead(int16_t *accData);
-static void mpu6050AccAlign(int16_t *accData);
-static void mpu6050GyroInit(void);
+static void mpu6050GyroInit(sensor_align_e align);
 static void mpu6050GyroRead(int16_t *gyroData);
-static void mpu6050GyroAlign(int16_t *gyroData);
-
-#ifdef MPU6050_DMP
-static void mpu6050DmpInit(void);
-float dmpdata[2];
-int16_t dmpGyroData[3];
-#endif
 
 extern uint16_t acc_1G;
 static uint8_t mpuAccelHalf = 0;
@@ -186,12 +163,11 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
 
     acc->init = mpu6050AccInit;
     acc->read = mpu6050AccRead;
-    acc->align = mpu6050AccAlign;
     gyro->init = mpu6050GyroInit;
     gyro->read = mpu6050GyroRead;
-    gyro->align = mpu6050GyroAlign;
+
     // 16.4 dps/lsb scalefactor
-    gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f));
+    gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
 
     // give halfacc (old revision) back to system
     if (scale)
@@ -223,48 +199,34 @@ bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
             break;
     }
 
-#ifdef MPU6050_DMP
-    mpu6050DmpInit();
-#endif
-
     return true;
 }
 
-static void mpu6050AccInit(void)
+static void mpu6050AccInit(sensor_align_e align)
 {
     if (mpuAccelHalf)
         acc_1G = 255 * 8;
     else
         acc_1G = 512 * 8;
+
+    if (align > 0)
+        accAlign = align;
 }
 
 static void mpu6050AccRead(int16_t *accData)
 {
     uint8_t buf[6];
+    int16_t data[3];
 
-#ifndef MPU6050_DMP
     i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
-    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]);
-#else
-    accData[0] = accData[1] = accData[2] = 0;
-#endif
-}
-
-static void mpu6050AccAlign(int16_t *accData)
-{
-    int16_t temp[2];
-    temp[0] = accData[0];
-    temp[1] = accData[1];
+    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]);
 
-    // official direction is RPY
-    accData[0] = temp[1];
-    accData[1] = -temp[0];
-    accData[2] = accData[2];
+    alignSensors(data, accData, accAlign);
 }
 
-static void mpu6050GyroInit(void)
+static void mpu6050GyroInit(sensor_align_e align)
 {
     gpio_config_t gpio;
 
@@ -277,7 +239,6 @@ static void mpu6050GyroInit(void)
     else if (hse_value == 12000000)
         gpioInit(GPIOC, &gpio);
 
-#ifndef MPU6050_DMP
     i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(5);
     i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00);      //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
@@ -289,522 +250,20 @@ static void mpu6050GyroInit(void)
     // 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);
-#endif
-}
-
-static void mpu6050GyroRead(int16_t * gyroData)
-{
-    uint8_t buf[6];
-#ifndef MPU6050_DMP
-    i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
-    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;
-#else
-    gyroData[0] = dmpGyroData[0] / 4 ;
-    gyroData[1] = dmpGyroData[1] / 4;
-    gyroData[2] = dmpGyroData[2] / 4;
-#endif
-}
-
-static void mpu6050GyroAlign(int16_t * gyroData)
-{
-    // official direction is RPY
-    gyroData[0] = gyroData[0];
-    gyroData[1] = gyroData[1];
-    gyroData[2] = -gyroData[2];
-}
-
-#ifdef MPU6050_DMP
-
-//This 3D array contains the default DMP memory bank binary that gets loaded during initialization.
-//In the Invensense UC3-A3 firmware this is uploaded in 128 byte tranmissions, but the Arduino Wire
-//library only supports 32 byte transmissions, including the register address to which you're writing,
-//so I broke it up into 16 byte transmission payloads which are sent in the dmp_init() function below.
-//
-//This was reconstructed from observed I2C traffic generated by the UC3-A3 demo code, and not extracted
-//directly from that code. That is true of all transmissions in this sketch, and any documentation has
-//been added after the fact by referencing the Invensense code.
-
-const unsigned char dmpMem[8][16][16] = {
-    {
-     {0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00},
-     {0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01},
-     {0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01},
-     {0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00},
-     {0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00},
-     {0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82},
-     {0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00},
-     {0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0},
-     {0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC},
-     {0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4},
-     {0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10}
-     },
-    {
-     {0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8},
-     {0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7},
-     {0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C},
-     {0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C},
-     {0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0}
-     },
-    {
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00},
-     {0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-     {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
-     },
-    {
-     {0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F},
-     {0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2},
-     {0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF},
-     {0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C},
-     {0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1},
-     {0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01},
-     {0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80},
-     {0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C},
-     {0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80},
-     {0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E},
-     {0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9},
-     {0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24},
-     {0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0},
-     {0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86},
-     {0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1},
-     {0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86}
-     },
-    {
-     {0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA},
-     {0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C},
-     {0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8},
-     {0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3},
-     {0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84},
-     {0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5},
-     {0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3},
-     {0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1},
-     {0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5},
-     {0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D},
-     {0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
-     {0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D},
-     {0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9},
-     {0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A},
-     {0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8},
-     {0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87}
-     },
-    {
-     {0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8},
-     {0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68},
-     {0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D},
-     {0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94},
-     {0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA},
-     {0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56},
-     {0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9},
-     {0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA},
-     {0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A},
-     {0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60},
-     {0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97},
-     {0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04},
-     {0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78},
-     {0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79},
-     {0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68},
-     {0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68}
-     },
-    {
-     {0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04},
-     {0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66},
-     {0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31},
-     {0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60},
-     {0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76},
-     {0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56},
-     {0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD},
-     {0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91},
-     {0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8},
-     {0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE},
-     {0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9},
-     {0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD},
-     {0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E},
-     {0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8},
-     {0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89},
-     {0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79}
-     },
-    {
-     {0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8},
-     {0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA},
-     {0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB},
-     {0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3},
-     {0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3},
-     {0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3},
-     {0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3},
-     {0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC},
-     {0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF}
-     }
-};
-
-
-//DMP update transmissions (Bank, Start Address, Update Length, Update Data...)
-
-const uint8_t dmp_updates[29][9] = {
-    {0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C},       //FCFG_1 inv_set_gyro_calibration
-    {0x03, 0xAB, 0x03, 0x36, 0x56, 0x76},       //FCFG_3 inv_set_gyro_calibration
-    {0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2}, //D_0_104 inv_set_gyro_calibration
-    {0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1}, //D_0_24 inv_set_gyro_calibration
-    {0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00}, //D_1_152 inv_set_accel_calibration
-    {0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97},     //FCFG_2 inv_set_accel_calibration
-    {0x03, 0x89, 0x03, 0x26, 0x46, 0x66},       //FCFG_7 inv_set_accel_calibration
-    {0x00, 0x6C, 0x02, 0x20, 0x00},     //D_0_108 inv_set_accel_calibration
-    {0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_00 inv_set_compass_calibration
-    {0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_01
-    {0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_02
-    {0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_10
-    {0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_11
-    {0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_12
-    {0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_20
-    {0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_21
-    {0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00}, //CPASS_MTX_22
-    {0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00}, //D_1_236 inv_apply_endian_accel
-    {0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97},     //FCFG_2 inv_set_mpu_sensors
-    {0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D},       //CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
-    {0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D}, //FCFG_5 inv_set_bias_update
-    {0x00, 0xA3, 0x01, 0x00},   //D_0_163 inv_set_dead_zone
-    //SET INT_ENABLE at i=22
-    {0x07, 0x86, 0x01, 0xFE},   //CFG_6 inv_set_fifo_interupt
-    {0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38},   //CFG_8 inv_send_quaternion
-    {0x07, 0x7E, 0x01, 0x30},   //CFG_16 inv_set_footer
-    {0x07, 0x46, 0x01, 0x9A},   //CFG_GYRO_SOURCE inv_send_gyro
-    {0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_9 inv_send_gyro -> inv_construct3_fifo
-    {0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_12 inv_send_accel -> inv_construct3_fifo
-    {0x02, 0x16, 0x02, 0x00, 0x00},     //D_0_22 inv_set_fifo_rate
-};
-
-static long dmp_lastRead = 0;
-static uint8_t dmp_processed_packet[8];
-static uint8_t dmp_received_packet[50];
-static uint8_t dmp_temp = 0;
-uint8_t dmp_fifoCountL = 0;
-static uint8_t dmp_fifoCountL2 = 0;
-static uint8_t dmp_packetCount = 0x00;
-static bool dmp_longPacket = false;
-static bool dmp_firstPacket = true;
-
-static void mpu6050DmpMemInit(void);
-static void mpu6050DmpBankSelect(uint8_t bank);
-static bool mpu6050DmpFifoReady(void);
-static void mpu6050DmpGetPacket(void);
-static void mpu6050DmpProcessQuat(void);
-void mpu6050DmpResetFifo(void);
-
-static void mpu6050DmpInit(void)
-{
-    uint8_t temp = 0;
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0xC0); // device reset
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
-    delay(10);
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x00);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x70);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x06);
-    i2cRead(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 1, &temp);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, 0x00);
-
-    /*
-       dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_XG_OFFS_TC);
-       dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_YG_OFFS_TC);
-       dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_ZG_OFFS_TC);
-       dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_USER_CTRL);
-     */
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0x32);        // I2C bypass enabled, latch int enabled, int read clear
-    i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
-    delay(5);
-
-    mpu6050DmpMemInit();
-}
-
-void mpu6050DmpLoop(void)
-{
-    uint8_t temp;
-    uint8_t buf[2];
-
-    if (mpu6050DmpFifoReady()) {
-        LED1_ON;
-        mpu6050DmpGetPacket();
-
-        i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &temp);
-        if (dmp_firstPacket) {
-            delay(1);
-            mpu6050DmpBankSelect(0x00);
-
-            mpu6050DmpBankSelect(0x00); // bank
-            i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
-            i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x04\x00\x00\x00"); // data
-
-            mpu6050DmpBankSelect(0x01);
-
-            i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x62);
-            i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 2, buf);
-            dmp_firstPacket = false;
-            mpu6050DmpFifoReady();
-        }
-
-        if (dmp_fifoCountL == 42) {
-            mpu6050DmpProcessQuat();
-        }
-        LED1_OFF;
-    }
-}
-
-#define dmp_quatTake32(a, b) (((a)[4*(b)+0]<<8) | ((a)[4*(b)+1]<<0))
-extern int16_t angle[2];
-
-static void mpu6050DmpProcessQuat(void)
-{
-    float quat0, quat1, quat2, quat3;
-    int32_t quatl0, quatl1, quatl2, quatl3;
-
-    quatl0 = dmp_quatTake32(dmp_received_packet, 0);
-    quatl1 = dmp_quatTake32(dmp_received_packet, 1);
-    quatl2 = dmp_quatTake32(dmp_received_packet, 2);
-    quatl3 = dmp_quatTake32(dmp_received_packet, 3);
-
-    if (quatl0 > 32767)
-        quatl0 -= 65536;
-    if (quatl1 > 32767)
-        quatl1 -= 65536;
-    if (quatl2 > 32767)
-        quatl2 -= 65536;
-    if (quatl3 > 32767)
-        quatl3 -= 65536;
-
-    quat0 = ((float) quatl0) / 16384.0f;
-    quat1 = ((float) quatl1) / 16384.0f;
-    quat2 = ((float) quatl2) / 16384.0f;
-    quat3 = ((float) quatl3) / 16384.0f;
-
-    dmpdata[0] = atan2f(2 * ((quat0 * quat1) + (quat2 * quat3)), 1.0 - (2 * ((quat1 * quat1) + (quat2 * quat2)))) * (180.0f / M_PI);
-    dmpdata[1] = asinf(2 * ((quat0 * quat2) - (quat3 * quat1))) * (180.0f / M_PI);
-    angle[0] = dmpdata[0] * 10;
-    angle[1] = dmpdata[1] * 10;
-
-    dmpGyroData[0] = ((dmp_received_packet[4 * 4 + 0] << 8) | (dmp_received_packet[4 * 4 + 1] << 0));
-    dmpGyroData[1] = ((dmp_received_packet[4 * 5 + 0] << 8) | (dmp_received_packet[4 * 5 + 1] << 0));
-    dmpGyroData[2] = ((dmp_received_packet[4 * 6 + 0] << 8) | (dmp_received_packet[4 * 6 + 1] << 0));
-}
-
-void mpu6050DmpResetFifo(void)
-{
-    uint8_t ctrl;
-
-    i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &ctrl);
-    ctrl |= 0x04;
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, ctrl);
-}
-
-static void mpu6050DmpGetPacket(void)
-{
-    if (dmp_fifoCountL > 32) {
-        dmp_fifoCountL2 = dmp_fifoCountL - 32;
-        dmp_longPacket = true;
-    }
-
-    if (dmp_longPacket) {
-        i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, 32, dmp_received_packet);
-        i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet + 32);
-        dmp_longPacket = false;
-    } else {
-        i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmp_fifoCountL, dmp_received_packet);
-    }
-}
-
-uint16_t dmpFifoLevel = 0;
-
-static bool mpu6050DmpFifoReady(void)
-{
-    uint8_t buf[2];
-
-    i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
-
-    dmp_fifoCountL = buf[1];
-    dmpFifoLevel = buf[0] << 8 | buf[1];
-
-    if (dmp_fifoCountL == 42 || dmp_fifoCountL == 44)
-        return true;
-    else {
-        // lame hack to empty out fifo, as dmpResetFifo doesn't actually seem to do it...
-        if (dmpFifoLevel > 100) {
-            // clear out fifo
-            uint8_t crap[16];
-            do {
-                i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmpFifoLevel > 16 ? 16 : dmpFifoLevel, crap);
-                i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
-                dmpFifoLevel = buf[0] << 8 | buf[1];
-            } while (dmpFifoLevel);
-        }
-    }
 
-    return false;
+    if (align > 0)
+        gyroAlign = align;
 }
 
-static void mpu6050DmpBankSelect(uint8_t bank)
+static void mpu6050GyroRead(int16_t *gyroData)
 {
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_BANK_SEL, bank);
-}
-
-static void mpu6050DmpBankInit(void)
-{
-    uint8_t i, j;
-    uint8_t incoming[9];
-
-    for (i = 0; i < 7; i++) {
-        mpu6050DmpBankSelect(i);
-        for (j = 0; j < 16; j++) {
-            uint8_t start_addy = j * 0x10;
-
-            i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
-            i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[i][j][0]);
-        }
-    }
-
-    mpu6050DmpBankSelect(7);
-
-    for (j = 0; j < 8; j++) {
-
-        uint8_t start_addy = j * 0x10;
-
-        i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
-        i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[7][j][0]);
-    }
-
-    i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, 0x80);
-    i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 9, (uint8_t *) & dmpMem[7][8][0]);
-
-    i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 8, incoming);
-}
-
-
-static void mpu6050DmpMemInit(void)
-{
-    uint8_t i;
-    uint8_t temp;
-
-    mpu6050DmpBankInit();
-
-    // Bank, Start Address, Update Length, Update Data...
-    for (i = 0; i < 22; i++) {
-        mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
-        i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
-        i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
-    }
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x32);
-
-    for (i = 22; i < 29; i++) {
-        mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
-        i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
-        i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
-    }
-
-    /*
-       dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_1);
-       dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_2);
-     */
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x02);     // ??
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03);     // CLKSEL =  PLL w Z ref
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x04);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18);    // full scale 2000 deg/s
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, 0x0B); // ext_sync_set=temp_out_L, accel DLPF 44Hz, gyro DLPF 42Hz
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_1, 0x03);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_2, 0x00);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_XG_OFFS_TC, 0x00);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_YG_OFFS_TC, 0x00);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_ZG_OFFS_TC, 0x00);
-
-    // clear offsets
-    i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_XG_OFFS_USRH, 6, "\x00\x00\x00\x00\x00\x00"); // data
-
-    mpu6050DmpBankSelect(0x01); // bank
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0xB2);
-    i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\xFF\xFF"); // data
-
-    mpu6050DmpBankSelect(0x01); // bank
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x90);
-    i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x09\x23\xA1\x35"); // data
-
-    i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &temp);
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04);      // fifo reset
-
-    // Insert FIFO count read?
-    mpu6050DmpFifoReady();
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00); // ?? I think this enables a lot of stuff but disables fifo
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL =  PLL w Z ref
-    delay(2);
-
-    i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 1, &temp);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
-    i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 1, &temp);
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 0x00); // full scale range +/- 2g
-    delay(2);
-    i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
-
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_THR, 0x02);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_THR, 0x9C);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_DUR, 0x50);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_DUR, 0x00);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04);      // fifo reset
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00);
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0xC8);      // fifo enable
-
-    mpu6050DmpBankSelect(0x01); // bank
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x6A);
-    i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\x06\x00"); // data
-
-    mpu6050DmpBankSelect(0x01); // bank
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
-    i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 8, "\x00\x00\x00\x00\x00\x00\x00\x00"); // data
-
-    mpu6050DmpBankSelect(0x00); // bank
-    i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
-    i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x40\x00\x00\x00"); // data
-}
-
-#else                          /* MPU6050_DMP */
-void mpu6050DmpLoop(void)
-{
-
-}
+    uint8_t buf[6];
+    int16_t data[3];
 
-void mpu6050DmpResetFifo(void)
-{
+    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);
 }
-#endif                          /* MPU6050_DMP */

src/drv_system.c

@@ -177,3 +177,51 @@ void systemReset(bool toBootloader)
     // Generate system reset
     SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
 }
+
+void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
+{
+    switch (rotation) {
+        case CW0_DEG:
+            dest[X] = src[X];
+            dest[Y] = src[Y];
+            dest[Z] = src[Z];
+            break;
+        case CW90_DEG:
+            dest[X] = src[Y];
+            dest[Y] = -src[X];
+            dest[Z] = src[Z];
+            break;
+        case CW180_DEG:
+            dest[X] = -src[X];
+            dest[Y] = -src[Y];
+            dest[Z] = src[Z];
+            break;
+        case CW270_DEG:
+            dest[X] = -src[Y];
+            dest[Y] = src[X];
+            dest[Z] = src[Z];
+            break;
+        case CW0_DEG_FLIP:
+            dest[X] = -src[X];
+            dest[Y] = src[Y];
+            dest[Z] = -src[Z];
+            break;
+        case CW90_DEG_FLIP:
+            dest[X] = src[Y];
+            dest[Y] = src[X];
+            dest[Z] = -src[Z];
+            break;
+        case CW180_DEG_FLIP:
+            dest[X] = src[X];
+            dest[Y] = -src[Y];
+            dest[Z] = -src[Z];
+            break;
+        case CW270_DEG_FLIP:
+            dest[X] = -src[Y];
+            dest[Y] = -src[X];
+            dest[Z] = -src[Z];
+            break;
+        default:
+            break;
+    }
+}

src/drv_system.h

@@ -15,3 +15,6 @@ void systemReset(bool toBootloader);
 
 // current crystal frequency - 8 or 12MHz
 extern uint32_t hse_value;
+
+// sensor orientation
+void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);

src/imu.c

@@ -147,10 +147,10 @@ void rotateV(struct fp_vector *v, float *delta)
     float cosx, sinx, cosy, siny, cosz, sinz;
     float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
 
-    cosx = cosf(-delta[PITCH]);
-    sinx = sinf(-delta[PITCH]);
-    cosy = cosf(delta[ROLL]);
-    siny = sinf(delta[ROLL]);
+    cosx = cosf(delta[ROLL]);
+    sinx = sinf(delta[ROLL]);
+    cosy = cosf(delta[PITCH]);
+    siny = sinf(delta[PITCH]);
     cosz = cosf(delta[YAW]);
     sinz = sinf(delta[YAW]);
 
@@ -161,13 +161,13 @@ void rotateV(struct fp_vector *v, float *delta)
     sinzsinx = sinx * sinz;
 
     mat[0][0] = coszcosy;
-    mat[0][1] = sinz * cosy;
-    mat[0][2] = -siny;
-    mat[1][0] = (coszsinx * siny) - sinzcosx;
-    mat[1][1] = (sinzsinx * siny) + (coszcosx);
-    mat[1][2] = cosy * sinx;
-    mat[2][0] = (coszcosx * siny) + (sinzsinx);
-    mat[2][1] = (sinzcosx * siny) - (coszsinx);
+    mat[0][1] = -cosy * sinz;
+    mat[0][2] = siny;
+    mat[1][0] = sinzcosx + (coszsinx * siny);
+    mat[1][1] = coszcosx - (sinzsinx * siny);
+    mat[1][2] = -sinx * cosy;
+    mat[2][0] = (sinzsinx) - (coszcosx * siny);
+    mat[2][1] = (coszsinx) + (sinzcosx * siny);
     mat[2][2] = cosy * cosx;
 
     v->X = v_tmp.X * mat[0][0] + v_tmp.Y * mat[1][0] + v_tmp.Z * mat[2][0];
@@ -257,13 +257,6 @@ static void getEstimatedAttitude(void)
     uint32_t currentT = micros();
     uint32_t deltaT;
     float scale, deltaGyroAngle[3];
-#ifndef BASEFLIGHT_CALC
-    float sqGZ;
-    float sqGX;
-    float sqGY;
-    float sqGX_sqGZ;
-    float invmagXZ;
-#endif
     deltaT = currentT - previousT;
     scale = deltaT * gyro.scale;
     previousT = currentT;
@@ -304,21 +297,8 @@ static void getEstimatedAttitude(void)
     }
 
     // Attitude of the estimated vector
-#ifdef BASEFLIGHT_CALC
-    // This hack removes gimbal lock (sorta) on pitch, so rolling around doesn't make pitch jump when roll reaches 90deg
-    angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
-    angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
-#else
-    // MW2.2 version
-    sqGZ = EstG.V.Z * EstG.V.Z;
-    sqGX = EstG.V.X * EstG.V.X;
-    sqGY = EstG.V.Y * EstG.V.Y;
-    sqGX_sqGZ = sqGX + sqGZ;
-    invmagXZ = 1.0f / sqrtf(sqGX_sqGZ);
-    invG = 1.0f / sqrtf(sqGX_sqGZ + sqGY);
-    angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
-    angle[PITCH] = _atan2f(EstG.V.Y, invmagXZ * sqGX_sqGZ);
-#endif
+    angle[ROLL] = _atan2f(EstG.V.Y, EstG.V.Z);
+    angle[PITCH] = _atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
 
 #ifdef MAG
     if (sensors(SENSOR_MAG)) {
@@ -339,7 +319,7 @@ static void getEstimatedAttitude(void)
         heading = hd;
 #else
         // MW 2.2 calculation
-        heading = _atan2f(EstM.V.Z * EstG.V.X - EstM.V.X * EstG.V.Z, EstM.V.Y * invG * sqGX_sqGZ - (EstM.V.X * EstG.V.X + EstM.V.Z * EstG.V.Z) * invG * EstG.V.Y);
+        heading = _atan2f(EstM.V.Z * EstG.V.X - EstM.V.X * EstG.V.Z, EstM.V.Y * invG * (EstG.V.X * EstG.V.X + EstG.V.Z * EstG.V.Z) - (EstM.V.X * EstG.V.X + EstM.V.Z * EstG.V.Z) * invG * EstG.V.Y);
         heading = heading + magneticDeclination;
         heading = heading / 10;
 #endif

src/mw.h

@@ -251,7 +251,9 @@ typedef struct master_t {
     uint16_t servo_pwm_rate;                // The update rate of servo outputs (50-498Hz)
 
     // global sensor-related stuff
-    int8_t align[3][3];                     // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y)
+    sensor_align_e gyro_align;              // gyro alignment
+    sensor_align_e acc_align;               // acc alignment
+    sensor_align_e mag_align;               // mag alignment
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     uint16_t gyro_lpf;                      // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
     uint16_t gyro_cmpf_factor;              // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.

src/sensors.c

@@ -50,8 +50,11 @@ void sensorsAutodetect(void)
     // Accelerometer. Fuck it. Let user break shit.
 retry:
     switch (mcfg.acc_hardware) {
-        case 0: // autodetect
-        case 1: // ADXL345
+        case ACC_NONE: // disable ACC
+            sensorsClear(SENSOR_ACC);
+            break;
+        case ACC_DEFAULT: // autodetect
+        case ACC_ADXL345: // ADXL345
             acc_params.useFifo = false;
             acc_params.dataRate = 800; // unused currently
             if (adxl345Detect(&acc_params, &acc))
@@ -59,7 +62,7 @@ retry:
             if (mcfg.acc_hardware == ACC_ADXL345)
                 break;
             ; // fallthrough
-        case 2: // MPU6050
+        case ACC_MPU6050: // MPU6050
             if (haveMpu6k) {
                 mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again.  re-fill acc struct
                 accHardware = ACC_MPU6050;
@@ -68,7 +71,7 @@ retry:
             }
             ; // fallthrough
 #ifndef OLIMEXINO
-        case 3: // MMA8452
+        case ACC_MMA8452: // MMA8452
             if (mma8452Detect(&acc)) {
                 accHardware = ACC_MMA8452;
                 if (mcfg.acc_hardware == ACC_MMA8452)
@@ -102,12 +105,12 @@ retry:
 
     // Now time to init things, acc first
     if (sensors(SENSOR_ACC))
-        acc.init();
+        acc.init(mcfg.acc_align);
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
-    gyro.init();
+    gyro.init(mcfg.gyro_align);
 
 #ifdef MAG
-    if (!hmc5883lDetect(mcfg.align[ALIGN_MAG]))
+    if (!hmc5883lDetect(mcfg.mag_align))
         sensorsClear(SENSOR_MAG);
 #endif
 
@@ -147,28 +150,6 @@ void batteryInit(void)
     batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI
 }
 
-// ALIGN_GYRO = 0,
-// ALIGN_ACCEL = 1,
-// ALIGN_MAG = 2
-static void alignSensors(uint8_t type, int16_t *data)
-{
-    int i;
-    int16_t tmp[3];
-
-    // make a copy :(
-    tmp[0] = data[0];
-    tmp[1] = data[1];
-    tmp[2] = data[2];
-
-    for (i = 0; i < 3; i++) {
-        int8_t axis = mcfg.align[type][i];
-        if (axis > 0)
-            data[axis - 1] = tmp[i];
-        else
-            data[-axis - 1] = -tmp[i];
-    }
-}
-
 static void ACC_Common(void)
 {
     static int32_t a[3];
@@ -254,12 +235,6 @@ static void ACC_Common(void)
 void ACC_getADC(void)
 {
     acc.read(accADC);
-    // if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
-    if (mcfg.align[ALIGN_ACCEL][0])
-        alignSensors(ALIGN_ACCEL, accADC);
-    else
-        acc.align(accADC);
-
     ACC_Common();
 }
 
@@ -392,12 +367,6 @@ void Gyro_getADC(void)
 {
     // range: +/- 8192; +/- 2000 deg/sec
     gyro.read(gyroADC);
-    // if we have CUSTOM alignment configured, user is "assumed" to know what they're doing
-    if (mcfg.align[ALIGN_GYRO][0])
-        alignSensors(ALIGN_GYRO, gyroADC);
-    else
-        gyro.align(gyroADC);
-
     GYRO_Common();
 }
 
@@ -405,12 +374,6 @@ void Gyro_getADC(void)
 static float magCal[3] = { 1.0f, 1.0f, 1.0f };     // gain for each axis, populated at sensor init
 static uint8_t magInit = 0;
 
-static void Mag_getRawADC(void)
-{
-    // MAG driver will align itself, so no need to alignSensors()
-    hmc5883lRead(magADC);
-}
-
 void Mag_init(void)
 {
     // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
@@ -432,11 +395,11 @@ int Mag_getADC(void)
     t = currentTime + 100000;
 
     // Read mag sensor
-    Mag_getRawADC();
+    hmc5883lRead(magADC);
 
-    magADC[ROLL]  = magADC[ROLL]  * magCal[ROLL];
-    magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
-    magADC[YAW]   = magADC[YAW]   * magCal[YAW];
+    magADC[X] = magADC[X] * magCal[X];
+    magADC[Y] = magADC[Y] * magCal[Y];
+    magADC[Z] = magADC[Z] * magCal[Z];
 
     if (f.CALIBRATE_MAG) {
         tCal = t;
@@ -449,9 +412,9 @@ int Mag_getADC(void)
     }
 
     if (magInit) {              // we apply offset only once mag calibration is done
-        magADC[ROLL] -= mcfg.magZero[ROLL];
-        magADC[PITCH] -= mcfg.magZero[PITCH];
-        magADC[YAW] -= mcfg.magZero[YAW];
+        magADC[X] -= mcfg.magZero[X];
+        magADC[Y] -= mcfg.magZero[Y];
+        magADC[Z] -= mcfg.magZero[Z];
     }
 
     if (tCal != 0) {