From 156384063f36be12bf99cafa5fb1ba21cdc5ad75 Mon Sep 17 00:00:00 2001 From: Jukka Aakko Date: Tue, 12 May 2020 18:22:48 +0300 Subject: [PATCH 1/2] MultiGyro support for 2.5.0RC1. Not flowngit add src/main/target/IFLIGHTF7_TWING/config.c! IFLIGHTF7 TWING config.c removed to align with IMU_1_ALIGN settings. Multigyro temperature functions. identation.. Beer drinkers typos corrected. Multigyro fixies from desktop testing. Removed gyro2 and acc2 aligments from msp. Fixed flag usage to compile all targets. --- src/main/fc/settings.yaml | 16 ++- src/main/sensors/acceleration.c | 28 ++++- src/main/sensors/acceleration.h | 1 + src/main/sensors/gyro.c | 139 +++++++++++++++++++---- src/main/sensors/gyro.h | 10 ++ src/main/target/IFLIGHTF7_TWING/target.c | 8 +- src/main/target/IFLIGHTF7_TWING/target.h | 2 +- src/main/target/common_post.h | 4 + 8 files changed, 178 insertions(+), 30 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e5b3d3471d5..7afd8f7e879 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -144,6 +144,9 @@ tables: - name: osd_ahi_style values: ["DEFAULT", "LINE"] enum: osd_ahi_style_e + - name: gyro_to_use_t + values: ["FIRST", "SECOND", "BOTH"] + enum: gyro_to_use_e groups: - name: PG_GYRO_CONFIG @@ -159,6 +162,10 @@ groups: field: gyro_align type: uint8_t table: alignment + - name: align_gyro2 + field: gyro2_align + type: uint8_t + table: alignment - name: gyro_hardware_lpf field: gyro_lpf table: gyro_lpf @@ -204,9 +211,10 @@ groups: min: 30 max: 1000 - name: gyro_to_use + table: gyro_to_use_t + field: gyro_to_use condition: USE_DUAL_GYRO - min: 0 - max: 1 + - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t @@ -244,6 +252,10 @@ groups: field: acc_align type: uint8_t table: alignment + - name: align_acc2 + field: acc2_align + type: uint8_t + table: alignment - name: acc_hardware table: acc_hardware - name: acc_lpf_hz diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 015d55f3597..d311108a039 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -286,9 +286,14 @@ bool accInit(uint32_t targetLooptime) { memset(&acc, 0, sizeof(acc)); - // Set inertial sensor tag (for dual-gyro selection) + +// Set inertial sensor tag (for dual-gyro selection) #ifdef USE_DUAL_GYRO - acc.dev.imuSensorToUse = gyroConfig()->gyro_to_use; // Use the same selection from gyroConfig() + acc.dev.imuSensorToUse = gyroConfig()->gyro_to_use; +#ifdef USE_MULTI_GYRO //TODO: Fixme to + if(gyroConfig()->gyro_to_use == BOTH ) + acc.dev.imuSensorToUse = 0; +#endif #else acc.dev.imuSensorToUse = 0; #endif @@ -310,8 +315,23 @@ bool accInit(uint32_t targetLooptime) // At this poinrt acc.dev.accAlign was set up by the driver from the busDev record // If configuration says different - override - if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { - acc.dev.accAlign = accelerometerConfig()->acc_align; + switch(gyroConfig()->gyro_to_use ){ + case FIRST: + default: +#ifdef USE_MULTI_GYRO + case BOTH: +#endif + if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig()->acc_align; + } + break; +#ifdef USE_DUAL_GYRO + case SECOND: + if (accelerometerConfig()->acc2_align != ALIGN_DEFAULT) { + acc.dev.accAlign = accelerometerConfig()->acc2_align; + } + break; +#endif } return true; } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 47f669b83a5..e08e74bf641 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -69,6 +69,7 @@ extern acc_t acc; typedef struct accelerometerConfig_s { sensor_align_e acc_align; // acc alignment + sensor_align_e acc2_align; // acc 2 alignment uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz flightDynamicsTrims_t accZero; // Accelerometer offset diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index bd6b43db1c6..7dd6e5a0cf5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -78,8 +78,12 @@ FILE_COMPILE_FOR_SPEED #endif FASTRAM gyro_t gyro; // gyro sensor object - +#ifdef USE_MULTI_GYRO +#define MAX_GYRO_COUNT 2 +#else #define MAX_GYRO_COUNT 1 +#endif + STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT]; // Not in FASTRAM since it may hold DMA buffers STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT]; @@ -111,7 +115,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyroMovementCalibrationThreshold = 32, .looptime = 1000, .gyroSync = 1, - .gyro_to_use = 0, + .gyro_to_use = FIRST, .gyro_notch_hz = 0, .gyro_notch_cutoff = 1, .gyro_stage2_lowpass_hz = 0, @@ -272,23 +276,52 @@ bool gyroInit(void) memset(&gyro, 0, sizeof(gyro)); // Set inertial sensor tag (for dual-gyro selection) + gyroSensor_e gyro0Hardware; +#ifdef USE_MULTI_GYRO + gyroSensor_e gyro1Hardware; +#endif + + switch (gyroConfig()->gyro_to_use){ + case FIRST: + case SECOND: #ifdef USE_DUAL_GYRO - gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use; + gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use; #else - gyroDev[0].imuSensorToUse = 0; + gyroDev[0].imuSensorToUse = 0; #endif - // Detecting gyro0 - gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); - if (gyroHardware == GYRO_NONE) { - gyro.initialized = false; - detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; - return true; - } + // Detecting gyro0 + gyro0Hardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); + if (gyro0Hardware == GYRO_NONE) { + gyro.initialized = false; + detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; + return true; + } + break; + +#ifdef USE_MULTI_GYRO + case BOTH: + gyroDev[0].imuSensorToUse = FIRST; + gyroDev[1].imuSensorToUse = SECOND; + gyro0Hardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); + gyro1Hardware = gyroDetect(&gyroDev[1], GYRO_AUTODETECT); + + if (gyro0Hardware == GYRO_NONE || gyro1Hardware == GYRO_NONE) { + gyro.initialized = false; + detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; + return true; + } + break; + +#endif + default: + return false; + }; - // Gyro is initialized gyro.initialized = true; - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; + + // FIXME: We need to record both gyros here + detectedSensors[SENSOR_INDEX_GYRO] = gyro0Hardware; sensorsSet(SENSOR_GYRO); // Driver initialisation @@ -298,15 +331,47 @@ bool gyroInit(void) gyroDev[0].initFn(&gyroDev[0]); // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value - gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime; + gyro.targetLooptime = (gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime); + +#ifdef USE_MULTI_GYRO + if (gyroConfig()->gyro_to_use == BOTH) { + gyroDev[1].lpf = gyroConfig()->gyro_lpf; + gyroDev[1].requestedSampleIntervalUs = gyroConfig()->looptime; + gyroDev[1].sampleRateIntervalUs = gyroConfig()->looptime; + gyroDev[1].initFn(&gyroDev[1]); + gyro.targetLooptime = (gyroConfig()->gyroSync ? MAX(gyroDev[0].sampleRateIntervalUs, gyroDev[1].sampleRateIntervalUs) : gyroConfig()->looptime); + } +#endif - // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record - // If configuration says different - override - if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { - gyroDev[0].gyroAlign = gyroConfig()->gyro_align; + + switch (gyroConfig()->gyro_to_use){ + case FIRST: + default: + if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { + gyroDev[0].gyroAlign = gyroConfig()->gyro_align; + } + break; +#ifdef USE_DUAL_GYRO + case SECOND: + if (gyroConfig()->gyro2_align != ALIGN_DEFAULT) { + gyroDev[0].gyroAlign = gyroConfig()->gyro2_align; + } + break; +#endif +#ifdef USE_MULTI_GYRO + case BOTH: + if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { + gyroDev[0].gyroAlign = gyroConfig()->gyro_align; + } + if (gyroConfig()->gyro2_align != ALIGN_DEFAULT) { + gyroDev[1].gyroAlign = gyroConfig()->gyro2_align; + } + break; +#endif } gyroInitFilters(); + #ifdef USE_DYNAMIC_FILTERS dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); gyroDataAnalyseStateInit( @@ -326,6 +391,11 @@ void gyroStartCalibration(void) } zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); +#ifdef USE_MULTI_GYRO + if(gyroConfig()->gyro_to_use == BOTH) { + zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); + } +#endif } bool gyroIsCalibrationComplete(void) @@ -333,6 +403,11 @@ bool gyroIsCalibrationComplete(void) if (!gyro.initialized) { return true; } +#ifdef USE_MULTI_GYRO + if (gyroConfig()->gyro_to_use == BOTH) + return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]) && + zeroCalibrationIsCompleteV(&gyroCalibration[1]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[1]); +#endif return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } @@ -422,6 +497,15 @@ void FAST_CODE NOINLINE gyroUpdate() if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { return; } +#ifdef USE_MULTI_GYRO + if (gyroConfig()->gyro_to_use == BOTH ) { + if(!gyroUpdateAndCalibrate(&gyroDev[1], &gyroCalibration[1], gyro.gyro2ADCf)) + return; + gyro.gyroADCf[X] = (gyro.gyroADCf[X] + gyro.gyro2ADCf[X]) / 2.0f; + gyro.gyroADCf[Y] = (gyro.gyroADCf[Y] + gyro.gyro2ADCf[Y]) / 2.0f; + gyro.gyroADCf[Z] = (gyro.gyroADCf[Z] + gyro.gyro2ADCf[Z]) / 2.0f; + } +#endif for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] @@ -473,6 +557,13 @@ bool gyroReadTemperature(void) } // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10] + // TODO: [degC * 10] is a bug in Finland. Negative temperature... +#ifdef USE_MULTI_GYRO + if (gyroConfig()->gyro_to_use == BOTH && gyroDev[0].temperatureFn && gyroDev[1].temperatureFn) + return MAX(gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]), gyroDev[1].temperatureFn(&gyroDev[1], &gyroTemperature[1])); + else if (gyroConfig()->gyro_to_use == BOTH) + return false; +#endif if (gyroDev[0].temperatureFn) { return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]); } @@ -485,7 +576,10 @@ int16_t gyroGetTemperature(void) if (!gyro.initialized) { return 0; } - +#ifdef USE_MULTI_GYRO + if (gyroConfig()->gyro_to_use == BOTH) + return MAX(gyroTemperature[0], gyroTemperature[1]); +#endif return gyroTemperature[0]; } @@ -508,5 +602,12 @@ bool gyroSyncCheckUpdate(void) return false; } +#ifdef USE_MULTI_GYRO + if(gyroConfig()->gyro_to_use == BOTH) { + if (!gyroDev[1].intStatusFn) + return false; + return gyroDev[0].intStatusFn(&gyroDev[0]) && gyroDev[1].intStatusFn(&gyroDev[1]); + } +#endif return gyroDev[0].intStatusFn(&gyroDev[0]); } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b709294b553..d2b691fb0d2 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -45,6 +45,12 @@ typedef enum { DYN_NOTCH_RANGE_LOW } dynamicFilterRange_e; +typedef enum { + FIRST = 0, + SECOND = 1, + BOTH = 2 +} gyro_to_use_e; + #define DYN_NOTCH_RANGE_HZ_HIGH 2000 #define DYN_NOTCH_RANGE_HZ_MEDIUM 1333 #define DYN_NOTCH_RANGE_HZ_LOW 1000 @@ -53,12 +59,16 @@ typedef struct gyro_s { bool initialized; uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; +#ifdef USE_MULTI_GYRO + float gyro2ADCf[XYZ_AXIS_COUNT]; +#endif } gyro_t; extern gyro_t gyro; typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment + sensor_align_e gyro2_align; // second 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 gyroSync; // Enable interrupt based loop uint16_t looptime; // imu loop time in us diff --git a/src/main/target/IFLIGHTF7_TWING/target.c b/src/main/target/IFLIGHTF7_TWING/target.c index 83090b5c94d..1ccdd057209 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.c +++ b/src/main/target/IFLIGHTF7_TWING/target.c @@ -37,11 +37,11 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), }; diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h index a6a9e63de23..cf6cd309c7f 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.h +++ b/src/main/target/IFLIGHTF7_TWING/target.h @@ -36,7 +36,7 @@ #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define USE_DUAL_GYRO +#define USE_MULTI_GYRO #define USE_IMU_MPU6500 #define IMU_0_ALIGN CW90_DEG diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 32b6e3531a8..f21f762bc2b 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -39,6 +39,10 @@ #define USE_RPM_FILTER #endif +#ifdef USE_MULTI_GYRO + #define USE_DUAL_GYRO +#endif + #ifdef USE_ITCM_RAM #define FAST_CODE __attribute__((section(".tcm_code"))) #define NOINLINE __NOINLINE From beaa334117dbfa621d2fc65832f009c3c5e8d3c4 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Wed, 10 Jun 2020 22:29:10 +0200 Subject: [PATCH 2/2] Refactor dual-gyro support to be less dependent on number of active gyros [ci skip] --- src/main/fc/fc_msp.c | 4 +- src/main/fc/settings.yaml | 6 +- src/main/sensors/acceleration.c | 10 +- src/main/sensors/gyro.c | 317 ++++++++++++++++++-------------- src/main/sensors/gyro.h | 22 +-- 5 files changed, 200 insertions(+), 159 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 066e02dd5ff..94370e9f4a7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1131,7 +1131,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_SENSOR_ALIGNMENT: - sbufWriteU8(dst, gyroConfig()->gyro_align); + sbufWriteU8(dst, gyroConfig()->gyro_align[0]); sbufWriteU8(dst, accelerometerConfig()->acc_align); sbufWriteU8(dst, compassConfig()->mag_align); #ifdef USE_OPFLOW @@ -2003,7 +2003,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_SENSOR_ALIGNMENT: if (dataSize >= 4) { - gyroConfigMutable()->gyro_align = sbufReadU8(src); + gyroConfigMutable()->gyro_align[0] = sbufReadU8(src); accelerometerConfigMutable()->acc_align = sbufReadU8(src); #ifdef USE_MAG compassConfigMutable()->mag_align = sbufReadU8(src); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7afd8f7e879..62d32a56292 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -158,12 +158,12 @@ groups: - name: gyro_sync field: gyroSync type: bool - - name: align_gyro - field: gyro_align + - name: align_gyro1 + field: gyro_align[0] type: uint8_t table: alignment - name: align_gyro2 - field: gyro2_align + field: gyro_align[1] type: uint8_t table: alignment - name: gyro_hardware_lpf diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d311108a039..9eb29d3d280 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -290,12 +290,14 @@ bool accInit(uint32_t targetLooptime) // Set inertial sensor tag (for dual-gyro selection) #ifdef USE_DUAL_GYRO acc.dev.imuSensorToUse = gyroConfig()->gyro_to_use; -#ifdef USE_MULTI_GYRO //TODO: Fixme to - if(gyroConfig()->gyro_to_use == BOTH ) - acc.dev.imuSensorToUse = 0; +#ifdef USE_MULTI_GYRO + // FIXME: Dual acc support + if (gyroConfig()->gyro_to_use == IMU_TO_USE_BOTH) { + acc.dev.imuSensorToUse = IMU_TO_USE_FIRST; + } #endif #else - acc.dev.imuSensorToUse = 0; + acc.dev.imuSensorToUse = IMU_TO_USE_FIRST; #endif if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 7dd6e5a0cf5..54471f83bad 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -77,17 +77,23 @@ FILE_COMPILE_FOR_SPEED #include "hardware_revision.h" #endif -FASTRAM gyro_t gyro; // gyro sensor object #ifdef USE_MULTI_GYRO -#define MAX_GYRO_COUNT 2 +# define MAX_GYRO_COUNT 2 #else -#define MAX_GYRO_COUNT 1 +# define MAX_GYRO_COUNT 1 #endif +typedef struct { + bool available; + bool syncUpdate; + gyroDev_t gyroDev; + zeroCalibrationVector_t gyroCal; + int16_t gyroTemp; +} gyroDevInstance_t; + +FASTRAM gyro_t gyro; // gyro sensor object -STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT]; // Not in FASTRAM since it may hold DMA buffers -STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT]; -STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration[MAX_GYRO_COUNT]; +STATIC_FASTRAM_UNIT_TESTED gyroDevInstance_t gyroInstance[MAX_GYRO_COUNT]; STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn; STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; @@ -99,10 +105,8 @@ STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn; STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT]; #ifdef USE_DYNAMIC_FILTERS - EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState; EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; - #endif PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9); @@ -111,7 +115,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros .gyro_soft_lpf_hz = 60, .gyro_soft_lpf_type = FILTER_BIQUAD, - .gyro_align = ALIGN_DEFAULT, + .gyro_align[0] = ALIGN_DEFAULT, + .gyro_align[1] = ALIGN_DEFAULT, .gyroMovementCalibrationThreshold = 32, .looptime = 1000, .gyroSync = 1, @@ -258,7 +263,7 @@ static void gyroInitFilters(void) { STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; notchFilter1ApplyFn = nullFilterApply; - + initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz); initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz); @@ -271,105 +276,87 @@ static void gyroInitFilters(void) } } -bool gyroInit(void) +gyroSensor_e gyroInitInstance(gyroDevInstance_t * instance, int imuTag, sensor_align_e gyroAlign) { - memset(&gyro, 0, sizeof(gyro)); + // Start with unavailable sensor + instance->gyroDev.imuSensorToUse = imuTag; - // Set inertial sensor tag (for dual-gyro selection) - gyroSensor_e gyro0Hardware; -#ifdef USE_MULTI_GYRO - gyroSensor_e gyro1Hardware; -#endif + gyroSensor_e gyroHardware = gyroDetect(&instance->gyroDev, GYRO_AUTODETECT); - switch (gyroConfig()->gyro_to_use){ - case FIRST: - case SECOND: -#ifdef USE_DUAL_GYRO - gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use; -#else - gyroDev[0].imuSensorToUse = 0; -#endif + // Fail early if not detected + if (gyroHardware == GYRO_NONE) { + return GYRO_NONE; + } - // Detecting gyro0 - gyro0Hardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); - if (gyro0Hardware == GYRO_NONE) { - gyro.initialized = false; - detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; - return true; - } - break; + instance->gyroDev.lpf = gyroConfig()->gyro_lpf; + instance->gyroDev.requestedSampleIntervalUs = gyroConfig()->looptime; + instance->gyroDev.sampleRateIntervalUs = gyroConfig()->looptime; + instance->gyroDev.initFn(&instance->gyroDev); -#ifdef USE_MULTI_GYRO - case BOTH: - gyroDev[0].imuSensorToUse = FIRST; - gyroDev[1].imuSensorToUse = SECOND; - gyro0Hardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); - gyro1Hardware = gyroDetect(&gyroDev[1], GYRO_AUTODETECT); - - if (gyro0Hardware == GYRO_NONE || gyro1Hardware == GYRO_NONE) { - gyro.initialized = false; - detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; - return true; - } - break; + // Override default alignment + if (gyroAlign != ALIGN_DEFAULT) { + instance->gyroDev.gyroAlign = gyroAlign; + } -#endif - default: - return false; - }; + instance->available = true; - gyro.initialized = true; + return gyroHardware; +} - // FIXME: We need to record both gyros here - detectedSensors[SENSOR_INDEX_GYRO] = gyro0Hardware; - sensorsSet(SENSOR_GYRO); +bool gyroInit(void) +{ + memset(&gyro, 0, sizeof(gyro)); + memset(&gyroInstance, 0, sizeof(gyroInstance)); + + // Prepare failure scenario + detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; + gyro.initialized = false; + + // Initialize gyroscopes + switch (gyroConfig()->gyro_to_use) { + case IMU_TO_USE_FIRST: + case IMU_TO_USE_SECOND: + { + sensor_align_e gyroAlign = gyroConfig()->gyro_align[gyroConfig()->gyro_to_use]; + gyroSensor_e gyroHardware = gyroInitInstance(&gyroInstance[0], imuTag, gyroAlign); + + if (gyroHardware != GYRO_NONE) { + gyro.initialized = true; + gyro.targetLooptime = (gyroConfig()->gyroSync ? gyroInstance[0].gyroDev.sampleRateIntervalUs : gyroConfig()->looptime); + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; + } + } + break; - // Driver initialisation - gyroDev[0].lpf = gyroConfig()->gyro_lpf; - gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime; - gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime; - gyroDev[0].initFn(&gyroDev[0]); + case IMU_TO_USE_BOTH: +#ifdef USE_MULTI_GYRO + { + gyroSensor_e gyroHardware0 = gyroInitInstance(&gyroInstance[0], 0, gyroConfig()->gyro_align[0]); + gyroSensor_e gyroHardware1 = gyroInitInstance(&gyroInstance[1], 1, gyroConfig()->gyro_align[1]); - // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value - gyro.targetLooptime = (gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime); + if (gyroHardware0 != GYRO_NONE && gyroHardware1 != GYRO_NONE) { + gyro.initialized = true; -#ifdef USE_MULTI_GYRO - if (gyroConfig()->gyro_to_use == BOTH) { - gyroDev[1].lpf = gyroConfig()->gyro_lpf; - gyroDev[1].requestedSampleIntervalUs = gyroConfig()->looptime; - gyroDev[1].sampleRateIntervalUs = gyroConfig()->looptime; - gyroDev[1].initFn(&gyroDev[1]); - gyro.targetLooptime = (gyroConfig()->gyroSync ? MAX(gyroDev[0].sampleRateIntervalUs, gyroDev[1].sampleRateIntervalUs) : gyroConfig()->looptime); - } -#endif + timeUs_t gyroSampleInterval0 = gyroInstance[0].gyroDev.sampleRateIntervalUs; + timeUs_t gyroSampleInterval1 = gyroInstance[1].gyroDev.sampleRateIntervalUs; - - switch (gyroConfig()->gyro_to_use){ - case FIRST: - default: - if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { - gyroDev[0].gyroAlign = gyroConfig()->gyro_align; - } - break; -#ifdef USE_DUAL_GYRO - case SECOND: - if (gyroConfig()->gyro2_align != ALIGN_DEFAULT) { - gyroDev[0].gyroAlign = gyroConfig()->gyro2_align; - } - break; -#endif -#ifdef USE_MULTI_GYRO - case BOTH: - if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { - gyroDev[0].gyroAlign = gyroConfig()->gyro_align; - } - if (gyroConfig()->gyro2_align != ALIGN_DEFAULT) { - gyroDev[1].gyroAlign = gyroConfig()->gyro2_align; - } - break; + gyro.targetLooptime = (gyroConfig()->gyroSync ? MAX(gyroSampleInterval0, gyroSampleInterval1) : gyroConfig()->looptime); + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware0; + //detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware1; + // FIXME: Record second gyro data + } + } #endif + break; } + // Fail early + if (!gyro.initialized) { + return true; + } + + sensorsSet(SENSOR_GYRO); + gyroInitFilters(); #ifdef USE_DYNAMIC_FILTERS @@ -390,12 +377,13 @@ void gyroStartCalibration(void) return; } - zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); -#ifdef USE_MULTI_GYRO - if(gyroConfig()->gyro_to_use == BOTH) { - zeroCalibrationStartV(&gyroCalibration[1], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + if (!gyroInstance[i].available) { + continue; + } + + zeroCalibrationStartV(&gyroInstance[i].gyroCal, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } -#endif } bool gyroIsCalibrationComplete(void) @@ -403,13 +391,19 @@ bool gyroIsCalibrationComplete(void) if (!gyro.initialized) { return true; } -#ifdef USE_MULTI_GYRO - if (gyroConfig()->gyro_to_use == BOTH) - return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]) && - zeroCalibrationIsCompleteV(&gyroCalibration[1]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[1]); -#endif - return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + if (!gyroInstance[i].available) { + continue; + } + + const bool isDone = zeroCalibrationIsCompleteV(&gyroInstance[i].gyroCal) && zeroCalibrationIsSuccessfulV(&gyroInstance[i].gyroCal); + if (!isDone) { + return false; + } + } + + return true; } STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) @@ -494,18 +488,37 @@ void FAST_CODE NOINLINE gyroUpdate() return; } - if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { - return; + // Update and calibrate all gyros + int activeGyros = 0; + gyro.gyroADCf[X] = 0; + gyro.gyroADCf[Y] = 0; + gyro.gyroADCf[Z] = 0; + + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + float gyroADCTmpf[XYZ_AXIS_COUNT]; + + if (!gyroInstance[i].available) { + continue; + } + + gyroUpdateAndCalibrate(&gyroInstance[i].gyroDev, &gyroInstance[i].gyroCal, gyroADCTmpf); + + gyro.gyroADCf[X] += gyroADCTmpf[X]; + gyro.gyroADCf[Y] += gyroADCTmpf[Y]; + gyro.gyroADCf[Z] += gyroADCTmpf[Z]; + activeGyros++; } -#ifdef USE_MULTI_GYRO - if (gyroConfig()->gyro_to_use == BOTH ) { - if(!gyroUpdateAndCalibrate(&gyroDev[1], &gyroCalibration[1], gyro.gyro2ADCf)) - return; - gyro.gyroADCf[X] = (gyro.gyroADCf[X] + gyro.gyro2ADCf[X]) / 2.0f; - gyro.gyroADCf[Y] = (gyro.gyroADCf[Y] + gyro.gyro2ADCf[Y]) / 2.0f; - gyro.gyroADCf[Z] = (gyro.gyroADCf[Z] + gyro.gyro2ADCf[Z]) / 2.0f; + + // Fail early + if (activeGyros == 0) { + return; } -#endif + + // Average all active gyros + gyro.gyroADCf[X] /= activeGyros; + gyro.gyroADCf[Y] /= activeGyros; + gyro.gyroADCf[Z] /= activeGyros; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] @@ -558,17 +571,22 @@ bool gyroReadTemperature(void) // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10] // TODO: [degC * 10] is a bug in Finland. Negative temperature... -#ifdef USE_MULTI_GYRO - if (gyroConfig()->gyro_to_use == BOTH && gyroDev[0].temperatureFn && gyroDev[1].temperatureFn) - return MAX(gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]), gyroDev[1].temperatureFn(&gyroDev[1], &gyroTemperature[1])); - else if (gyroConfig()->gyro_to_use == BOTH) - return false; -#endif - if (gyroDev[0].temperatureFn) { - return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]); + + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + if (!gyroInstance[i].available) { + continue; + } + + if (!gyroInstance[i].gyroDev.temperatureFn) { + return false; + } + + if (!gyroInstance[i].gyroDev.temperatureFn(&gyroInstance[i].gyroDev, &gyroInstance[i].gyroTemp)) { + return false; + } } - return false; + return true; } int16_t gyroGetTemperature(void) @@ -576,11 +594,17 @@ int16_t gyroGetTemperature(void) if (!gyro.initialized) { return 0; } -#ifdef USE_MULTI_GYRO - if (gyroConfig()->gyro_to_use == BOTH) - return MAX(gyroTemperature[0], gyroTemperature[1]); -#endif - return gyroTemperature[0]; + + int16_t maxTemp = -32768; + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + if (!gyroInstance[i].available) { + continue; + } + + maxTemp = MAX(maxTemp, gyroInstance[i].gyroTemp); + } + + return maxTemp; } int16_t gyroRateDps(int axis) @@ -598,16 +622,35 @@ bool gyroSyncCheckUpdate(void) return false; } - if (!gyroDev[0].intStatusFn) { - return false; + // This is tricky. A call to intStatusFn will reset the flag. We need to make a flag sticky locally + + // Pass one - poll and cache gyroDev + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + if (!gyroInstance[i].available) { + continue; + } + + // If one of the gyros is incapable of gyro sync - fail early + if (!gyroInstance[i].gyroDev.intStatusFn) { + return false; + } + + if (gyroInstance[i].gyroDev.intStatusFn(&gyroInstance[i].gyroDev)) { + gyroInstance[i].syncUpdate = true; + } } -#ifdef USE_MULTI_GYRO - if(gyroConfig()->gyro_to_use == BOTH) { - if (!gyroDev[1].intStatusFn) + // Pass two - succeed only if all available gyros are synced + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + if (gyroInstance[i].available && !gyroInstance[i].syncUpdate) { return false; - return gyroDev[0].intStatusFn(&gyroDev[0]) && gyroDev[1].intStatusFn(&gyroDev[1]); + } } -#endif - return gyroDev[0].intStatusFn(&gyroDev[0]); + + // Pass three - at this point all gyros are synced. Reset flags + for (int i = 0; i < MAX_GYRO_COUNT; i++) { + gyroInstance[i].syncUpdate = false; + } + + return true; } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index d2b691fb0d2..2abb68d16fa 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -46,10 +46,10 @@ typedef enum { } dynamicFilterRange_e; typedef enum { - FIRST = 0, - SECOND = 1, - BOTH = 2 -} gyro_to_use_e; + IMU_TO_USE_FIRST = 0, + IMU_TO_USE_SECOND = 1, + IMU_TO_USE_BOTH = 2 +} imuToUse_e; #define DYN_NOTCH_RANGE_HZ_HIGH 2000 #define DYN_NOTCH_RANGE_HZ_MEDIUM 1333 @@ -59,20 +59,16 @@ typedef struct gyro_s { bool initialized; uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; -#ifdef USE_MULTI_GYRO - float gyro2ADCf[XYZ_AXIS_COUNT]; -#endif } gyro_t; extern gyro_t gyro; typedef struct gyroConfig_s { - sensor_align_e gyro_align; // gyro alignment - sensor_align_e gyro2_align; // second 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 gyroSync; // Enable interrupt based loop - 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. + sensor_align_e gyro_align[2]; // 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 gyroSync; // Enable interrupt based loop + 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_soft_lpf_type; uint8_t gyro_to_use;