rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1751 from martinbudden/bf_sensor_tidy 

Rationalise content of sensor data structures
This commit is contained in:
borisbstyle 2016-12-05 11:01:25 +01:00 committed by GitHub
parent 3f495d0759 35e4862ddb
commit b90bcddd8d
28 changed files with 155 additions and 192 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/blackbox/blackbox.c
View File

@ -1271,9 +1271,9 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff:%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
gyroConfig()->gyro_soft_notch_cutoff_2);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz:%d", (int)(accelerometerConfig()->acc_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", sensorSelectionConfig()->acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", sensorSelectionConfig()->baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", sensorSelectionConfig()->mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("acc_hardware:%d", accelerometerConfig()->acc_hardware);
BLACKBOX_PRINT_HEADER_LINE("baro_hardware:%d", barometerConfig()->baro_hardware);
BLACKBOX_PRINT_HEADER_LINE("mag_hardware:%d", compassConfig()->mag_hardware);
BLACKBOX_PRINT_HEADER_LINE("gyro_cal_on_first_arm:%d", armingConfig()->gyro_cal_on_first_arm);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation:%d", rxConfig()->rcInterpolation);
BLACKBOX_PRINT_HEADER_LINE("rc_interpolation_interval:%d", rxConfig()->rcInterpolationInterval);

7
src/main/config/config_master.h
View File

@ -66,9 +66,6 @@
#define servoConfig(x) (&masterConfig.servoConfig)
#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)
#define gyroConfig(x) (&masterConfig.gyroConfig)
@ -122,10 +119,6 @@ typedef struct master_s {
gimbalConfig_t gimbalConfig;
#endif
// global sensor-related stuff
sensorSelectionConfig_t sensorSelectionConfig;
sensorAlignmentConfig_t sensorAlignmentConfig;
sensorTrims_t sensorTrims;
boardAlignment_t boardAlignment;
imuConfig_t imuConfig;

2
src/main/drivers/accgyro.h
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;

1
src/main/drivers/compass.h
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

12
src/main/drivers/sensor.h
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

31
src/main/fc/config.c
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)
{
@ -587,22 +580,24 @@ void createDefaultConfig(master_t *config)
config->debug_mode = DEBUG_MODE;
resetAccelerometerTrims(&config->sensorTrims.accZero);
resetAccelerometerTrims(&config->accelerometerConfig.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;
config->boardAlignment.yawDegrees = 0;
config->sensorSelectionConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
config->accelerometerConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
config->max_angle_inclination = 700; // 70 degrees
config->rcControlsConfig.yaw_control_direction = 1;
config->gyroConfig.gyroMovementCalibrationThreshold = 32;
// xxx_hardware: 0:default/autodetect, 1: disable
config->sensorSelectionConfig.mag_hardware = 1;
config->compassConfig.mag_hardware = 1;
config->sensorSelectionConfig.baro_hardware = 1;
config->barometerConfig.baro_hardware = 1;
resetBatteryConfig(&config->batteryConfig);
@ -842,7 +837,7 @@ void activateConfig(void)
#endif
useFailsafeConfig(&masterConfig.failsafeConfig);
setAccelerationTrims(&sensorTrims()->accZero);
setAccelerationTrims(&accelerometerConfig()->accZero);
setAccelerationFilter(accelerometerConfig()->acc_lpf_hz);
mixerUseConfigs(
@ -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)

1
src/main/fc/config.h
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);

24
src/main/fc/fc_msp.c
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:
@ -1125,9 +1125,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_SENSOR_CONFIG:
sbufWriteU8(dst, sensorSelectionConfig()->acc_hardware);
sbufWriteU8(dst, sensorSelectionConfig()->baro_hardware);
sbufWriteU8(dst, sensorSelectionConfig()->mag_hardware);
sbufWriteU8(dst, accelerometerConfig()->acc_hardware);
sbufWriteU8(dst, barometerConfig()->baro_hardware);
sbufWriteU8(dst, compassConfig()->mag_hardware);
break;
case MSP_REBOOT:
@ -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:
@ -1487,9 +1487,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
case MSP_SET_SENSOR_CONFIG:
sensorSelectionConfig()->acc_hardware = sbufReadU8(src);
sensorSelectionConfig()->baro_hardware = sbufReadU8(src);
sensorSelectionConfig()->mag_hardware = sbufReadU8(src);
accelerometerConfig()->acc_hardware = sbufReadU8(src);
barometerConfig()->baro_hardware = sbufReadU8(src);
compassConfig()->mag_hardware = sbufReadU8(src);
break;
case MSP_RESET_CONF:

2
src/main/fc/fc_tasks.c
View File

@ -158,7 +158,7 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
static void taskUpdateCompass(timeUs_t currentTimeUs)
{
if (sensors(SENSOR_MAG)) {
compassUpdate(currentTimeUs, &sensorTrims()->magZero);
compassUpdate(currentTimeUs, &compassConfig()->magZero);
}
}
#endif

18
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_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 } },
@ -869,7 +869,7 @@ const clivalue_t valueTable[] = {
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, &rxConfig()->rx_min_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } },
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, &rxConfig()->rx_max_usec, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX } },
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &accelerometerConfig()->acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, &accelerometerConfig()->acc_lpf_hz, .config.minmax = { 0, 400 } },
{ "accxy_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.xy, .config.minmax = { 0, 100 } },
{ "accz_deadband", VAR_UINT8 | MASTER_VALUE, &imuConfig()->accDeadband.z, .config.minmax = { 0, 100 } },
@ -882,11 +882,11 @@ const clivalue_t valueTable[] = {
{ "baro_noise_lpf", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_noise_lpf, .config.minmax = { 0 , 1 } },
{ "baro_cf_vel", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_vel, .config.minmax = { 0 , 1 } },
{ "baro_cf_alt", VAR_FLOAT | MASTER_VALUE, &barometerConfig()->baro_cf_alt, .config.minmax = { 0 , 1 } },
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &barometerConfig()->baro_hardware, .config.lookup = { TABLE_BARO_HARDWARE } },
#endif
#ifdef MAG
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &sensorSelectionConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &compassConfig()->mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &compassConfig()->mag_declination, .config.minmax = { -18000, 18000 } },
#endif
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dterm_filter_type, .config.lookup = { TABLE_LOWPASS_TYPE } },
@ -945,9 +945,9 @@ const clivalue_t valueTable[] = {
#endif
#ifdef MAG
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[X], .config.minmax = { -32768, 32767 } },
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Y], .config.minmax = { -32768, 32767 } },
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &sensorTrims()->magZero.raw[Z], .config.minmax = { -32768, 32767 } },
{ "magzero_x", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[X], .config.minmax = { -32768, 32767 } },
{ "magzero_y", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[Y], .config.minmax = { -32768, 32767 } },
{ "magzero_z", VAR_INT16 | MASTER_VALUE, &compassConfig()->magZero.raw[Z], .config.minmax = { -32768, 32767 } },
#endif
#ifdef LED_STRIP
{ "ledstrip_visual_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &ledStripConfig()->ledstrip_visual_beeper, .config.lookup = { TABLE_OFF_ON } },

6
src/main/main.c
View File

@ -422,11 +422,7 @@ void init(void)
#else
const void *sonarConfig = NULL;
#endif
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
&masterConfig.sensorSelectionConfig,
compassConfig()->mag_declination,
&masterConfig.gyroConfig,
sonarConfig)) {
if (!sensorsAutodetect(gyroConfig(), accelerometerConfig(), compassConfig(), barometerConfig(), sonarConfig)) {
// if gyro was not detected due to whatever reason, we give up now.
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()) {
performAcclerationCalibration(rollAndPitchTrims);

4
src/main/sensors/acceleration.h
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,9 @@ 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
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
flightDynamicsTrims_t accZero;
} accelerometerConfig_t;
void accInit(uint32_t gyroTargetLooptime);

1
src/main/sensors/barometer.h
View File

@ -30,6 +30,7 @@ typedef enum {
#define BARO_SAMPLE_COUNT_MAX 48
typedef struct barometerConfig_s {
uint8_t baro_hardware; // Barometer hardware to use
uint8_t baro_sample_count; // size of baro filter array
float baro_noise_lpf; // additional LPF to reduce baro noise
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)

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

@ -23,7 +23,7 @@
#include "common/maths.h"
#include "common/axis.h"
#include "sensors.h"
#include "drivers/sensor.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);
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;

4
src/main/sensors/compass.h
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,9 @@ 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
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
flightDynamicsTrims_t magZero;
} compassConfig_t;
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[Z] = gyro.dev.gyroADCRaw[Z];
alignSensors(gyroADC, gyro.gyroAlign);
alignSensors(gyroADC, gyro.dev.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete();
if (!calibrationComplete) {

2
src/main/sensors/gyro.h
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;

159
src/main/sensors/initialisation.c
View File

@ -157,21 +157,21 @@ bool fakeAccDetect(accDev_t *acc)
}
#endif
bool detectGyro(void)
bool gyroDetect(gyroDev_t *dev)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
gyro.gyroAlign = ALIGN_DEFAULT;
gyro.dev.gyroAlign = ALIGN_DEFAULT;
switch(gyroHardware) {
case GYRO_DEFAULT:
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(&gyro.dev)) {
if (mpu6050GyroDetect(dev)) {
gyroHardware = GYRO_MPU6050;
#ifdef GYRO_MPU6050_ALIGN
gyro.gyroAlign = GYRO_MPU6050_ALIGN;
gyro.dev.gyroAlign = GYRO_MPU6050_ALIGN;
#endif
break;
}
@ -179,10 +179,10 @@ bool detectGyro(void)
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro.dev)) {
if (l3g4200dDetect(dev)) {
gyroHardware = GYRO_L3G4200D;
#ifdef GYRO_L3G4200D_ALIGN
gyro.gyroAlign = GYRO_L3G4200D_ALIGN;
gyro.dev.gyroAlign = GYRO_L3G4200D_ALIGN;
#endif
break;
}
@ -191,10 +191,10 @@ bool detectGyro(void)
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro.dev)) {
if (mpu3050Detect(dev)) {
gyroHardware = GYRO_MPU3050;
#ifdef GYRO_MPU3050_ALIGN
gyro.gyroAlign = GYRO_MPU3050_ALIGN;
gyro.dev.gyroAlign = GYRO_MPU3050_ALIGN;
#endif
break;
}
@ -203,10 +203,10 @@ bool detectGyro(void)
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro.dev)) {
if (l3gd20Detect(dev)) {
gyroHardware = GYRO_L3GD20;
#ifdef GYRO_L3GD20_ALIGN
gyro.gyroAlign = GYRO_L3GD20_ALIGN;
gyro.dev.gyroAlign = GYRO_L3GD20_ALIGN;
#endif
break;
}
@ -215,10 +215,10 @@ bool detectGyro(void)
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro.dev)) {
if (mpu6000SpiGyroDetect(dev)) {
gyroHardware = GYRO_MPU6000;
#ifdef GYRO_MPU6000_ALIGN
gyro.gyroAlign = GYRO_MPU6000_ALIGN;
gyro.dev.gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
@ -228,14 +228,14 @@ bool detectGyro(void)
case GYRO_MPU6500:
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500GyroDetect(&gyro.dev) || mpu6500SpiGyroDetect(&gyro.dev))
if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev))
#else
if (mpu6500GyroDetect(&gyro.dev))
if (mpu6500GyroDetect(dev))
#endif
{
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
gyro.gyroAlign = GYRO_MPU6500_ALIGN;
gyro.dev.gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
@ -246,11 +246,11 @@ bool detectGyro(void)
case GYRO_MPU9250:
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiGyroDetect(&gyro.dev))
if (mpu9250SpiGyroDetect(dev))
{
gyroHardware = GYRO_MPU9250;
#ifdef GYRO_MPU9250_ALIGN
gyro.gyroAlign = GYRO_MPU9250_ALIGN;
gyro.dev.gyroAlign = GYRO_MPU9250_ALIGN;
#endif
break;
@ -260,11 +260,11 @@ bool detectGyro(void)
case GYRO_ICM20689:
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiGyroDetect(&gyro.dev))
if (icm20689SpiGyroDetect(dev))
{
gyroHardware = GYRO_ICM20689;
#ifdef GYRO_ICM20689_ALIGN
gyro.gyroAlign = GYRO_ICM20689_ALIGN;
gyro.dev.gyroAlign = GYRO_ICM20689_ALIGN;
#endif
break;
@ -274,7 +274,7 @@ bool detectGyro(void)
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro.dev)) {
if (fakeGyroDetect(dev)) {
gyroHardware = GYRO_FAKE;
break;
}
@ -294,7 +294,7 @@ bool detectGyro(void)
return true;
}
static bool detectAcc(accelerationSensor_e accHardwareToUse)
static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware;
@ -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:
@ -313,12 +313,12 @@ retry:
acc_params.useFifo = false;
acc_params.dataRate = 800; // unused currently
#ifdef NAZE
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc.dev)) {
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, dev)) {
#else
if (adxl345Detect(&acc_params, &acc.dev)) {
if (adxl345Detect(&acc_params, dev)) {
#endif
#ifdef ACC_ADXL345_ALIGN
acc.accAlign = ACC_ADXL345_ALIGN;
acc.dev.accAlign = ACC_ADXL345_ALIGN;
#endif
accHardware = ACC_ADXL345;
break;
@ -327,9 +327,9 @@ retry:
; // fallthrough
case ACC_LSM303DLHC:
#ifdef USE_ACC_LSM303DLHC
if (lsm303dlhcAccDetect(&acc.dev)) {
if (lsm303dlhcAccDetect(dev)) {
#ifdef ACC_LSM303DLHC_ALIGN
acc.accAlign = ACC_LSM303DLHC_ALIGN;
acc.dev.accAlign = ACC_LSM303DLHC_ALIGN;
#endif
accHardware = ACC_LSM303DLHC;
break;
@ -338,9 +338,9 @@ retry:
; // fallthrough
case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(&acc.dev)) {
if (mpu6050AccDetect(dev)) {
#ifdef ACC_MPU6050_ALIGN
acc.accAlign = ACC_MPU6050_ALIGN;
acc.dev.accAlign = ACC_MPU6050_ALIGN;
#endif
accHardware = ACC_MPU6050;
break;
@ -351,12 +351,12 @@ retry:
#ifdef USE_ACC_MMA8452
#ifdef NAZE
// Not supported with this frequency
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc.dev)) {
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) {
#else
if (mma8452Detect(&acc.dev)) {
if (mma8452Detect(dev)) {
#endif
#ifdef ACC_MMA8452_ALIGN
acc.accAlign = ACC_MMA8452_ALIGN;
acc.dev.accAlign = ACC_MMA8452_ALIGN;
#endif
accHardware = ACC_MMA8452;
break;
@ -365,9 +365,9 @@ retry:
; // fallthrough
case ACC_BMA280: // BMA280
#ifdef USE_ACC_BMA280
if (bma280Detect(&acc.dev)) {
if (bma280Detect(dev)) {
#ifdef ACC_BMA280_ALIGN
acc.accAlign = ACC_BMA280_ALIGN;
acc.dev.accAlign = ACC_BMA280_ALIGN;
#endif
accHardware = ACC_BMA280;
break;
@ -376,9 +376,9 @@ retry:
; // fallthrough
case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc.dev)) {
if (mpu6000SpiAccDetect(dev)) {
#ifdef ACC_MPU6000_ALIGN
acc.accAlign = ACC_MPU6000_ALIGN;
acc.dev.accAlign = ACC_MPU6000_ALIGN;
#endif
accHardware = ACC_MPU6000;
break;
@ -388,13 +388,13 @@ retry:
case ACC_MPU6500:
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
#ifdef USE_ACC_SPI_MPU6500
if (mpu6500AccDetect(&acc.dev) || mpu6500SpiAccDetect(&acc.dev))
if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev))
#else
if (mpu6500AccDetect(&acc.dev))
if (mpu6500AccDetect(dev))
#endif
{
#ifdef ACC_MPU6500_ALIGN
acc.accAlign = ACC_MPU6500_ALIGN;
acc.dev.accAlign = ACC_MPU6500_ALIGN;
#endif
accHardware = ACC_MPU6500;
break;
@ -404,10 +404,10 @@ retry:
case ACC_ICM20689:
#ifdef USE_ACC_SPI_ICM20689
if (icm20689SpiAccDetect(&acc.dev))
if (icm20689SpiAccDetect(dev))
{
#ifdef ACC_ICM20689_ALIGN
acc.accAlign = ACC_ICM20689_ALIGN;
acc.dev.accAlign = ACC_ICM20689_ALIGN;
#endif
accHardware = ACC_ICM20689;
break;
@ -416,7 +416,7 @@ retry:
; // fallthrough
case ACC_FAKE:
#ifdef USE_FAKE_ACC
if (fakeAccDetect(&acc.dev)) {
if (fakeAccDetect(dev)) {
accHardware = ACC_FAKE;
break;
}
@ -446,7 +446,7 @@ retry:
}
#ifdef BARO
static bool detectBaro(baroSensor_e baroHardwareToUse)
static bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
{
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
@ -476,7 +476,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough
case BARO_BMP085:
#ifdef USE_BARO_BMP085
if (bmp085Detect(bmp085Config, &baro.dev)) {
if (bmp085Detect(bmp085Config, dev)) {
baroHardware = BARO_BMP085;
break;
}
@ -484,7 +484,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough
case BARO_MS5611:
#ifdef USE_BARO_MS5611
if (ms5611Detect(&baro.dev)) {
if (ms5611Detect(dev)) {
baroHardware = BARO_MS5611;
break;
}
@ -492,7 +492,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
; // fallthough
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(&baro.dev)) {
if (bmp280Detect(dev)) {
baroHardware = BARO_BMP280;
break;
}
@ -514,7 +514,7 @@ static bool detectBaro(baroSensor_e baroHardwareToUse)
#endif
#ifdef MAG
static bool detectMag(magSensor_e magHardwareToUse)
static bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
{
magSensor_e magHardware;
@ -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:
@ -555,9 +555,9 @@ retry:
case MAG_HMC5883:
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(&mag.dev, hmc5883Config)) {
if (hmc5883lDetect(dev, hmc5883Config)) {
#ifdef MAG_HMC5883_ALIGN
mag.magAlign = MAG_HMC5883_ALIGN;
mag.dev.magAlign = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
break;
@ -567,9 +567,9 @@ retry:
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (ak8975Detect(&mag.dev)) {
if (ak8975Detect(dev)) {
#ifdef MAG_AK8975_ALIGN
mag.magAlign = MAG_AK8975_ALIGN;
mag.dev.magAlign = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
break;
@ -579,9 +579,9 @@ retry:
case MAG_AK8963:
#ifdef USE_MAG_AK8963
if (ak8963Detect(&mag.dev)) {
if (ak8963Detect(dev)) {
#ifdef MAG_AK8963_ALIGN
mag.magAlign = MAG_AK8963_ALIGN;
mag.dev.magAlign = MAG_AK8963_ALIGN;
#endif
magHardware = MAG_AK8963;
break;
@ -611,7 +611,7 @@ retry:
#endif
#ifdef SONAR
static bool detectSonar(void)
static bool sonarDetect(void)
{
if (feature(FEATURE_SONAR)) {
// the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
@ -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,
const gyroConfig_t *gyroConfig,
bool sensorsAutodetect(const gyroConfig_t *gyroConfig,
const accelerometerConfig_t *accelerometerConfig,
const compassConfig_t *compassConfig,
const barometerConfig_t *barometerConfig,
const sonarConfig_t *sonarConfig)
{
memset(&acc, 0, sizeof(acc));
@ -653,7 +640,7 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
if (!gyroDetect(&gyro.dev)) {
return false;
}
@ -664,7 +651,7 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
gyro.dev.init(&gyro.dev); // driver initialisation
gyroInit(gyroConfig); // sensor initialisation
if (detectAcc(sensorSelectionConfig->acc_hardware)) {
if (accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
acc.dev.acc_1G = 256; // set default
acc.dev.init(&acc.dev); // driver initialisation
accInit(gyro.targetLooptime); // sensor initialisation
@ -674,30 +661,40 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
mag.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.
#ifdef MAG
// FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
if (detectMag(sensorSelectionConfig->mag_hardware)) {
if (compassDetect(&mag.dev, compassConfig->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
detectBaro(sensorSelectionConfig->baro_hardware);
baroDetect(&baro.dev, barometerConfig->baro_hardware);
#else
UNUSED(barometerConfig);
#endif
#ifdef SONAR
if (detectSonar()) {
if (sonarDetect()) {
sonarInit(sonarConfig);
}
#else
UNUSED(sonarConfig);
#endif
reconfigureAlignment(sensorAlignmentConfig);
if (gyroConfig->gyro_align != ALIGN_DEFAULT) {
gyro.dev.gyroAlign = gyroConfig->gyro_align;
}
if (accelerometerConfig->acc_align != ALIGN_DEFAULT) {
acc.dev.accAlign = accelerometerConfig->acc_align;
}
if (compassConfig->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign = compassConfig->mag_align;
}
return true;
}

14
src/main/sensors/initialisation.h
View File

@ -17,12 +17,8 @@
#pragma once
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 gyroConfig_t *gyroConfig,
const accelerometerConfig_t *accConfig,
const compassConfig_t *compassConfig,
const barometerConfig_t *baroConfig,
const sonarConfig_t *sonarConfig);

29
src/main/sensors/sensors.h
View File

@ -51,32 +51,3 @@ typedef enum {
SENSOR_GPS = 1 << 5,
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
uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device
} sensorSelectionConfig_t;
typedef struct sensorTrims_s {
flightDynamicsTrims_t accZero;
flightDynamicsTrims_t magZero;
} sensorTrims_t;

2
src/main/target/ALIENFLIGHTF3/config.c
View File

@ -47,7 +47,7 @@ void targetConfiguration(master_t *config)
{
config->rxConfig.spektrum_sat_bind = 5;
config->rxConfig.spektrum_sat_bind_autoreset = 1;
config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default
config->compassConfig.mag_hardware = MAG_NONE; // disabled by default
if (hardwareMotorType == MOTOR_BRUSHED) {
config->motorConfig.minthrottle = 1000;

2
src/main/target/ALIENFLIGHTF4/config.c
View File

@ -58,7 +58,7 @@ void targetConfiguration(master_t *config)
{
config->batteryConfig.currentMeterOffset = CURRENTOFFSET;
config->batteryConfig.currentMeterScale = CURRENTSCALE;
config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default
config->compassConfig.mag_hardware = MAG_NONE; // disabled by default
if (hardwareMotorType == MOTOR_BRUSHED) {
config->motorConfig.minthrottle = 1000;

4
src/main/target/BLUEJAYF4/config.c
View File

@ -31,8 +31,8 @@
void targetConfiguration(master_t *config)
{
if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) {
config->sensorAlignmentConfig.gyro_align = CW180_DEG;
config->sensorAlignmentConfig.acc_align = CW180_DEG;
config->gyroConfig.gyro_align = CW180_DEG;
config->accelerometerConfig.acc_align = CW180_DEG;
config->beeperConfig.ioTag = IO_TAG(BEEPER_OPT);
}

2
src/main/target/COLIBRI/config.c
View File

@ -48,7 +48,7 @@ void targetConfiguration(master_t *config)
config->boardAlignment.pitchDegrees = 10;
//config->rcControlsConfig.deadband = 10;
//config->rcControlsConfig.yaw_deadband = 10;
config->sensorSelectionConfig.mag_hardware = 1;
config->compassConfig.mag_hardware = 1;
config->profile[0].controlRateProfile[0].dynThrPID = 45;
config->profile[0].controlRateProfile[0].tpa_breakpoint = 1700;

5
src/main/target/MULTIFLITEPICO/config.c
View File

@ -40,8 +40,9 @@
#include "config/config_master.h"
// alternative defaults settings for MULTIFLITEPICO targets
void targetConfiguration(master_t *config) {
config->sensorSelectionConfig.mag_hardware = MAG_NONE; // disabled by default
void targetConfiguration(master_t *config)
{
config->compassConfig.mag_hardware = MAG_NONE; // disabled by default
config->batteryConfig.vbatscale = 100;
config->batteryConfig.vbatresdivval = 15;

1
src/test/unit/alignsensor_unittest.cc
View File

@ -21,6 +21,7 @@
extern "C" {
#include "common/axis.h"
#include "drivers/sensor.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
}