Moved alignment from sensor into device
This commit is contained in:
parent
86158a046d
commit
60e2227396
|
@ -67,7 +67,6 @@
|
|||
#define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
|
||||
#define gimbalConfig(x) (&masterConfig.gimbalConfig)
|
||||
#define sensorSelectionConfig(x) (&masterConfig.sensorSelectionConfig)
|
||||
#define sensorAlignmentConfig(x) (&masterConfig.sensorAlignmentConfig)
|
||||
#define sensorTrims(x) (&masterConfig.sensorTrims)
|
||||
#define boardAlignment(x) (&masterConfig.boardAlignment)
|
||||
#define imuConfig(x) (&masterConfig.imuConfig)
|
||||
|
@ -124,7 +123,6 @@ typedef struct master_s {
|
|||
|
||||
// global sensor-related stuff
|
||||
sensorSelectionConfig_t sensorSelectionConfig;
|
||||
sensorAlignmentConfig_t sensorAlignmentConfig;
|
||||
sensorTrims_t sensorTrims;
|
||||
boardAlignment_t boardAlignment;
|
||||
|
||||
|
|
|
@ -42,6 +42,7 @@ typedef struct gyroDev_s {
|
|||
volatile bool dataReady;
|
||||
uint16_t lpf;
|
||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||
sensor_align_e gyroAlign;
|
||||
} gyroDev_t;
|
||||
|
||||
typedef struct accDev_s {
|
||||
|
@ -49,4 +50,5 @@ typedef struct accDev_s {
|
|||
sensorReadFuncPtr read; // read 3 axis data function
|
||||
uint16_t acc_1G;
|
||||
char revisionCode; // a revision code for the sensor, if known
|
||||
sensor_align_e accAlign;
|
||||
} accDev_t;
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
typedef struct magDev_s {
|
||||
sensorInitFuncPtr init; // initialize function
|
||||
sensorReadFuncPtr read; // read 3 axis data function
|
||||
sensor_align_e magAlign;
|
||||
} magDev_t;
|
||||
|
||||
#ifndef MAG_I2C_INSTANCE
|
||||
|
|
|
@ -17,6 +17,18 @@
|
|||
|
||||
#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;
|
||||
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
|
||||
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||
|
|
|
@ -230,13 +230,6 @@ void resetBarometerConfig(barometerConfig_t *barometerConfig)
|
|||
}
|
||||
#endif
|
||||
|
||||
void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
|
||||
{
|
||||
sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT;
|
||||
sensorAlignmentConfig->acc_align = ALIGN_DEFAULT;
|
||||
sensorAlignmentConfig->mag_align = ALIGN_DEFAULT;
|
||||
}
|
||||
|
||||
#ifdef LED_STRIP
|
||||
void resetLedStripConfig(ledStripConfig_t *ledStripConfig)
|
||||
{
|
||||
|
@ -589,7 +582,9 @@ void createDefaultConfig(master_t *config)
|
|||
|
||||
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.pitchDegrees = 0;
|
||||
|
@ -1004,13 +999,6 @@ void validateAndFixGyroConfig(void)
|
|||
}
|
||||
}
|
||||
|
||||
void readEEPROMAndNotify(void)
|
||||
{
|
||||
// re-read written data
|
||||
readEEPROM();
|
||||
beeperConfirmationBeeps(1);
|
||||
}
|
||||
|
||||
void ensureEEPROMContainsValidData(void)
|
||||
{
|
||||
if (isEEPROMContentValid()) {
|
||||
|
@ -1029,7 +1017,8 @@ void resetEEPROM(void)
|
|||
void saveConfigAndNotify(void)
|
||||
{
|
||||
writeEEPROM();
|
||||
readEEPROMAndNotify();
|
||||
readEEPROM();
|
||||
beeperConfirmationBeeps(1);
|
||||
}
|
||||
|
||||
void changeProfile(uint8_t profileIndex)
|
||||
|
|
|
@ -70,7 +70,6 @@ void setPreferredBeeperOffMask(uint32_t mask);
|
|||
void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex);
|
||||
|
||||
void resetEEPROM(void);
|
||||
void readEEPROMAndNotify(void);
|
||||
void ensureEEPROMContainsValidData(void);
|
||||
|
||||
void saveConfigAndNotify(void);
|
||||
|
|
|
@ -1079,9 +1079,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
break;
|
||||
|
||||
case MSP_SENSOR_ALIGNMENT:
|
||||
sbufWriteU8(dst, sensorAlignmentConfig()->gyro_align);
|
||||
sbufWriteU8(dst, sensorAlignmentConfig()->acc_align);
|
||||
sbufWriteU8(dst, sensorAlignmentConfig()->mag_align);
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_align);
|
||||
sbufWriteU8(dst, accelerometerConfig()->acc_align);
|
||||
sbufWriteU8(dst, compassConfig()->mag_align);
|
||||
break;
|
||||
|
||||
case MSP_ADVANCED_CONFIG:
|
||||
|
@ -1432,9 +1432,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
break;
|
||||
|
||||
case MSP_SET_SENSOR_ALIGNMENT:
|
||||
sensorAlignmentConfig()->gyro_align = sbufReadU8(src);
|
||||
sensorAlignmentConfig()->acc_align = sbufReadU8(src);
|
||||
sensorAlignmentConfig()->mag_align = sbufReadU8(src);
|
||||
gyroConfig()->gyro_align = sbufReadU8(src);
|
||||
accelerometerConfig()->acc_align = sbufReadU8(src);
|
||||
compassConfig()->mag_align = sbufReadU8(src);
|
||||
break;
|
||||
|
||||
case MSP_SET_ADVANCED_CONFIG:
|
||||
|
|
|
@ -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_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 } },
|
||||
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->gyro_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->acc_align, .config.lookup = { TABLE_ALIGNMENT } },
|
||||
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorAlignmentConfig()->mag_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, &accelerometerConfig()->acc_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_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDegrees, .config.minmax = { -180, 360 } },
|
||||
|
|
|
@ -422,10 +422,10 @@ void init(void)
|
|||
#else
|
||||
const void *sonarConfig = NULL;
|
||||
#endif
|
||||
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
|
||||
&masterConfig.sensorSelectionConfig,
|
||||
compassConfig()->mag_declination,
|
||||
&masterConfig.gyroConfig,
|
||||
if (!sensorsAutodetect(sensorSelectionConfig(),
|
||||
gyroConfig(),
|
||||
accelerometerConfig(),
|
||||
compassConfig(),
|
||||
sonarConfig)) {
|
||||
// if gyro was not detected due to whatever reason, we give up now.
|
||||
failureMode(FAILURE_MISSING_ACC);
|
||||
|
|
|
@ -215,7 +215,7 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
|
|||
}
|
||||
}
|
||||
|
||||
alignSensors(acc.accSmooth, acc.accAlign);
|
||||
alignSensors(acc.accSmooth, acc.dev.accAlign);
|
||||
|
||||
if (!isAccelerationCalibrationComplete()) {
|
||||
performAcclerationCalibration(rollAndPitchTrims);
|
||||
|
|
|
@ -37,7 +37,6 @@ typedef enum {
|
|||
|
||||
typedef struct acc_s {
|
||||
accDev_t dev;
|
||||
sensor_align_e accAlign;
|
||||
uint32_t accSamplingInterval;
|
||||
int32_t accSmooth[XYZ_AXIS_COUNT];
|
||||
} acc_t;
|
||||
|
@ -57,6 +56,7 @@ typedef union rollAndPitchTrims_u {
|
|||
|
||||
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
|
||||
sensor_align_e acc_align; // acc alignment
|
||||
} accelerometerConfig_t;
|
||||
|
||||
void accInit(uint32_t gyroTargetLooptime);
|
||||
|
|
|
@ -23,7 +23,7 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "sensors.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
#include "boardalignment.h"
|
||||
|
||||
|
|
|
@ -60,7 +60,7 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
|
|||
|
||||
mag.dev.read(magADCRaw);
|
||||
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)) {
|
||||
tCal = currentTime;
|
||||
|
|
|
@ -33,7 +33,6 @@ typedef enum {
|
|||
|
||||
typedef struct mag_s {
|
||||
magDev_t dev;
|
||||
sensor_align_e magAlign;
|
||||
int32_t magADC[XYZ_AXIS_COUNT];
|
||||
float magneticDeclination;
|
||||
} mag_t;
|
||||
|
@ -43,6 +42,7 @@ extern mag_t mag;
|
|||
typedef struct compassConfig_s {
|
||||
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.
|
||||
sensor_align_e mag_align; // mag alignment
|
||||
} compassConfig_t;
|
||||
|
||||
void compassInit(void);
|
||||
|
|
|
@ -181,7 +181,7 @@ void gyroUpdate(void)
|
|||
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
||||
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
||||
|
||||
alignSensors(gyroADC, gyro.gyroAlign);
|
||||
alignSensors(gyroADC, gyro.dev.gyroAlign);
|
||||
|
||||
const bool calibrationComplete = isGyroCalibrationComplete();
|
||||
if (!calibrationComplete) {
|
||||
|
|
|
@ -37,13 +37,13 @@ typedef enum {
|
|||
typedef struct gyro_s {
|
||||
gyroDev_t dev;
|
||||
uint32_t targetLooptime;
|
||||
sensor_align_e gyroAlign;
|
||||
float gyroADCf[XYZ_AXIS_COUNT];
|
||||
} gyro_t;
|
||||
|
||||
extern gyro_t gyro;
|
||||
|
||||
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 gyro_sync_denom; // Gyro sample divider
|
||||
uint8_t gyro_soft_lpf_type;
|
||||
|
|
|
@ -161,7 +161,7 @@ bool detectGyro(void)
|
|||
{
|
||||
gyroSensor_e gyroHardware = GYRO_DEFAULT;
|
||||
|
||||
gyro.gyroAlign = ALIGN_DEFAULT;
|
||||
gyro.dev.gyroAlign = ALIGN_DEFAULT;
|
||||
|
||||
switch(gyroHardware) {
|
||||
case GYRO_DEFAULT:
|
||||
|
@ -171,7 +171,7 @@ bool detectGyro(void)
|
|||
if (mpu6050GyroDetect(&gyro.dev)) {
|
||||
gyroHardware = GYRO_MPU6050;
|
||||
#ifdef GYRO_MPU6050_ALIGN
|
||||
gyro.gyroAlign = GYRO_MPU6050_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
@ -182,7 +182,7 @@ bool detectGyro(void)
|
|||
if (l3g4200dDetect(&gyro.dev)) {
|
||||
gyroHardware = GYRO_L3G4200D;
|
||||
#ifdef GYRO_L3G4200D_ALIGN
|
||||
gyro.gyroAlign = GYRO_L3G4200D_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
@ -194,7 +194,7 @@ bool detectGyro(void)
|
|||
if (mpu3050Detect(&gyro.dev)) {
|
||||
gyroHardware = GYRO_MPU3050;
|
||||
#ifdef GYRO_MPU3050_ALIGN
|
||||
gyro.gyroAlign = GYRO_MPU3050_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
@ -206,7 +206,7 @@ bool detectGyro(void)
|
|||
if (l3gd20Detect(&gyro.dev)) {
|
||||
gyroHardware = GYRO_L3GD20;
|
||||
#ifdef GYRO_L3GD20_ALIGN
|
||||
gyro.gyroAlign = GYRO_L3GD20_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
@ -218,7 +218,7 @@ bool detectGyro(void)
|
|||
if (mpu6000SpiGyroDetect(&gyro.dev)) {
|
||||
gyroHardware = GYRO_MPU6000;
|
||||
#ifdef GYRO_MPU6000_ALIGN
|
||||
gyro.gyroAlign = GYRO_MPU6000_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
@ -235,7 +235,7 @@ bool detectGyro(void)
|
|||
{
|
||||
gyroHardware = GYRO_MPU6500;
|
||||
#ifdef GYRO_MPU6500_ALIGN
|
||||
gyro.gyroAlign = GYRO_MPU6500_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN;
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
@ -250,7 +250,7 @@ bool detectGyro(void)
|
|||
{
|
||||
gyroHardware = GYRO_MPU9250;
|
||||
#ifdef GYRO_MPU9250_ALIGN
|
||||
gyro.gyroAlign = GYRO_MPU9250_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN;
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
@ -264,7 +264,7 @@ bool detectGyro(void)
|
|||
{
|
||||
gyroHardware = GYRO_ICM20689;
|
||||
#ifdef GYRO_ICM20689_ALIGN
|
||||
gyro.gyroAlign = GYRO_ICM20689_ALIGN;
|
||||
gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN;
|
||||
#endif
|
||||
|
||||
break;
|
||||
|
@ -303,7 +303,7 @@ static bool detectAcc(accelerationSensor_e accHardwareToUse)
|
|||
#endif
|
||||
|
||||
retry:
|
||||
acc.accAlign = ALIGN_DEFAULT;
|
||||
acc.dev.accAlign = ALIGN_DEFAULT;
|
||||
|
||||
switch (accHardwareToUse) {
|
||||
case ACC_DEFAULT:
|
||||
|
@ -318,7 +318,7 @@ retry:
|
|||
if (adxl345Detect(&acc_params, &acc.dev)) {
|
||||
#endif
|
||||
#ifdef ACC_ADXL345_ALIGN
|
||||
acc.accAlign = ACC_ADXL345_ALIGN;
|
||||
acc.dev.accAlign = ACC_ADXL345_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_ADXL345;
|
||||
break;
|
||||
|
@ -329,7 +329,7 @@ retry:
|
|||
#ifdef USE_ACC_LSM303DLHC
|
||||
if (lsm303dlhcAccDetect(&acc.dev)) {
|
||||
#ifdef ACC_LSM303DLHC_ALIGN
|
||||
acc.accAlign = ACC_LSM303DLHC_ALIGN;
|
||||
acc.dev.accAlign = ACC_LSM303DLHC_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_LSM303DLHC;
|
||||
break;
|
||||
|
@ -340,7 +340,7 @@ retry:
|
|||
#ifdef USE_ACC_MPU6050
|
||||
if (mpu6050AccDetect(&acc.dev)) {
|
||||
#ifdef ACC_MPU6050_ALIGN
|
||||
acc.accAlign = ACC_MPU6050_ALIGN;
|
||||
acc.dev.accAlign = ACC_MPU6050_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_MPU6050;
|
||||
break;
|
||||
|
@ -356,7 +356,7 @@ retry:
|
|||
if (mma8452Detect(&acc.dev)) {
|
||||
#endif
|
||||
#ifdef ACC_MMA8452_ALIGN
|
||||
acc.accAlign = ACC_MMA8452_ALIGN;
|
||||
acc.dev.accAlign = ACC_MMA8452_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_MMA8452;
|
||||
break;
|
||||
|
@ -367,7 +367,7 @@ retry:
|
|||
#ifdef USE_ACC_BMA280
|
||||
if (bma280Detect(&acc.dev)) {
|
||||
#ifdef ACC_BMA280_ALIGN
|
||||
acc.accAlign = ACC_BMA280_ALIGN;
|
||||
acc.dev.accAlign = ACC_BMA280_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_BMA280;
|
||||
break;
|
||||
|
@ -378,7 +378,7 @@ retry:
|
|||
#ifdef USE_ACC_SPI_MPU6000
|
||||
if (mpu6000SpiAccDetect(&acc.dev)) {
|
||||
#ifdef ACC_MPU6000_ALIGN
|
||||
acc.accAlign = ACC_MPU6000_ALIGN;
|
||||
acc.dev.accAlign = ACC_MPU6000_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_MPU6000;
|
||||
break;
|
||||
|
@ -394,7 +394,7 @@ retry:
|
|||
#endif
|
||||
{
|
||||
#ifdef ACC_MPU6500_ALIGN
|
||||
acc.accAlign = ACC_MPU6500_ALIGN;
|
||||
acc.dev.accAlign = ACC_MPU6500_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_MPU6500;
|
||||
break;
|
||||
|
@ -407,7 +407,7 @@ retry:
|
|||
if (icm20689SpiAccDetect(&acc.dev))
|
||||
{
|
||||
#ifdef ACC_ICM20689_ALIGN
|
||||
acc.accAlign = ACC_ICM20689_ALIGN;
|
||||
acc.dev.accAlign = ACC_ICM20689_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_ICM20689;
|
||||
break;
|
||||
|
@ -547,7 +547,7 @@ static bool detectMag(magSensor_e magHardwareToUse)
|
|||
|
||||
retry:
|
||||
|
||||
mag.magAlign = ALIGN_DEFAULT;
|
||||
mag.dev.magAlign = ALIGN_DEFAULT;
|
||||
|
||||
switch(magHardwareToUse) {
|
||||
case MAG_DEFAULT:
|
||||
|
@ -557,7 +557,7 @@ retry:
|
|||
#ifdef USE_MAG_HMC5883
|
||||
if (hmc5883lDetect(&mag.dev, hmc5883Config)) {
|
||||
#ifdef MAG_HMC5883_ALIGN
|
||||
mag.magAlign = MAG_HMC5883_ALIGN;
|
||||
mag.dev.magAlign = MAG_HMC5883_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_HMC5883;
|
||||
break;
|
||||
|
@ -569,7 +569,7 @@ retry:
|
|||
#ifdef USE_MAG_AK8975
|
||||
if (ak8975Detect(&mag.dev)) {
|
||||
#ifdef MAG_AK8975_ALIGN
|
||||
mag.magAlign = MAG_AK8975_ALIGN;
|
||||
mag.dev.magAlign = MAG_AK8975_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_AK8975;
|
||||
break;
|
||||
|
@ -581,7 +581,7 @@ retry:
|
|||
#ifdef USE_MAG_AK8963
|
||||
if (ak8963Detect(&mag.dev)) {
|
||||
#ifdef MAG_AK8963_ALIGN
|
||||
mag.magAlign = MAG_AK8963_ALIGN;
|
||||
mag.dev.magAlign = MAG_AK8963_ALIGN;
|
||||
#endif
|
||||
magHardware = MAG_AK8963;
|
||||
break;
|
||||
|
@ -623,23 +623,10 @@ static bool detectSonar(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig)
|
||||
{
|
||||
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,
|
||||
bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig,
|
||||
const gyroConfig_t *gyroConfig,
|
||||
const accelerometerConfig_t *accConfig,
|
||||
const compassConfig_t *compassConfig,
|
||||
const sonarConfig_t *sonarConfig)
|
||||
{
|
||||
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
|
||||
if (detectMag(sensorSelectionConfig->mag_hardware)) {
|
||||
// calculate magnetic declination
|
||||
const int16_t deg = magDeclinationFromConfig / 100;
|
||||
const int16_t min = magDeclinationFromConfig % 100;
|
||||
const int16_t deg = compassConfig->mag_declination / 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
|
||||
compassInit();
|
||||
}
|
||||
#else
|
||||
UNUSED(magDeclinationFromConfig);
|
||||
UNUSED(compassConfig);
|
||||
#endif
|
||||
|
||||
#ifdef BARO
|
||||
|
@ -697,7 +684,15 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
|
|||
UNUSED(sonarConfig);
|
||||
#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;
|
||||
}
|
||||
|
|
|
@ -21,8 +21,8 @@ struct sensorAlignmentConfig_s;
|
|||
struct sensorSelectionConfig_s;
|
||||
struct gyroConfig_s;
|
||||
struct sonarConfig_s;
|
||||
bool sensorsAutodetect(const struct sensorAlignmentConfig_s *sensorAlignmentConfig,
|
||||
const struct sensorSelectionConfig_s *sensorSelectionConfig,
|
||||
int16_t magDeclinationFromConfig,
|
||||
const struct gyroConfig_s *gyroConfig,
|
||||
const struct sonarConfig_s *sonarConfig);
|
||||
bool sensorsAutodetect(const sensorSelectionConfig_t *sensorSelectionConfig,
|
||||
const gyroConfig_t *gyroConfig,
|
||||
const accelerometerConfig_t *accConfig,
|
||||
const compassConfig_t *compassConfig,
|
||||
const sonarConfig_t *sonarConfig);
|
||||
|
|
|
@ -52,24 +52,6 @@ typedef enum {
|
|||
SENSOR_GPSMAG = 1 << 6
|
||||
} 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 {
|
||||
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
|
||||
uint8_t baro_hardware; // Barometer hardware to use
|
||||
|
|
Loading…
Reference in New Issue