[go: nahoru, domu]
Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Better Gyro Fusion #784

Draft
wants to merge 4 commits into
base: emuflight-1.0.0-master
Choose a base branch
from
Draft

Better Gyro Fusion #784

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
22c7f07
Better way of fusing gyros together using variance.
Quick-Flash Feb 28, 2022
f8dc7e0
rebase off of hydra branch
Quick-Flash Jan 21, 2022
d9bc0d4
reduce cpu and punish high error more
Quick-Flash Feb 28, 2022
4cf2e5c
Update gyro.c
Quick-Flash Feb 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Next Next commit
Better way of fusing gyros together using variance.
  • Loading branch information
@Quick-Flash @nerdCopter
Quick-Flash authored and nerdCopter committed Nov 22, 2022
commit 22c7f0702ebf102ee0714c61945181dad2f0b7be
4 changes: 2 additions & 2 deletions src/main/cli/cli.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3632,11 +3632,11 @@ static void cliDumpGyroRegisters(const char *cmdName, char *cmdline)
UNUSED(cmdline);

#ifdef USE_MULTI_GYRO
if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE)) {
cliPrintLinef("\r\n# Gyro 1");
cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
}
if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE)) {
cliPrintLinef("\r\n# Gyro 2");
cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_2);
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/cli/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -191,7 +191,7 @@ static const char * const lookupTableAlignment[] = {

#ifdef USE_MULTI_GYRO
static const char * const lookupTableGyro[] = {
"FIRST", "SECOND", "BOTH"
"FIRST", "SECOND", "BOTH_SIMPLE", "BOTH_VARIANCE"
};
#endif

Expand Down
4 changes: 2 additions & 2 deletions src/main/cms/cms_menu_imu.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ static const char * const osdTableThrottleLimitType[] = {

#ifdef USE_MULTI_GYRO
static const char * const osdTableGyroToUse[] = {
"FIRST", "SECOND", "BOTH"
"FIRST", "SECOND", "BOTH_SIMP", "BOTH_VAR"
};
#endif

Expand Down Expand Up @@ -742,7 +742,7 @@ static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 } },
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 } },
#ifdef USE_MULTI_GYRO
{ "GYRO TO USE", OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gyroConfig_gyro_to_use, 2, osdTableGyroToUse} },
{ "GYRO TO USE", OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gyroConfig_gyro_to_use, 3, osdTableGyroToUse} },
#endif

{ "BACK", OME_Back, NULL, NULL },
Expand Down
14 changes: 14 additions & 0 deletions src/main/drivers/accgyro/accgyro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@

#define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors
#define GYRO_SCALE_4000DPS (4000.0f / (1 << 15)) // 8.192 dps/lsb scalefactor for 4000dps sensors
#define GYRO_VARIANCE_WINDOW 8

typedef enum {
GYRO_NONE = 0,
Expand Down Expand Up @@ -90,6 +91,18 @@ typedef enum {
GYRO_EXTI_NO_INT
} gyroModeSPI_e;

typedef struct gyroVariance_s {
uint16_t windex;
uint16_t w;
float axisWindow[GYRO_VARIANCE_WINDOW];
float varianceWindow[GYRO_VARIANCE_WINDOW];
float axisSumVar;
float axisVar;
float axisSumMean;
float axisMean;
float inverseN;
} gyroVariance_t;

typedef struct gyroDev_s {
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
pthread_mutex_t lock;
Expand Down Expand Up @@ -128,6 +141,7 @@ typedef struct gyroDev_s {
fp_rotationMatrix_t rotationMatrix;
uint16_t gyroSampleRateHz;
uint16_t accSampleRateHz;
gyroVariance_t variance[XYZ_AXIS_COUNT];
uint8_t accDataReg;
uint8_t gyroDataReg;
} gyroDev_t;
Expand Down
6 changes: 4 additions & 2 deletions src/main/msp/msp.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1741,7 +1741,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
case GYRO_CONFIG_USE_GYRO_2:
gyroAlignment = gyroDeviceConfig(1)->alignment;
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
// for dual-gyro in "BOTH" mode we only read/write gyro 0
default:
gyroAlignment = gyroDeviceConfig(0)->alignment;
Expand Down Expand Up @@ -2813,7 +2814,8 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
case GYRO_CONFIG_USE_GYRO_2:
gyroDeviceConfigMutable(1)->alignment = gyroAlignment;
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
// For dual-gyro in "BOTH" mode we'll only update gyro 0
default:
gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
Expand Down
68 changes: 64 additions & 4 deletions src/main/sensors/gyro.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@

#include "platform.h"

#include "arm_math.h"

#include "build/debug.h"

#include "common/axis.h"
Expand Down Expand Up @@ -151,7 +153,8 @@ FAST_CODE bool gyroIsCalibrationComplete(void)
case GYRO_CONFIG_USE_GYRO_2: {
return isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
}
case GYRO_CONFIG_USE_GYRO_BOTH: {
case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE: {
return isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
}
#endif
Expand Down Expand Up @@ -239,6 +242,11 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
if (axis == Z) {
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
}

for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroSensor->gyroDev.variance[axis].inverseN = 1.0f / 8.0f;
gyroSensor->gyroDev.variance[axis].w = 8;
}
}
}

Expand Down Expand Up @@ -402,6 +410,32 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor)
}
}

FAST_CODE float gyroVariance(gyroVariance_t *variance, float gyroRate) {
variance->axisWindow[variance->windex] = gyroRate;
variance->axisSumMean += variance->axisWindow[variance->windex];

float varianceElement = variance->axisWindow[variance->windex] - variance->axisMean;
varianceElement = varianceElement * varianceElement;
variance->axisSumVar += varianceElement;
variance->varianceWindow[variance->windex] = varianceElement;
variance->windex++;

if (variance->windex > variance->w) {
variance->windex = 0;
}

variance->axisSumMean -= variance->axisWindow[variance->windex];
variance->axisSumVar -= variance->varianceWindow[variance->windex];

//New mean
variance->axisMean = variance->axisSumMean * variance->inverseN;
variance->axisVar = variance->axisSumVar * variance->inverseN;

float squirt = 0;
arm_sqrt_f32(variance->axisVar, &squirt);
return squirt;
}

FAST_CODE void gyroUpdate(void)
{
switch (gyro.gyroToUse) {
Expand All @@ -422,7 +456,7 @@ FAST_CODE void gyroUpdate(void)
gyro.gyroADC[Z] = gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale;
}
break;
case GYRO_CONFIG_USE_GYRO_BOTH:
case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
gyroUpdateSensor(&gyro.gyroSensor1);
gyroUpdateSensor(&gyro.gyroSensor2);
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
Expand All @@ -431,6 +465,30 @@ FAST_CODE void gyroUpdate(void)
gyro.gyroADC[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
}
break;
case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
gyroUpdateSensor(&gyro.gyroSensor1);
gyroUpdateSensor(&gyro.gyroSensor2);
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
float gyroScaled1, gyroScaled2, varianceGyro1, varianceGyro2;
gyroScaled1 = gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale;
gyroScaled2 = gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale;
varianceGyro1 = gyroVariance(&gyro.gyroSensor1.gyroDev.variance[X], gyroScaled1);
varianceGyro2 = gyroVariance(&gyro.gyroSensor2.gyroDev.variance[X], gyroScaled2);
gyro.gyroADC[X] = ((gyroScaled1 * varianceGyro2) + (gyroScaled2 * varianceGyro1)) / (varianceGyro1 + varianceGyro2);

gyroScaled1 = gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale;
gyroScaled2 = gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale;
varianceGyro1 = gyroVariance(&gyro.gyroSensor1.gyroDev.variance[Y], gyroScaled1);
varianceGyro2 = gyroVariance(&gyro.gyroSensor2.gyroDev.variance[Y], gyroScaled2);
gyro.gyroADC[Y] = ((gyroScaled1 * varianceGyro2) + (gyroScaled2 * varianceGyro1)) / (varianceGyro1 + varianceGyro2);

gyroScaled1 = gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale;
gyroScaled2 = gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale;
varianceGyro1 = gyroVariance(&gyro.gyroSensor1.gyroDev.variance[Z], gyroScaled1);
varianceGyro2 = gyroVariance(&gyro.gyroSensor2.gyroDev.variance[Z], gyroScaled2);
gyro.gyroADC[Z] = ((gyroScaled1 * varianceGyro2) + (gyroScaled2 * varianceGyro1)) / (varianceGyro1 + varianceGyro2);
}
break;
#endif
}

Expand Down Expand Up @@ -495,7 +553,8 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale));
break;

case GYRO_CONFIG_USE_GYRO_BOTH:
case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
Expand Down Expand Up @@ -577,7 +636,8 @@ void gyroReadTemperature(void)
gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor2);
break;

case GYRO_CONFIG_USE_GYRO_BOTH:
case GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE:
case GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE:
gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyro.gyroSensor1), gyroReadSensorTemperature(gyro.gyroSensor2));
break;
#endif // USE_MULTI_GYRO
Expand Down
3 changes: 2 additions & 1 deletion src/main/sensors/gyro.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,7 +155,8 @@ typedef enum {

#define GYRO_CONFIG_USE_GYRO_1 0
#define GYRO_CONFIG_USE_GYRO_2 1
#define GYRO_CONFIG_USE_GYRO_BOTH 2
#define GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE 2
#define GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE 3

enum {
FILTER_LPF1 = 0,
Expand Down
8 changes: 4 additions & 4 deletions src/main/sensors/gyro_init.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -616,7 +616,7 @@ bool gyroInit(void)
}

#if defined(USE_MULTI_GYRO)
if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
if ((gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK))
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) {
if (gyroDetectionFlags & GYRO_1_MASK) {
Expand All @@ -632,14 +632,14 @@ bool gyroInit(void)
// Only allow using both gyros simultaneously if they are the same hardware type.
if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
gyroDetectionFlags |= GYRO_IDENTICAL_MASK;
} else if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
} else if (gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE) {
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
eepromWriteRequired = true;
}

if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE) {
static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
// SPI DMA buffer required per device
gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
Expand All @@ -655,7 +655,7 @@ bool gyroInit(void)
writeEEPROM();
}

if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse >= GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE) {
static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
// SPI DMA buffer required per device
gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;
Expand Down
2 changes: 1 addition & 1 deletion src/main/target/SPRACINGH7EXTREME/target.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,7 +161,7 @@
#define GYRO_2_ALIGN ALIGN_CUSTOM
#define GYRO_2_CUSTOM_ALIGN SENSOR_ALIGNMENT( 0, 0, 225)

#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH_VARIANCE

#define USE_FLASHFS
#define USE_FLASH_TOOLS
Expand Down