Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Removed gyroDev_t from gyro_t

Browse source
This commit is contained in:
Martin Budden 2017-03-13 19:01:53 +00:00
parent b3ee1409e8
commit 5b66844cbb
9 changed files with 90 additions and 44 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/gyro_sync.c
View file

@ -24,7 +24,7 @@
#include "accgyro.h"
#include "gyro_sync.h"
bool gyroSyncCheckUpdate(gyroDev_t *gyro)
bool gyroSyncCheckIntStatus(gyroDev_t *gyro)
{
if (!gyro->intStatus)
return false;

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

@ -17,6 +17,6 @@
#include "drivers/accgyro.h"
bool gyroSyncCheckUpdate(gyroDev_t *gyro);
bool gyroSyncCheckIntStatus(gyroDev_t *gyro);
uint8_t gyroMPU6xxxGetDividerDrops(const gyroDev_t *gyro);
uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);

3
src/main/fc/cli.c
View file

@ -516,6 +516,9 @@ static const clivalue_t valueTable[] = {
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = {0, 500 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = {1, 500 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
#endif
#ifdef USE_DUAL_GYRO
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
#endif
// PG_ACCELEROMETER_CONFIG
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },

7
src/main/fc/fc_core.c
View file

@ -220,11 +220,6 @@ void annexCode(void)
warningLedUpdate();
}
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.dev.temperature) {
gyro.dev.temperature(&gyro.dev, &telemTemperature1);
}
}
void mwDisarm(void)
@ -563,7 +558,7 @@ void taskGyro(timeUs_t currentTimeUs) {
if (gyroConfig()->gyroSync) {
while (true) {
if (gyroSyncCheckUpdate(&gyro.dev) || ((currentDeltaTime + cmpTimeUs(micros(), currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(micros(), currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
break;
}
}

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

@ -586,7 +586,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU16(dst, acc.accADC[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]);

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

@ -275,8 +275,8 @@ bool accInit(uint32_t targetLooptime)
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
acc.dev.bus = *gyroSensorBus();
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;
}

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

@ -70,6 +70,9 @@
gyro_t gyro; // gyro sensor object
static gyroDev_t gyroDev0;
static int16_t gyroTemperature0;
typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT];
stdev_t var[XYZ_AXIS_COUNT];
@ -101,6 +104,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.looptime = 2000,
.gyroSync = 0,
.gyroSyncDenominator = 2,
.gyro_to_use = 0,
.gyro_soft_notch_hz_1 = 0,
.gyro_soft_notch_cutoff_1 = 1,
.gyro_soft_notch_hz_2 = 0,
@ -117,6 +121,10 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
#if defined(MPU_INT_EXTI)
static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) };
return &mpuIntExtiConfig;
#elif defined(USE_DUAL_GYRO)
static extiConfig_t mpuIntExtiConfig;
mpuIntExtiConfig.tag = gyroConfig()->gyro_to_use == 0 ? IO_TAG(GYRO_0_INT_EXTI) : IO_TAG(GYRO_1_INT_EXTI);
return &mpuIntExtiConfig;
#elif defined(USE_HARDWARE_REVISION_DETECTION)
return selectMPUIntExtiConfigByHardwareRevision();
#else
@ -130,7 +138,16 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
const busDevice_t *gyroSensorBus(void)
{
return &gyro.dev.bus;
return &gyroDev0.bus;
}
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)
@ -257,23 +274,29 @@ bool gyroInit(void)
memset(&gyro, 0, sizeof(gyro));
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
#ifdef USE_GYRO_MPU
mpuDetect(&gyro.dev);
mpuResetFn = gyro.dev.mpuConfiguration.resetFn;
#ifdef USE_DUAL_GYRO
// set cnsPin using GYRO_n_CS_PIN defined in target.h
gyroDev0.bus.spi.csnPin = gyroConfig()->gyro_to_use == 0 ? IOGetByTag(IO_TAG(GYRO_0_CS_PIN)) : IOGetByTag(IO_TAG(GYRO_1_CS_PIN));
#else
gyroDev0.bus.spi.csnPin = IO_NONE; // set cnsPin to IO_NONE so mpuDetect will set it according to value defined in target.h
#endif // USE_DUAL_GYRO
mpuDetect(&gyroDev0);
mpuResetFn = gyroDev0.mpuConfiguration.resetFn;
#endif
gyro.dev.mpuIntExtiConfig = extiConfig;
if (gyroDetect(&gyro.dev, GYRO_AUTODETECT) == GYRO_NONE) {
gyroDev0.mpuIntExtiConfig = extiConfig;
if (gyroDetect(&gyroDev0, GYRO_AUTODETECT) == GYRO_NONE) {
return false;
}
// After refactoring this function is always called after gyro sampling rate is known, so
// no additional condition is required
// Set gyro sample rate before driver initialisation
gyro.dev.lpf = gyroConfig()->gyro_lpf;
gyro.targetLooptime = gyroSetSampleRate(&gyro.dev, gyroConfig()->looptime, gyroConfig()->gyro_lpf, gyroConfig()->gyroSync, gyroConfig()->gyroSyncDenominator);
gyroDev0.lpf = gyroConfig()->gyro_lpf;
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->looptime, gyroConfig()->gyro_lpf, gyroConfig()->gyroSync, gyroConfig()->gyroSyncDenominator);
// driver initialisation
gyro.dev.init(&gyro.dev);
gyroDev0.init(&gyroDev0);
if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
gyro.dev.gyroAlign = gyroConfig()->gyro_align;
gyroDev0.gyroAlign = gyroConfig()->gyro_align;
}
gyroInitFilters();
return true;
@ -384,15 +407,15 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (gyro.dev.read(&gyro.dev)) {
if (gyroDev0.read(&gyroDev0)) {
if (isCalibrationComplete(&gyroCalibration)) {
// Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
gyroADC[X] = (int32_t)gyro.dev.gyroADCRaw[X] - (int32_t)gyro.dev.gyroZero[X];
gyroADC[Y] = (int32_t)gyro.dev.gyroADCRaw[Y] - (int32_t)gyro.dev.gyroZero[Y];
gyroADC[Z] = (int32_t)gyro.dev.gyroADCRaw[Z] - (int32_t)gyro.dev.gyroZero[Z];
alignSensors(gyroADC, gyro.dev.gyroAlign);
gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z];
alignSensors(gyroADC, gyroDev0.gyroAlign);
} else {
performGyroCalibration(&gyro.dev, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
// Reset gyro values to zero to prevent other code from using uncalibrated data
gyro.gyroADCf[X] = 0.0f;
gyro.gyroADCf[Y] = 0.0f;
@ -406,7 +429,7 @@ void gyroUpdate(void)
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float gyroADCf = (float)gyroADC[axis] * gyro.dev.scale;
float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
@ -423,3 +446,25 @@ void gyroUpdate(void)
gyro.gyroADCf[axis] = gyroADCf;
}
}
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);
}
bool gyroSyncCheckUpdate(void)
{
return gyroSyncCheckIntStatus(&gyroDev0);
}

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

@ -36,7 +36,6 @@ typedef enum {
} gyroSensor_e;
typedef struct gyro_s {
gyroDev_t dev;
uint32_t targetLooptime;
float gyroADCf[XYZ_AXIS_COUNT];
} gyro_t;
@ -51,6 +50,7 @@ typedef struct gyroConfig_s {
uint16_t looptime; // imu loop time in us
uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_soft_lpf_hz;
uint8_t gyro_to_use;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
@ -63,5 +63,13 @@ bool gyroInit(void);
void gyroInitFilters(void);
void gyroUpdate(void);
const busDevice_t *gyroSensorBus(void);
struct mpuConfiguration_s;
const struct mpuConfiguration_s *gyroMpuConfiguration(void);
struct mpuDetectionResult_s;
const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
bool gyroIsCalibrationComplete(void);
void gyroReadTemperature(void);
int16_t gyroGetTemperature(void);
int16_t gyroRateDps(int axis);
bool gyroSyncCheckUpdate(void);

27
src/main/target/PIXRACER/target.h
View file

@ -44,22 +44,17 @@
#define GYRO_MPU6500_ALIGN CW180_DEG_FLIP
#define ACC_MPU6500_ALIGN CW180_DEG_FLIP
#ifdef PIXRACER_ICM20608
// Variant that uses ICM20608 gyro/acc
#define ICM20608_CS_PIN PC15
#define MPU6500_CS_PIN PC15
#define MPU6500_SPI_INSTANCE SPI1
#define MPU_INT_EXTI PC14 // ICM20608
#else
// Variant to use MPU9250 gyro/acc/mag
#define USE_MAG_AK8963
#define MPU6500_CS_PIN PC2
#define MPU6500_SPI_INSTANCE SPI1
#define MPU_INT_EXTI PD15 // MPU9250
#endif
#define USE_DUAL_GYRO
#define ICM20608_CS_PIN PC15
#define ICM20608_SPI_INSTANCE SPI1
#define GYRO_0_CS_PIN ICM20608_CS_PIN
#define GYRO_0_INT_EXTI PC14
// MPU9250 gyro/acc/mag
#define USE_MAG_AK8963
#define MPU6500_CS_PIN PC2
#define GYRO_1_CS_PIN MPU6500_CS_PIN
#define MPU6500_SPI_INSTANCE SPI1
#define GYRO_1_INT_EXTI PD15
#define ACC
#define GYRO