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 e5b3d3471d5..62d32a56292 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 @@ -155,8 +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: gyro_align[1] type: uint8_t table: alignment - name: gyro_hardware_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..9eb29d3d280 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -286,11 +286,18 @@ 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 + // 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)) { @@ -310,8 +317,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..54471f83bad 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -77,13 +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 +#else +# define MAX_GYRO_COUNT 1 +#endif -#define MAX_GYRO_COUNT 1 +typedef struct { + bool available; + bool syncUpdate; + gyroDev_t gyroDev; + zeroCalibrationVector_t gyroCal; + int16_t gyroTemp; +} gyroDevInstance_t; -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]; +FASTRAM gyro_t gyro; // gyro sensor object + +STATIC_FASTRAM_UNIT_TESTED gyroDevInstance_t gyroInstance[MAX_GYRO_COUNT]; STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn; STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; @@ -95,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); @@ -107,11 +115,12 @@ 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, - .gyro_to_use = 0, + .gyro_to_use = FIRST, .gyro_notch_hz = 0, .gyro_notch_cutoff = 1, .gyro_stage2_lowpass_hz = 0, @@ -254,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); @@ -267,46 +276,89 @@ 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) -#ifdef USE_DUAL_GYRO - gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use; -#else - gyroDev[0].imuSensorToUse = 0; -#endif + gyroSensor_e gyroHardware = gyroDetect(&instance->gyroDev, GYRO_AUTODETECT); - // Detecting gyro0 - gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); + // Fail early if not detected if (gyroHardware == GYRO_NONE) { - gyro.initialized = false; - detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; - return true; + return GYRO_NONE; } - // Gyro is initialized - gyro.initialized = true; - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; - sensorsSet(SENSOR_GYRO); + instance->gyroDev.lpf = gyroConfig()->gyro_lpf; + instance->gyroDev.requestedSampleIntervalUs = gyroConfig()->looptime; + instance->gyroDev.sampleRateIntervalUs = gyroConfig()->looptime; + instance->gyroDev.initFn(&instance->gyroDev); + + // Override default alignment + if (gyroAlign != ALIGN_DEFAULT) { + instance->gyroDev.gyroAlign = gyroAlign; + } + + instance->available = true; + + return gyroHardware; +} + +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; + + 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]); - // Driver initialisation - gyroDev[0].lpf = gyroConfig()->gyro_lpf; - gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime; - gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime; - gyroDev[0].initFn(&gyroDev[0]); + if (gyroHardware0 != GYRO_NONE && gyroHardware1 != GYRO_NONE) { + gyro.initialized = true; - // 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; + timeUs_t gyroSampleInterval0 = gyroInstance[0].gyroDev.sampleRateIntervalUs; + timeUs_t gyroSampleInterval1 = gyroInstance[1].gyroDev.sampleRateIntervalUs; - // 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; + 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 dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); gyroDataAnalyseStateInit( @@ -325,7 +377,13 @@ void gyroStartCalibration(void) return; } - zeroCalibrationStartV(&gyroCalibration[0], 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); + } } bool gyroIsCalibrationComplete(void) @@ -334,7 +392,18 @@ bool gyroIsCalibrationComplete(void) return true; } - 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) @@ -419,10 +488,38 @@ void FAST_CODE NOINLINE gyroUpdate() return; } - if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { + // 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++; + } + + // Fail early + if (activeGyros == 0) { return; } + // 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] float gyroADCf = gyro.gyroADCf[axis]; @@ -473,11 +570,23 @@ bool gyroReadTemperature(void) } // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10] - if (gyroDev[0].temperatureFn) { - return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]); + // TODO: [degC * 10] is a bug in Finland. Negative temperature... + + 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) @@ -486,7 +595,16 @@ int16_t gyroGetTemperature(void) return 0; } - 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) @@ -504,9 +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; + } + } + + // 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]); + // 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 b709294b553..2abb68d16fa 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 { + 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 #define DYN_NOTCH_RANGE_HZ_LOW 1000 @@ -58,11 +64,11 @@ typedef struct gyro_s { 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 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; 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