Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

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

Browse source 
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.
This commit is contained in:
Dominic Clifton 2014-04-21 14:14:14 +01:00
parent 0f3e4add48
commit 297609d4c3
18 changed files with 124 additions and 185 deletions
Download patch file Download diff file Expand all files Collapse all files

52
src/boardalignment.c
View file

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

30
src/drivers/accgyro_adxl345.c
View file

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

27
src/drivers/accgyro_bma280.c
View file

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

2
src/drivers/accgyro_common.h
View file

@ -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

6
src/drivers/accgyro_fy90q.c
View file

@ -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
}

30
src/drivers/accgyro_l3g4200d.c
View file

@ -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;
}

26
src/drivers/accgyro_mma845x.c
View file

@ -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;
}

27
src/drivers/accgyro_mpu3050.c
View file

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

44
src/drivers/accgyro_mpu6050.c
View file

@ -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;
}

14
src/drivers/compass_hmc5883l.c
View file

@ -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];
}

2
src/drivers/compass_hmc5883l.h
View file

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

3
src/sensors_acceleration.c
View file

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

1
src/sensors_acceleration.h
View file

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

3
src/sensors_compass.c
View file

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

1
src/sensors_compass.h
View file

@ -6,3 +6,4 @@ int Mag_getADC(void);
#endif
extern int16_t magADC[XYZ_AXIS_COUNT];
extern sensor_align_e magAlign;

2
src/sensors_gyro.c
View file

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

1
src/sensors_gyro.h
View file

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

34
src/sensors_initialisation.c
View file

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