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

Moved alignment from sensor into device

Browse source
This commit is contained in:
Martin Budden 2016-12-04 21:48:29 +00:00
parent 86158a046d
commit 60e2227396
19 changed files with 82 additions and 104 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/config/config_master.h
View file

@ -67,7 +67,6 @@
#define servoMixerConfig(x) (&masterConfig.servoMixerConfig) #define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
#define gimbalConfig(x) (&masterConfig.gimbalConfig) #define gimbalConfig(x) (&masterConfig.gimbalConfig)
#define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig) #define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig)
#define sensorAlignmentConfig(x) (&masterConfig.sensorAlignmentConfig)
#define sensorTrims(x) (&masterConfig.sensorTrims) #define sensorTrims(x) (&masterConfig.sensorTrims)
#define boardAlignment(x) (&masterConfig.boardAlignment) #define boardAlignment(x) (&masterConfig.boardAlignment)
#define imuConfig(x) (&masterConfig.imuConfig) #define imuConfig(x) (&masterConfig.imuConfig)
@ -124,7 +123,6 @@ typedef struct master_s {
// global sensor-related stuff // global sensor-related stuff
sensorSelectionConfig_t sensorSelectionConfig; sensorSelectionConfig_t sensorSelectionConfig;
sensorAlignmentConfig_t sensorAlignmentConfig;
sensorTrims_t sensorTrims; sensorTrims_t sensorTrims;
boardAlignment_t boardAlignment; boardAlignment_t boardAlignment;

2
src/main/drivers/accgyro.h
View file

@ -42,6 +42,7 @@ typedef struct gyroDev_s {
volatile bool dataReady; volatile bool dataReady;
uint16_t lpf; uint16_t lpf;
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
sensor_align_e gyroAlign;
} gyroDev_t; } gyroDev_t;
typedef struct accDev_s { typedef struct accDev_s {
@ -49,4 +50,5 @@ typedef struct accDev_s {
sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr read; // read 3 axis data function
uint16_t acc_1G; uint16_t acc_1G;
char revisionCode; // a revision code for the sensor, if known char revisionCode; // a revision code for the sensor, if known
sensor_align_e accAlign;
} accDev_t; } accDev_t;

1
src/main/drivers/compass.h
View file

@ -22,6 +22,7 @@
typedef struct magDev_s { typedef struct magDev_s {
sensorInitFuncPtr init; // initialize function sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr read; // read 3 axis data function
sensor_align_e magAlign;
} magDev_t; } magDev_t;
#ifndef MAG_I2C_INSTANCE #ifndef MAG_I2C_INSTANCE

12
src/main/drivers/sensor.h
View file

@ -17,6 +17,18 @@
#pragma once #pragma once
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;
struct accDev_s; struct accDev_s;
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype

21
src/main/fc/config.c
View file

@ -230,13 +230,6 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig)
} }
#endif #endif
void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
{
sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT;
sensorAlignmentConfig->acc_align = ALIGN_DEFAULT;
sensorAlignmentConfig->mag_align = ALIGN_DEFAULT;
}
#ifdef LED_STRIP #ifdef LED_STRIP
void resetLedStripConfig(ledStripConfig_t *ledStripConfig) void resetLedStripConfig(ledStripConfig_t *ledStripConfig)
{ {
@ -589,7 +582,9 @@ void createDefaultConfig(master_t *config)
resetAccelerometerTrims(&config->sensorTrims.accZero); resetAccelerometerTrims(&config->sensorTrims.accZero);
resetSensorAlignment(&config->sensorAlignmentConfig); config->gyroConfig.gyro_align = ALIGN_DEFAULT;
config->accelerometerConfig.acc_align = ALIGN_DEFAULT;
config->compassConfig.mag_align = ALIGN_DEFAULT;
config->boardAlignment.rollDegrees = 0; config->boardAlignment.rollDegrees = 0;
config->boardAlignment.pitchDegrees = 0; config->boardAlignment.pitchDegrees = 0;
@ -1004,13 +999,6 @@ void validateAndFixGyroConfig(void)
} }
} }
void readEEPROMAndNotify(void)
{
// re-read written data
readEEPROM();
beeperConfirmationBeeps(1);
}
void ensureEEPROMContainsValidData(void) void ensureEEPROMContainsValidData(void)
{ {
if (isEEPROMContentValid()) { if (isEEPROMContentValid()) {
@ -1029,7 +1017,8 @@ void resetEEPROM(void)
void saveConfigAndNotify(void) void saveConfigAndNotify(void)
{ {
writeEEPROM(); writeEEPROM();
readEEPROMAndNotify(); readEEPROM();
beeperConfirmationBeeps(1);
} }
void changeProfile(uint8_t profileIndex) void changeProfile(uint8_t profileIndex)

1
src/main/fc/config.h
View file

@ -70,7 +70,6 @@ void setPreferredBeeperOffMask(uint32_t mask);
void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex);
void resetEEPROM(void); void resetEEPROM(void);
void readEEPROMAndNotify(void);
void ensureEEPROMContainsValidData(void); void ensureEEPROMContainsValidData(void);
void saveConfigAndNotify(void); void saveConfigAndNotify(void);

12
src/main/fc/fc_msp.c
View file

@ -1079,9 +1079,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break; break;
case MSP_SENSOR_ALIGNMENT: case MSP_SENSOR_ALIGNMENT:
sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align); sbufWriteU8(dst, gyroConfig()->gyro_align);
sbufWriteU8(dst, sensorAlignmentConfig()->acc_align); sbufWriteU8(dst, accelerometerConfig()->acc_align);
sbufWriteU8(dst, sensorAlignmentConfig()->mag_align); sbufWriteU8(dst, compassConfig()->mag_align);
break; break;
case MSP_ADVANCED_CONFIG: case MSP_ADVANCED_CONFIG:
@ -1432,9 +1432,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break; break;
case MSP_SET_SENSOR_ALIGNMENT: case MSP_SET_SENSOR_ALIGNMENT:
sensorAlignmentConfig()->gyro_align = sbufReadU8(src); gyroConfig()->gyro_align = sbufReadU8(src);
sensorAlignmentConfig()->acc_align = sbufReadU8(src); accelerometerConfig()->acc_align = sbufReadU8(src);
sensorAlignmentConfig()->mag_align = sbufReadU8(src); compassConfig()->mag_align = sbufReadU8(src);
break; break;
case MSP_SET_ADVANCED_CONFIG: case MSP_SET_ADVANCED_CONFIG:

6
src/main/io/serial_cli.c
View file

@ -803,9 +803,9 @@ const clivalue_t valueTable[] = {
{ "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } }, { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useVBatAlerts, .config.lookup = { TABLE_OFF_ON } },
{ "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } }, { "use_consumption_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &batteryConfig()->useConsumptionAlerts, .config.lookup = { TABLE_OFF_ON } },
{ "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } }, { "consumption_warning_percentage", VAR_UINT8 | MASTER_VALUE, &batteryConfig()->consumptionWarningPercentage, .config.minmax = { 0, 100 } },
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } },
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_align, .config.lookup = { TABLE_ALIGNMENT } },
{ "align_board_roll", VAR_INT16 | MASTER_VALUE, &boardAlignment()->rollDegrees, .config.minmax = { -180, 360 } }, { "align_board_roll", VAR_INT16 | MASTER_VALUE, &boardAlignment()->rollDegrees, .config.minmax = { -180, 360 } },
{ "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDegrees, .config.minmax = { -180, 360 } }, { "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDegrees, .config.minmax = { -180, 360 } },

8
src/main/main.c
View file

@ -422,10 +422,10 @@ void init(void)
#else #else
const void *sonarConfig = NULL; const void *sonarConfig = NULL;
#endif #endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig, if (!sensorsAutodetect(sensorSelectionConfig(),
&masterConfig.sensorSelectionConfig, gyroConfig(),
compassConfig()->mag_declination, accelerometerConfig(),
&masterConfig.gyroConfig, compassConfig(),
sonarConfig)) { sonarConfig)) {
// if gyro was not detected due to whatever reason, we give up now. // if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC); failureMode(FAILURE_MISSING_ACC);

2
src/main/sensors/acceleration.c
View file

@ -215,7 +215,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
} }
} }
alignSensors(acc.accSmooth, acc.accAlign); alignSensors(acc.accSmooth, acc.dev.accAlign);
if (!isAccelerationCalibrationComplete()) { if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims); performAcclerationCalibration(rollAndPitchTrims);

2
src/main/sensors/acceleration.h
View file

@ -37,7 +37,6 @@ typedef enum {
typedef struct acc_s { typedef struct acc_s {
accDev_t dev; accDev_t dev;
sensor_align_e accAlign;
uint32_t accSamplingInterval; uint32_t accSamplingInterval;
int32_t accSmooth[XYZ_AXIS_COUNT]; int32_t accSmooth[XYZ_AXIS_COUNT];
} acc_t; } acc_t;
@ -57,6 +56,7 @@ typedef union rollAndPitchTrims_u {
typedef struct accelerometerConfig_s { typedef struct accelerometerConfig_s {
uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
sensor_align_e acc_align; // acc alignment
} accelerometerConfig_t; } accelerometerConfig_t;
void accInit(uint32_t gyroTargetLooptime); void accInit(uint32_t gyroTargetLooptime);

2
src/main/sensors/boardalignment.c
View file

@ -23,7 +23,7 @@
#include "common/maths.h" #include "common/maths.h"
#include "common/axis.h" #include "common/axis.h"
#include "sensors.h" #include "drivers/sensor.h"
#include "boardalignment.h" #include "boardalignment.h"

2
src/main/sensors/compass.c
View file

@ -60,7 +60,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
mag.dev.read(magADCRaw); mag.dev.read(magADCRaw);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) mag.magADC[axis] = magADCRaw[axis]; // int32_t copy to work with for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) mag.magADC[axis] = magADCRaw[axis]; // int32_t copy to work with
alignSensors(mag.magADC, mag.magAlign); alignSensors(mag.magADC, mag.dev.magAlign);
if (STATE(CALIBRATE_MAG)) { if (STATE(CALIBRATE_MAG)) {
tCal = currentTime; tCal = currentTime;

2
src/main/sensors/compass.h
View file

@ -33,7 +33,6 @@ typedef enum {
typedef struct mag_s { typedef struct mag_s {
magDev_t dev; magDev_t dev;
sensor_align_e magAlign;
int32_t magADC[XYZ_AXIS_COUNT]; int32_t magADC[XYZ_AXIS_COUNT];
float magneticDeclination; float magneticDeclination;
} mag_t; } mag_t;
@ -43,6 +42,7 @@ extern mag_t mag;
typedef struct compassConfig_s { typedef struct compassConfig_s {
int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
// For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
sensor_align_e mag_align; // mag alignment
} compassConfig_t; } compassConfig_t;
void compassInit(void); void compassInit(void);

2
src/main/sensors/gyro.c
View file

@ -181,7 +181,7 @@ void gyroUpdate(void)
gyroADC[Y] = gyro.dev.gyroADCRaw[Y]; gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyroADC[Z] = gyro.dev.gyroADCRaw[Z]; gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
alignSensors(gyroADC, gyro.gyroAlign); alignSensors(gyroADC, gyro.dev.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete(); const bool calibrationComplete = isGyroCalibrationComplete();
if (!calibrationComplete) { if (!calibrationComplete) {

2
src/main/sensors/gyro.h
View file

@ -37,13 +37,13 @@ typedef enum {
typedef struct gyro_s { typedef struct gyro_s {
gyroDev_t dev; gyroDev_t dev;
uint32_t targetLooptime; uint32_t targetLooptime;
sensor_align_e gyroAlign;
float gyroADCf[XYZ_AXIS_COUNT]; float gyroADCf[XYZ_AXIS_COUNT];
} gyro_t; } gyro_t;
extern gyro_t gyro; extern gyro_t gyro;
typedef struct gyroConfig_s { typedef struct gyroConfig_s {
sensor_align_e gyro_align; // gyro alignment
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
uint8_t gyro_sync_denom; // Gyro sample divider uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_soft_lpf_type; uint8_t gyro_soft_lpf_type;

79
src/main/sensors/initialisation.c
View file

@ -161,7 +161,7 @@ bool detectGyro(void)
{ {
gyroSensor_e gyroHardware = GYRO_DEFAULT; gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyro.gyroAlign = ALIGN_DEFAULT; gyro.dev.gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) { switch(gyroHardware) {
case GYRO_DEFAULT: case GYRO_DEFAULT:
@ -171,7 +171,7 @@ bool detectGyro(void)
if (mpu6050GyroDetect(&gyro.dev)) { if (mpu6050GyroDetect(&gyro.dev)) {
gyroHardware = GYRO_MPU6050; gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN #ifdef GYRO_MPU6050_ALIGN
gyro.gyroAlign = GYRO_MPU6050_ALIGN; gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN;
#endif #endif
break; break;
} }
@ -182,7 +182,7 @@ bool detectGyro(void)
if (l3g4200dDetect(&gyro.dev)) { if (l3g4200dDetect(&gyro.dev)) {
gyroHardware = GYRO_L3G4200D; gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN #ifdef GYRO_L3G4200D_ALIGN
gyro.gyroAlign = GYRO_L3G4200D_ALIGN; gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN;
#endif #endif
break; break;
} }
@ -194,7 +194,7 @@ bool detectGyro(void)
if (mpu3050Detect(&gyro.dev)) { if (mpu3050Detect(&gyro.dev)) {
gyroHardware = GYRO_MPU3050; gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN #ifdef GYRO_MPU3050_ALIGN
gyro.gyroAlign = GYRO_MPU3050_ALIGN; gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN;
#endif #endif
break; break;
} }
@ -206,7 +206,7 @@ bool detectGyro(void)
if (l3gd20Detect(&gyro.dev)) { if (l3gd20Detect(&gyro.dev)) {
gyroHardware = GYRO_L3GD20; gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN #ifdef GYRO_L3GD20_ALIGN
gyro.gyroAlign = GYRO_L3GD20_ALIGN; gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN;
#endif #endif
break; break;
} }
@ -218,7 +218,7 @@ bool detectGyro(void)
if (mpu6000SpiGyroDetect(&gyro.dev)) { if (mpu6000SpiGyroDetect(&gyro.dev)) {
gyroHardware = GYRO_MPU6000; gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN #ifdef GYRO_MPU6000_ALIGN
gyro.gyroAlign = GYRO_MPU6000_ALIGN; gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN;
#endif #endif
break; break;
} }
@ -235,7 +235,7 @@ bool detectGyro(void)
{ {
gyroHardware = GYRO_MPU6500; gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN #ifdef GYRO_MPU6500_ALIGN
gyro.gyroAlign = GYRO_MPU6500_ALIGN; gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN;
#endif #endif
break; break;
@ -250,7 +250,7 @@ bool detectGyro(void)
{ {
gyroHardware = GYRO_MPU9250; gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN #ifdef GYRO_MPU9250_ALIGN
gyro.gyroAlign = GYRO_MPU9250_ALIGN; gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN;
#endif #endif
break; break;
@ -264,7 +264,7 @@ bool detectGyro(void)
{ {
gyroHardware = GYRO_ICM20689; gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN #ifdef GYRO_ICM20689_ALIGN
gyro.gyroAlign = GYRO_ICM20689_ALIGN; gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN;
#endif #endif
break; break;
@ -303,7 +303,7 @@ static bool detectAcc(accelerationSensor_e accHardwareToUse)
#endif #endif
retry: retry:
acc.accAlign = ALIGN_DEFAULT; acc.dev.accAlign = ALIGN_DEFAULT;
switch (accHardwareToUse) { switch (accHardwareToUse) {
case ACC_DEFAULT: case ACC_DEFAULT:
@ -318,7 +318,7 @@ retry:
if (adxl345Detect(&acc_params, &acc.dev)) { if (adxl345Detect(&acc_params, &acc.dev)) {
#endif #endif
#ifdef ACC_ADXL345_ALIGN #ifdef ACC_ADXL345_ALIGN
acc.accAlign = ACC_ADXL345_ALIGN; acc.dev.accAlign = ACC_ADXL345_ALIGN;
#endif #endif
accHardware = ACC_ADXL345; accHardware = ACC_ADXL345;
break; break;
@ -329,7 +329,7 @@ retry:
#ifdef USE_ACC_LSM303DLHC #ifdef USE_ACC_LSM303DLHC
if (lsm303dlhcAccDetect(&acc.dev)) { if (lsm303dlhcAccDetect(&acc.dev)) {
#ifdef ACC_LSM303DLHC_ALIGN #ifdef ACC_LSM303DLHC_ALIGN
acc.accAlign = ACC_LSM303DLHC_ALIGN; acc.dev.accAlign = ACC_LSM303DLHC_ALIGN;
#endif #endif
accHardware = ACC_LSM303DLHC; accHardware = ACC_LSM303DLHC;
break; break;
@ -340,7 +340,7 @@ retry:
#ifdef USE_ACC_MPU6050 #ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(&acc.dev)) { if (mpu6050AccDetect(&acc.dev)) {
#ifdef ACC_MPU6050_ALIGN #ifdef ACC_MPU6050_ALIGN
acc.accAlign = ACC_MPU6050_ALIGN; acc.dev.accAlign = ACC_MPU6050_ALIGN;
#endif #endif
accHardware = ACC_MPU6050; accHardware = ACC_MPU6050;
break; break;
@ -356,7 +356,7 @@ retry:
if (mma8452Detect(&acc.dev)) { if (mma8452Detect(&acc.dev)) {
#endif #endif
#ifdef ACC_MMA8452_ALIGN #ifdef ACC_MMA8452_ALIGN
acc.accAlign = ACC_MMA8452_ALIGN; acc.dev.accAlign = ACC_MMA8452_ALIGN;
#endif #endif
accHardware = ACC_MMA8452; accHardware = ACC_MMA8452;
break; break;
@ -367,7 +367,7 @@ retry:
#ifdef USE_ACC_BMA280 #ifdef USE_ACC_BMA280
if (bma280Detect(&acc.dev)) { if (bma280Detect(&acc.dev)) {
#ifdef ACC_BMA280_ALIGN #ifdef ACC_BMA280_ALIGN
acc.accAlign = ACC_BMA280_ALIGN; acc.dev.accAlign = ACC_BMA280_ALIGN;
#endif #endif
accHardware = ACC_BMA280; accHardware = ACC_BMA280;
break; break;
@ -378,7 +378,7 @@ retry:
#ifdef USE_ACC_SPI_MPU6000 #ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc.dev)) { if (mpu6000SpiAccDetect(&acc.dev)) {
#ifdef ACC_MPU6000_ALIGN #ifdef ACC_MPU6000_ALIGN
acc.accAlign = ACC_MPU6000_ALIGN; acc.dev.accAlign = ACC_MPU6000_ALIGN;
#endif #endif
accHardware = ACC_MPU6000; accHardware = ACC_MPU6000;
break; break;
@ -394,7 +394,7 @@ retry:
#endif #endif
{ {
#ifdef ACC_MPU6500_ALIGN #ifdef ACC_MPU6500_ALIGN
acc.accAlign = ACC_MPU6500_ALIGN; acc.dev.accAlign = ACC_MPU6500_ALIGN;
#endif #endif
accHardware = ACC_MPU6500; accHardware = ACC_MPU6500;
break; break;
@ -407,7 +407,7 @@ retry:
if (icm20689SpiAccDetect(&acc.dev)) if (icm20689SpiAccDetect(&acc.dev))
{ {
#ifdef ACC_ICM20689_ALIGN #ifdef ACC_ICM20689_ALIGN
acc.accAlign = ACC_ICM20689_ALIGN; acc.dev.accAlign = ACC_ICM20689_ALIGN;
#endif #endif
accHardware = ACC_ICM20689; accHardware = ACC_ICM20689;
break; break;
@ -547,7 +547,7 @@ static bool detectMag(magSensor_e magHardwareToUse)
retry: retry:
mag.magAlign = ALIGN_DEFAULT; mag.dev.magAlign = ALIGN_DEFAULT;
switch(magHardwareToUse) { switch(magHardwareToUse) {
case MAG_DEFAULT: case MAG_DEFAULT:
@ -557,7 +557,7 @@ retry:
#ifdef USE_MAG_HMC5883 #ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag.dev, hmc5883Config)) { if (hmc5883lDetect(&mag.dev, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN #ifdef MAG_HMC5883_ALIGN
mag.magAlign = MAG_HMC5883_ALIGN; mag.dev.magAlign = MAG_HMC5883_ALIGN;
#endif #endif
magHardware = MAG_HMC5883; magHardware = MAG_HMC5883;
break; break;
@ -569,7 +569,7 @@ retry:
#ifdef USE_MAG_AK8975 #ifdef USE_MAG_AK8975
if (ak8975Detect(&mag.dev)) { if (ak8975Detect(&mag.dev)) {
#ifdef MAG_AK8975_ALIGN #ifdef MAG_AK8975_ALIGN
mag.magAlign = MAG_AK8975_ALIGN; mag.dev.magAlign = MAG_AK8975_ALIGN;
#endif #endif
magHardware = MAG_AK8975; magHardware = MAG_AK8975;
break; break;
@ -581,7 +581,7 @@ retry:
#ifdef USE_MAG_AK8963 #ifdef USE_MAG_AK8963
if (ak8963Detect(&mag.dev)) { if (ak8963Detect(&mag.dev)) {
#ifdef MAG_AK8963_ALIGN #ifdef MAG_AK8963_ALIGN
mag.magAlign = MAG_AK8963_ALIGN; mag.dev.magAlign = MAG_AK8963_ALIGN;
#endif #endif
magHardware = MAG_AK8963; magHardware = MAG_AK8963;
break; break;
@ -623,23 +623,10 @@ static bool detectSonar(void)
} }
#endif #endif
static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig) bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig,
{
if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) {
gyro.gyroAlign = sensorAlignmentConfig->gyro_align;
}
if (sensorAlignmentConfig->acc_align != ALIGN_DEFAULT) {
acc.accAlign = sensorAlignmentConfig->acc_align;
}
if (sensorAlignmentConfig->mag_align != ALIGN_DEFAULT) {
mag.magAlign = sensorAlignmentConfig->mag_align;
}
}
bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
const sensorSelectionConfig_t *sensorSelectionConfig,
int16_t magDeclinationFromConfig,
const gyroConfig_t *gyroConfig, const gyroConfig_t *gyroConfig,
const accelerometerConfig_t *accConfig,
const compassConfig_t *compassConfig,
const sonarConfig_t *sonarConfig) const sonarConfig_t *sonarConfig)
{ {
memset(&acc, 0, sizeof(acc)); memset(&acc, 0, sizeof(acc));
@ -676,13 +663,13 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (detectMag(sensorSelectionConfig->mag_hardware)) { if (detectMag(sensorSelectionConfig->mag_hardware)) {
// calculate magnetic declination // calculate magnetic declination
const int16_t deg = magDeclinationFromConfig / 100; const int16_t deg = compassConfig->mag_declination / 100;
const int16_t min = magDeclinationFromConfig % 100; const int16_t min = compassConfig->mag_declination % 100;
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
compassInit(); compassInit();
} }
#else #else
UNUSED(magDeclinationFromConfig); UNUSED(compassConfig);
#endif #endif
#ifdef BARO #ifdef BARO
@ -697,7 +684,15 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
UNUSED(sonarConfig); UNUSED(sonarConfig);
#endif #endif
reconfigureAlignment(sensorAlignmentConfig); if (gyroConfig->gyro_align != ALIGN_DEFAULT) {
gyro.dev.gyroAlign = gyroConfig->gyro_align;
}
if (accConfig->acc_align != ALIGN_DEFAULT) {
acc.dev.accAlign = accConfig->acc_align;
}
if (compassConfig->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign = compassConfig->mag_align;
}
return true; return true;
} }

10
src/main/sensors/initialisation.h
View file

@ -21,8 +21,8 @@ struct sensorAlignmentConfig_s;
struct sensorSelectionConfig_s; struct sensorSelectionConfig_s;
struct gyroConfig_s; struct gyroConfig_s;
struct sonarConfig_s; struct sonarConfig_s;
bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig, bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig,
const struct sensorSelectionConfig_s *sensorSelectionConfig, const gyroConfig_t *gyroConfig,
int16_t magDeclinationFromConfig, const accelerometerConfig_t *accConfig,
const struct gyroConfig_s *gyroConfig, const compassConfig_t *compassConfig,
const struct sonarConfig_s *sonarConfig); const sonarConfig_t *sonarConfig);

18
src/main/sensors/sensors.h
View file

@ -52,24 +52,6 @@ typedef enum {
SENSOR_GPSMAG = 1 << 6 SENSOR_GPSMAG = 1 << 6
} sensors_e; } sensors_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 sensorAlignmentConfig_s {
sensor_align_e gyro_align; // gyro alignment
sensor_align_e acc_align; // acc alignment
sensor_align_e mag_align; // mag alignment
} sensorAlignmentConfig_t;
typedef struct sensorSelectionConfig_s { typedef struct sensorSelectionConfig_s {
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
uint8_t baro_hardware; // Barometer hardware to use uint8_t baro_hardware; // Barometer hardware to use