Moved alignment from sensor into device

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

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_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 } },

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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