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

Split gyro device out of gyro sensor struct

This commit is contained in:
Martin Budden 2017-02-05 22:24:13 +00:00
parent 9a8124ffc4
commit f2b423525d
16 changed files with 168 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/drivers/accgyro_fake.c
View File

@ -20,6 +20,8 @@
#include "platform.h"
#ifdef USE_FAKE_GYRO
#include "common/axis.h"
#include "common/utils.h"
@ -27,8 +29,6 @@
#include "accgyro_fake.h"
#ifdef USE_FAKE_GYRO
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
static void fakeGyroInit(gyroDev_t *gyro)

5
src/main/fc/fc_core.c
View File

@ -93,7 +93,6 @@ int16_t headFreeModeHold;
uint8_t motorControlEnable = false;
int16_t telemTemperature1; // gyro sensor temperature
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
bool isRXDataNew;
@ -470,8 +469,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// Read out gyro temperature if used for telemmetry
if (feature(FEATURE_TELEMETRY) && gyro.dev.temperature) {
gyro.dev.temperature(&gyro.dev, &telemTemperature1);
if (feature(FEATURE_TELEMETRY)) {
gyroReadTemperature();
}
#ifdef MAG

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

@ -664,7 +664,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, acc.accSmooth[i] / scale);
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, lrintf(gyro.gyroADCf[i] / gyro.dev.scale));
sbufWriteU16(dst, gyroRateDps(i));
}
for (int i = 0; i < 3; i++) {
sbufWriteU16(dst, mag.magADC[i]);

13
src/main/sensors/acceleration.c
View File

@ -35,18 +35,18 @@
#include "drivers/accgyro_bma280.h"
#include "drivers/accgyro_fake.h"
#include "drivers/accgyro_l3g4200d.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_mpu6500.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
#include "drivers/bus_spi.h"
#include "drivers/accgyro_spi_icm20689.h"
#include "drivers/accgyro_spi_mpu6000.h"
#include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/accgyro_spi_mpu9250.h"
#include "drivers/bus_spi.h"
#include "drivers/system.h"
#include "fc/config.h"
@ -250,8 +250,8 @@ bool accInit(uint32_t gyroSamplingInverval)
{
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration;
acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult;
acc.dev.mpuConfiguration = *gyroMpuConfiguration();
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
return false;
}
@ -278,6 +278,9 @@ bool accInit(uint32_t gyroSamplingInverval)
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, acc.accSamplingInterval);
}
}
if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
acc.dev.accAlign = accelerometerConfig()->acc_align;
}
return true;
}

17
src/main/sensors/compass.c
View File

@ -44,6 +44,7 @@
#include "hardware_revision.h"
#endif
magDev_t magDev;
mag_t mag; // mag access functions
#ifdef MAG
@ -146,17 +147,25 @@ retry:
return true;
}
void compassInit(void)
bool compassInit(void)
{
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
// calculate magnetic declination
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.
if (!compassDetect(&magDev, compassConfig()->mag_hardware)) {
return false;
}
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
LED1_ON;
mag.dev.init();
magDev.init();
LED1_OFF;
magInit = 1;
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
magDev.magAlign = compassConfig()->mag_align;
}
return true;
}
void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
@ -165,11 +174,11 @@ void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
mag.dev.read(magADCRaw);
magDev.read(magADCRaw);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] = magADCRaw[axis];
}
alignSensors(mag.magADC, mag.dev.magAlign);
alignSensors(mag.magADC, magDev.magAlign);
if (STATE(CALIBRATE_MAG)) {
tCal = currentTime;

6
src/main/sensors/compass.h
View File

@ -18,7 +18,7 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/compass.h"
#include "drivers/sensor.h"
#include "sensors/sensors.h"
@ -32,7 +32,6 @@ typedef enum {
} magSensor_e;
typedef struct mag_s {
magDev_t dev;
int32_t magADC[XYZ_AXIS_COUNT];
float magneticDeclination;
} mag_t;
@ -49,8 +48,7 @@ typedef struct compassConfig_s {
PG_DECLARE(compassConfig_t, compassConfig);
bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse);
void compassInit(void);
bool compassInit(void);
union flightDynamicsTrims_u;
void compassUpdate(uint32_t currentTime, union flightDynamicsTrims_u *magZero);

107
src/main/sensors/gyro.c
View File

@ -36,13 +36,13 @@
#include "drivers/accgyro_bma280.h"
#include "drivers/accgyro_fake.h"
#include "drivers/accgyro_l3g4200d.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_mpu6500.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
#include "drivers/accgyro_spi_icm20689.h"
#include "drivers/accgyro_spi_mpu6000.h"
#include "drivers/accgyro_spi_mpu6500.h"
@ -59,19 +59,22 @@
#include "scheduler/scheduler.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
#endif
gyro_t gyro; // gyro access functions
gyro_t gyro;
STATIC_UNIT_TESTED gyroDev_t gyroDev0;
static int16_t gyroTemperature0;
static int32_t gyroADC[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static uint16_t calibratingG = 0;
static filterApplyFnPtr softLpfFilterApplyFn;
@ -106,6 +109,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyroMovementCalibrationThreshold = 32
);
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
static const extiConfig_t *selectMPUIntExtiConfig(void)
{
#if defined(MPU_INT_EXTI)
@ -117,8 +121,18 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
return NULL;
#endif
}
#endif
static bool gyroDetect(gyroDev_t *dev)
const mpuConfiguration_t *gyroMpuConfiguration(void)
{
return &gyroDev0.mpuConfiguration;
}
const mpuDetectionResult_t *gyroMpuDetectionResult(void)
{
return &gyroDev0.mpuDetectionResult;
}
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
@ -246,34 +260,29 @@ static bool gyroDetect(gyroDev_t *dev)
gyroHardware = GYRO_NONE;
}
if (gyroHardware == GYRO_NONE) {
return false;
if (gyroHardware != GYRO_NONE) {
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
}
detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
sensorsSet(SENSOR_GYRO);
return true;
return gyroHardware;
}
bool gyroInit(void)
{
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig();
mpuDetect(&gyro.dev);
mpuResetFn = gyro.dev.mpuConfiguration.resetFn;
gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig();
mpuDetect(&gyroDev0);
mpuResetFn = gyroDev0.mpuConfiguration.resetFn;
#endif
if (!gyroDetect(&gyro.dev)) {
const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0);
if (gyroHardware == GYRO_NONE) {
return false;
}
switch (detectedSensors[SENSOR_INDEX_GYRO]) {
default:
// gyro does not support 32kHz
gyroConfigMutable()->gyro_use_32khz = false;
break;
switch (gyroHardware) {
case GYRO_MPU6500:
case GYRO_MPU9250:
case GYRO_ICM20689:
@ -281,12 +290,19 @@ bool gyroInit(void)
case GYRO_ICM20602:
// do nothing, as gyro supports 32kHz
break;
default:
// gyro does not support 32kHz
gyroConfigMutable()->gyro_use_32khz = false;
break;
}
// Must set gyro sample rate before initialisation
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
gyro.dev.lpf = gyroConfig()->gyro_lpf;
gyro.dev.init(&gyro.dev);
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
gyroDev0.lpf = gyroConfig()->gyro_lpf;
gyroDev0.init(&gyroDev0);
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
gyroDev0.gyroAlign = gyroConfig()->gyro_align;
}
gyroInitFilters();
return true;
}
@ -369,7 +385,7 @@ void gyroSetCalibrationCycles(void)
calibratingG = gyroCalculateCalibratingCycles();
}
static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
{
static int32_t g[3];
static stdev_t var[3];
@ -445,27 +461,27 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (gyro.dev.update) {
if (gyroDev0.update) {
// if the gyro update function is set then return, since the gyro is read in gyroUpdateISR
return;
}
if (!gyro.dev.read(&gyro.dev)) {
if (!gyroDev0.read(&gyroDev0)) {
return;
}
gyro.dev.dataReady = false;
gyroDev0.dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroADC[X] = gyro.dev.gyroADCRaw[X];
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
gyroADC[X] = gyroDev0.gyroADCRaw[X];
gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
alignSensors(gyroADC, gyro.dev.gyroAlign);
alignSensors(gyroADC, gyroDev0.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete();
if (calibrationComplete) {
#if defined(GYRO_USES_SPI) && defined(USE_MPU_DATA_READY_SIGNAL)
// SPI-based gyro so can read and update in ISR
if (gyroConfig()->gyro_isr_update) {
mpuGyroSetIsrUpdate(&gyro.dev, gyroUpdateISR);
mpuGyroSetIsrUpdate(&gyroDev0, gyroUpdateISR);
return;
}
#endif
@ -479,7 +495,7 @@ void gyroUpdate(void)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
// scale gyro output to degrees per second
float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
// Apply LPF
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
@ -493,8 +509,25 @@ void gyroUpdate(void)
}
if (!calibrationComplete) {
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyro.dev.scale);
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyro.dev.scale);
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyro.dev.scale);
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
}
}
void gyroReadTemperature(void)
{
if (gyroDev0.temperature) {
gyroDev0.temperature(&gyroDev0, &gyroTemperature0);
}
}
int16_t gyroGetTemperature(void)
{
return gyroTemperature0;
}
int16_t gyroRateDps(int axis)
{
return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale);
}

9
src/main/sensors/gyro.h
View File

@ -18,7 +18,6 @@
#pragma once
#include "config/parameter_group.h"
#include "drivers/accgyro.h"
#include "common/axis.h"
typedef enum {
@ -38,7 +37,6 @@ typedef enum {
} gyroSensor_e;
typedef struct gyro_s {
gyroDev_t dev;
uint32_t targetLooptime;
float gyroADCf[XYZ_AXIS_COUNT];
} gyro_t;
@ -62,8 +60,13 @@ typedef struct gyroConfig_s {
PG_DECLARE(gyroConfig_t, gyroConfig);
void gyroSetCalibrationCycles(void);
bool gyroInit(void);
void gyroInitFilters(void);
void gyroUpdate(void);
const mpuConfiguration_t *gyroMpuConfiguration(void);
const mpuDetectionResult_t *gyroMpuDetectionResult(void);
void gyroSetCalibrationCycles(void);
bool isGyroCalibrationComplete(void);
void gyroReadTemperature(void);
int16_t gyroGetTemperature(void);
int16_t gyroRateDps(int axis);

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

@ -63,11 +63,8 @@ bool sensorsAutodetect(void)
accInit(gyro.targetLooptime);
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
if (compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
compassInit();
}
compassInit();
#endif
#ifdef BARO
@ -80,15 +77,5 @@ bool sensorsAutodetect(void)
}
#endif
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;
}

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

@ -25,7 +25,6 @@ typedef enum {
SENSOR_INDEX_COUNT
} sensorIndex_e;
extern int16_t telemTemperature1; //FIXME move to temp sensor...?
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
typedef struct int16_flightDynamicsTrims_s {

2
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -613,7 +613,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
bstWrite16(acc.accSmooth[i] / scale);
}
for (i = 0; i < 3; i++) {
bstWrite16(lrintf(gyro.gyroADCf[i] /gyro.dev.scale));
bstWrite16(gyroRateDps(i));
}
for (i = 0; i < 3; i++) {
bstWrite16(mag.magADC[i]);

2
src/main/telemetry/frsky.c
View File

@ -223,7 +223,7 @@ static void sendTemperature1(void)
#elif defined(BARO)
serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf
#else
serialize16(telemTemperature1 / 10);
serialize16(gyroGetTemperature() / 10);
#endif
}

2
src/main/telemetry/ibus.c
View File

@ -300,7 +300,7 @@ static void dispatchMeasurementReply(ibusAddress_t address)
#ifdef BARO
value = (baro.baroTemperature + 5) / 10; // +5 to make integer division rounding correct
#else
value = telemTemperature1 * 10;
value = gyroGetTemperature() * 10;
#endif
sendIbusMeasurement(address, value + IBUS_TEMPERATURE_OFFSET);
break;

54
src/test/Makefile
View File

@ -675,6 +675,60 @@ $(OBJECT_DIR)/alignsensor_unittest : \
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/build/debug.o : \
$(USER_DIR)/build/debug.c \
$(USER_DIR)/build/debug.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/build/debug.c -o $@
$(OBJECT_DIR)/drivers/gyro_sync.o : \
$(USER_DIR)/drivers/gyro_sync.c \
$(USER_DIR)/drivers/gyro_sync.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/gyro_sync.c -o $@
$(OBJECT_DIR)/drivers/accgyro_fake.o : \
$(USER_DIR)/drivers/accgyro_fake.c \
$(USER_DIR)/drivers/accgyro_fake.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/accgyro_fake.c -o $@
$(OBJECT_DIR)/sensors/gyro.o : \
$(USER_DIR)/sensors/gyro.c \
$(USER_DIR)/sensors/gyro.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/gyro.c -o $@
$(OBJECT_DIR)/sensor_gyro_unittest.o : \
$(TEST_DIR)/sensor_gyro_unittest.cc \
$(USER_DIR)/sensors/gyro.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/sensor_gyro_unittest.cc -o $@
$(OBJECT_DIR)/sensor_gyro_unittest : \
$(OBJECT_DIR)/build/debug.o \
$(OBJECT_DIR)/common/filter.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/drivers/accgyro_fake.o \
$(OBJECT_DIR)/drivers/gyro_sync.o \
$(OBJECT_DIR)/sensors/boardalignment.o \
$(OBJECT_DIR)/sensors/gyro.o \
$(OBJECT_DIR)/sensor_gyro_unittest.o \
$(OBJECT_DIR)/gtest_main.a
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/build/version.o : \
$(USER_DIR)/build/version.c \
$(USER_DIR)/build/version.h \

5
src/test/unit/platform.h
View File

@ -65,6 +65,11 @@ uint8_t DMA_GetFlagStatus(void *);
void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState );
void DMA_ClearFlag(uint32_t);
typedef struct
{
void* test;
} SPI_TypeDef;
typedef struct
{
void* test;

1
src/test/unit/target.h
View File

@ -19,6 +19,7 @@
#define CMS
#define CMS_MAX_DEVICE 4
#define USE_FAKE_GYRO
#define MAG
#define BARO
#define GPS