From 566d5805506bfe12a6bd7a64da1fa86dfc17ce3d Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Mon, 8 Jul 2019 21:46:51 +0200 Subject: [PATCH 1/4] [GYRO] Refactor gyro driver for dual-gyro support --- src/main/sensors/gyro.c | 78 ++++++++++++++++++++++++++--------------- 1 file changed, 49 insertions(+), 29 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c51466322e..3ec5128579 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -81,9 +81,7 @@ FASTRAM gyro_t gyro; // gyro sensor object STATIC_UNIT_TESTED gyroDev_t gyroDev0; // Not in FASTRAM since it may hold DMA buffers STATIC_FASTRAM int16_t gyroTemperature0; - -STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration; -STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT]; +STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration0; STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn; STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; @@ -256,10 +254,6 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard gyroHardware = GYRO_NONE; } - if (gyroHardware != GYRO_NONE) { - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; - sensorsSet(SENSOR_GYRO); - } return gyroHardware; } @@ -274,10 +268,16 @@ bool gyroInit(void) gyroDev0.imuSensorToUse = 0; #endif - if (gyroDetect(&gyroDev0, GYRO_AUTODETECT) == GYRO_NONE) { + // Detecting gyro0 + gyroSensor_e gyroHardware0 = gyroDetect(&gyroDev0, GYRO_AUTODETECT); + + if (gyroHardware0 == GYRO_NONE) { return false; } + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware0; + sensorsSet(SENSOR_GYRO); + // Driver initialisation gyroDev0.lpf = gyroConfig()->gyro_lpf; gyroDev0.requestedSampleIntervalUs = gyroConfig()->looptime; @@ -356,15 +356,15 @@ void gyroInitFilters(void) void gyroStartCalibration(void) { - zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); + zeroCalibrationStartV(&gyroCalibration0, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } bool gyroIsCalibrationComplete(void) { - return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration); + return zeroCalibrationIsCompleteV(&gyroCalibration0) && zeroCalibrationIsSuccessfulV(&gyroCalibration0); } -STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) +STATIC_UNIT_TESTED void performgyroCalibration0(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration0) { fpVector3_t v; @@ -373,11 +373,11 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVe v.v[1] = dev->gyroADCRaw[1]; v.v[2] = dev->gyroADCRaw[2]; - zeroCalibrationAddValueV(gyroCalibration, &v); + zeroCalibrationAddValueV(gyroCalibration0, &v); // Check if calibration is complete after this cycle - if (zeroCalibrationIsCompleteV(gyroCalibration)) { - zeroCalibrationGetZeroV(gyroCalibration, &v); + if (zeroCalibrationIsCompleteV(gyroCalibration0)) { + zeroCalibrationGetZeroV(gyroCalibration0, &v); dev->gyroZero[0] = v.v[0]; dev->gyroZero[1] = v.v[1]; dev->gyroZero[2] = v.v[2]; @@ -402,33 +402,53 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate) } } -void gyroUpdate() +static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf) { // range: +/- 8192; +/- 2000 deg/sec - if (gyroDev0.readFn(&gyroDev0)) { - if (zeroCalibrationIsCompleteV(&gyroCalibration)) { + if (gyroDev->readFn(gyroDev)) { + if (zeroCalibrationIsCompleteV(gyroCal)) { + int32_t gyroADCtmp[XYZ_AXIS_COUNT]; + // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment - 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]; - applySensorAlignment(gyroADC, gyroADC, gyroDev0.gyroAlign); - applyBoardAlignment(gyroADC); + gyroADCtmp[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X]; + gyroADCtmp[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y]; + gyroADCtmp[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z]; + + // Apply sensor alignment + applySensorAlignment(gyroADCtmp, gyroADCtmp, gyroDev->gyroAlign); + applyBoardAlignment(gyroADCtmp); + + // Convert to deg/s and store in unified data + gyroADCf[X] = (float)gyroADCtmp[X] * gyroDev->scale; + gyroADCf[Y] = (float)gyroADCtmp[Y] * gyroDev->scale; + gyroADCf[Z] = (float)gyroADCtmp[Z] * gyroDev->scale; + + return true; } else { - performGyroCalibration(&gyroDev0, &gyroCalibration); + performgyroCalibration0(gyroDev, gyroCal); + // Reset gyro values to zero to prevent other code from using uncalibrated data - gyro.gyroADCf[X] = 0.0f; - gyro.gyroADCf[Y] = 0.0f; - gyro.gyroADCf[Z] = 0.0f; - // still calibrating, so no need to further process gyro data - return; + gyroADCf[X] = 0.0f; + gyroADCf[Y] = 0.0f; + gyroADCf[Z] = 0.0f; + + return false; } } else { // no gyro reading to process + return false; + } +} + +void FAST_CODE NOINLINE gyroUpdate() +{ + if (!gyroUpdateAndCalibrate(&gyroDev0, &gyroCalibration0, gyro.gyroADCf)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale; + // At this point gyro.gyroADCf contains unfiltered gyro value + float gyroADCf = gyro.gyroADCf[axis]; DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); From 6d045b27c0025135cbcd5c2419ab9509bc839427 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 25 Apr 2020 14:13:21 +0200 Subject: [PATCH 2/4] [GYRO] More refactoring --- src/main/sensors/gyro.c | 62 ++++++++++++++++++++--------------------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 3ec5128579..28283aa7da 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -79,9 +79,11 @@ FILE_COMPILE_FOR_SPEED FASTRAM gyro_t gyro; // gyro sensor object -STATIC_UNIT_TESTED gyroDev_t gyroDev0; // Not in FASTRAM since it may hold DMA buffers -STATIC_FASTRAM int16_t gyroTemperature0; -STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration0; +#define MAX_GYRO_COUNT 1 + +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 filterApplyFnPtr gyroLpfApplyFn; STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT]; @@ -263,32 +265,30 @@ bool gyroInit(void) // Set inertial sensor tag (for dual-gyro selection) #ifdef USE_DUAL_GYRO - gyroDev0.imuSensorToUse = gyroConfig()->gyro_to_use; + gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use; #else - gyroDev0.imuSensorToUse = 0; + gyroDev[0].imuSensorToUse = 0; #endif // Detecting gyro0 - gyroSensor_e gyroHardware0 = gyroDetect(&gyroDev0, GYRO_AUTODETECT); - - if (gyroHardware0 == GYRO_NONE) { + gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); + if (gyroHardware == GYRO_NONE) { return false; } - - detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware0; + detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; sensorsSet(SENSOR_GYRO); // Driver initialisation - gyroDev0.lpf = gyroConfig()->gyro_lpf; - gyroDev0.requestedSampleIntervalUs = gyroConfig()->looptime; - gyroDev0.sampleRateIntervalUs = gyroConfig()->looptime; - gyroDev0.initFn(&gyroDev0); + gyroDev[0].lpf = gyroConfig()->gyro_lpf; + gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime; + gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime; + 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 ? gyroDev0.sampleRateIntervalUs : gyroConfig()->looptime; + gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime; if (gyroConfig()->gyro_align != ALIGN_DEFAULT) { - gyroDev0.gyroAlign = gyroConfig()->gyro_align; + gyroDev[0].gyroAlign = gyroConfig()->gyro_align; } gyroInitFilters(); @@ -356,15 +356,15 @@ void gyroInitFilters(void) void gyroStartCalibration(void) { - zeroCalibrationStartV(&gyroCalibration0, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); + zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } bool gyroIsCalibrationComplete(void) { - return zeroCalibrationIsCompleteV(&gyroCalibration0) && zeroCalibrationIsSuccessfulV(&gyroCalibration0); + return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } -STATIC_UNIT_TESTED void performgyroCalibration0(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration0) +STATIC_UNIT_TESTED void performgyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { fpVector3_t v; @@ -373,11 +373,11 @@ STATIC_UNIT_TESTED void performgyroCalibration0(gyroDev_t *dev, zeroCalibrationV v.v[1] = dev->gyroADCRaw[1]; v.v[2] = dev->gyroADCRaw[2]; - zeroCalibrationAddValueV(gyroCalibration0, &v); + zeroCalibrationAddValueV(gyroCalibration, &v); // Check if calibration is complete after this cycle - if (zeroCalibrationIsCompleteV(gyroCalibration0)) { - zeroCalibrationGetZeroV(gyroCalibration0, &v); + if (zeroCalibrationIsCompleteV(gyroCalibration)) { + zeroCalibrationGetZeroV(gyroCalibration, &v); dev->gyroZero[0] = v.v[0]; dev->gyroZero[1] = v.v[1]; dev->gyroZero[2] = v.v[2]; @@ -425,7 +425,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC return true; } else { - performgyroCalibration0(gyroDev, gyroCal); + performgyroCalibration(gyroDev, gyroCal); // Reset gyro values to zero to prevent other code from using uncalibrated data gyroADCf[X] = 0.0f; @@ -442,12 +442,12 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC void FAST_CODE NOINLINE gyroUpdate() { - if (!gyroUpdateAndCalibrate(&gyroDev0, &gyroCalibration0, gyro.gyroADCf)) { + if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { return; } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // At this point gyro.gyroADCf contains unfiltered gyro value + // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] float gyroADCf = gyro.gyroADCf[axis]; DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); @@ -493,8 +493,8 @@ void FAST_CODE NOINLINE gyroUpdate() bool gyroReadTemperature(void) { // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10] - if (gyroDev0.temperatureFn) { - return gyroDev0.temperatureFn(&gyroDev0, &gyroTemperature0); + if (gyroDev[0].temperatureFn) { + return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]); } return false; @@ -502,18 +502,18 @@ bool gyroReadTemperature(void) int16_t gyroGetTemperature(void) { - return gyroTemperature0; + return gyroTemperature[0]; } int16_t gyroRateDps(int axis) { - return lrintf(gyro.gyroADCf[axis] / gyroDev0.scale); + return lrintf(gyro.gyroADCf[axis]); } bool gyroSyncCheckUpdate(void) { - if (!gyroDev0.intStatusFn) + if (!gyroDev[0].intStatusFn) return false; - return gyroDev0.intStatusFn(&gyroDev0); + return gyroDev[0].intStatusFn(&gyroDev[0]); } From 4adb123e9020171f698ed6546908966cffc07aa7 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 25 Apr 2020 14:40:33 +0200 Subject: [PATCH 3/4] [GYRO] Fix tests --- src/main/sensors/gyro.c | 4 +- src/test/unit/sensor_gyro_unittest.cc | 55 +++++++++++++-------------- 2 files changed, 29 insertions(+), 30 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 28283aa7da..822a787a32 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -364,7 +364,7 @@ bool gyroIsCalibrationComplete(void) return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } -STATIC_UNIT_TESTED void performgyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) +STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration) { fpVector3_t v; @@ -425,7 +425,7 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC return true; } else { - performgyroCalibration(gyroDev, gyroCal); + performGyroCalibration(gyroDev, gyroCal); // Reset gyro values to zero to prevent other code from using uncalibrated data gyroADCf[X] = 0.0f; diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index a17312e4dc..0e49751519 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -39,7 +39,7 @@ extern "C" { #include "sensors/sensors.h" extern zeroCalibrationVector_t gyroCalibration; - extern gyroDev_t gyroDev0; + extern gyroDev_t gyroDev[]; STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware); STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration); @@ -50,9 +50,8 @@ extern "C" { TEST(SensorGyro, Detect) { - const gyroSensor_e detected = gyroDetect(&gyroDev0, GYRO_AUTODETECT); + const gyroSensor_e detected = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); EXPECT_EQ(GYRO_FAKE, detected); - EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]); } TEST(SensorGyro, Init) @@ -67,11 +66,11 @@ TEST(SensorGyro, Read) gyroInit(); EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]); fakeGyroSet(5, 6, 7); - const bool read = gyroDev0.readFn(&gyroDev0); + const bool read = gyroDev[0].readFn(&gyroDev[0]); EXPECT_EQ(true, read); - EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]); - EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]); - EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]); + EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]); + EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]); + EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]); } TEST(SensorGyro, Calibrate) @@ -79,25 +78,25 @@ TEST(SensorGyro, Calibrate) gyroStartCalibration(); gyroInit(); fakeGyroSet(5, 6, 7); - const bool read = gyroDev0.readFn(&gyroDev0); + const bool read = gyroDev[0].readFn(&gyroDev[0]); EXPECT_EQ(true, read); - EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]); - EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]); - EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]); - gyroDev0.gyroZero[X] = 8; - gyroDev0.gyroZero[Y] = 9; - gyroDev0.gyroZero[Z] = 10; - performGyroCalibration(&gyroDev0, &gyroCalibration); - EXPECT_EQ(0, gyroDev0.gyroZero[X]); - EXPECT_EQ(0, gyroDev0.gyroZero[Y]); - EXPECT_EQ(0, gyroDev0.gyroZero[Z]); + EXPECT_EQ(5, gyroDev[0].gyroADCRaw[X]); + EXPECT_EQ(6, gyroDev[0].gyroADCRaw[Y]); + EXPECT_EQ(7, gyroDev[0].gyroADCRaw[Z]); + gyroDev[0].gyroZero[X] = 8; + gyroDev[0].gyroZero[Y] = 9; + gyroDev[0].gyroZero[Z] = 10; + performGyroCalibration(&gyroDev[0], &gyroCalibration); + EXPECT_EQ(0, gyroDev[0].gyroZero[X]); + EXPECT_EQ(0, gyroDev[0].gyroZero[Y]); + EXPECT_EQ(0, gyroDev[0].gyroZero[Z]); EXPECT_EQ(false, gyroIsCalibrationComplete()); while (!gyroIsCalibrationComplete()) { - performGyroCalibration(&gyroDev0, &gyroCalibration); + performGyroCalibration(&gyroDev[0], &gyroCalibration); } - EXPECT_EQ(5, gyroDev0.gyroZero[X]); - EXPECT_EQ(6, gyroDev0.gyroZero[Y]); - EXPECT_EQ(7, gyroDev0.gyroZero[Z]); + EXPECT_EQ(5, gyroDev[0].gyroZero[X]); + EXPECT_EQ(6, gyroDev[0].gyroZero[Y]); + EXPECT_EQ(7, gyroDev[0].gyroZero[Z]); } TEST(SensorGyro, Update) @@ -112,9 +111,9 @@ TEST(SensorGyro, Update) gyroUpdate(); } EXPECT_EQ(true, gyroIsCalibrationComplete()); - EXPECT_EQ(5, gyroDev0.gyroZero[X]); - EXPECT_EQ(6, gyroDev0.gyroZero[Y]); - EXPECT_EQ(7, gyroDev0.gyroZero[Z]); + EXPECT_EQ(5, gyroDev[0].gyroZero[X]); + EXPECT_EQ(6, gyroDev[0].gyroZero[Y]); + EXPECT_EQ(7, gyroDev[0].gyroZero[Z]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[X]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Y]); EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); @@ -125,9 +124,9 @@ TEST(SensorGyro, Update) EXPECT_FLOAT_EQ(0, gyro.gyroADCf[Z]); fakeGyroSet(15, 26, 97); gyroUpdate(); - EXPECT_FLOAT_EQ(10 * gyroDev0.scale, gyro.gyroADCf[X]); // gyroADCf values are scaled - EXPECT_FLOAT_EQ(20 * gyroDev0.scale, gyro.gyroADCf[Y]); - EXPECT_FLOAT_EQ(90 * gyroDev0.scale, gyro.gyroADCf[Z]); + EXPECT_FLOAT_EQ(10 * gyroDev[0].scale, gyro.gyroADCf[X]); // gyroADCf values are scaled + EXPECT_FLOAT_EQ(20 * gyroDev[0].scale, gyro.gyroADCf[Y]); + EXPECT_FLOAT_EQ(90 * gyroDev[0].scale, gyro.gyroADCf[Z]); } From 121edfc86b7e231fa0223c17c7f9376cf5e65f5b Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Sat, 25 Apr 2020 14:57:48 +0200 Subject: [PATCH 4/4] [GYRO] Gracefully handle gyro detection failure --- src/main/sensors/diagnostics.c | 6 +- src/main/sensors/gyro.c | 138 ++++++++++++++++++++------------- src/main/sensors/gyro.h | 2 +- 3 files changed, 92 insertions(+), 54 deletions(-) diff --git a/src/main/sensors/diagnostics.c b/src/main/sensors/diagnostics.c index ca1c4c67f0..480eaa27ab 100644 --- a/src/main/sensors/diagnostics.c +++ b/src/main/sensors/diagnostics.c @@ -30,7 +30,11 @@ extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; hardwareSensorStatus_e getHwGyroStatus(void) { - // Gyro is assumed to be always healthy + // Gyro is assumed to be always healthy, but it must be present + if (detectedSensors[SENSOR_INDEX_GYRO] == GYRO_NONE) { + return HW_SENSOR_UNAVAILABLE; + } + return HW_SENSOR_OK; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 822a787a32..a12bc8ef15 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -259,6 +259,56 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard return gyroHardware; } +static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff) +{ + *applyFn = nullFilterApply; + if (cutoff > 0) { + switch (type) + { + case FILTER_PT1: + *applyFn = (filterApplyFnPtr)pt1FilterApply; + for (int axis = 0; axis < 3; axis++) { + pt1FilterInit(&state[axis].pt1, cutoff, getLooptime()* 1e-6f); + } + break; + case FILTER_BIQUAD: + *applyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < 3; axis++) { + biquadFilterInitLPF(&state[axis].biquad, cutoff, getLooptime()); + } + break; + } + } +} + +static void gyroInitFilters(void) +{ + STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; + notchFilter1ApplyFn = nullFilterApply; + + STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; + notchFilter2ApplyFn = 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); + + if (gyroConfig()->gyro_soft_notch_hz_1) { + notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < 3; axis++) { + notchFilter1[axis] = &gyroFilterNotch_1[axis]; + biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); + } + } + + if (gyroConfig()->gyro_soft_notch_hz_2) { + notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + for (int axis = 0; axis < 3; axis++) { + notchFilter2[axis] = &gyroFilterNotch_2[axis]; + biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); + } + } +} + bool gyroInit(void) { memset(&gyro, 0, sizeof(gyro)); @@ -273,8 +323,13 @@ bool gyroInit(void) // Detecting gyro0 gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT); if (gyroHardware == GYRO_NONE) { - return false; + gyro.initialized = false; + detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE; + return true; } + + // Gyro is initialized + gyro.initialized = true; detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware; sensorsSet(SENSOR_GYRO); @@ -304,63 +359,21 @@ bool gyroInit(void) return true; } -static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff) -{ - *applyFn = nullFilterApply; - if (cutoff > 0) { - switch (type) - { - case FILTER_PT1: - *applyFn = (filterApplyFnPtr)pt1FilterApply; - for (int axis = 0; axis < 3; axis++) { - pt1FilterInit(&state[axis].pt1, cutoff, getLooptime()* 1e-6f); - } - break; - case FILTER_BIQUAD: - *applyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - biquadFilterInitLPF(&state[axis].biquad, cutoff, getLooptime()); - } - break; - } - } -} - -void gyroInitFilters(void) -{ - STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; - notchFilter1ApplyFn = nullFilterApply; - - STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT]; - notchFilter2ApplyFn = 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); - - if (gyroConfig()->gyro_soft_notch_hz_1) { - notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); - } - } - - if (gyroConfig()->gyro_soft_notch_hz_2) { - notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - notchFilter2[axis] = &gyroFilterNotch_2[axis]; - biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); - } - } -} - void gyroStartCalibration(void) { + if (!gyro.initialized) { + return; + } + zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false); } bool gyroIsCalibrationComplete(void) { + if (!gyro.initialized) { + return true; + } + return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]); } @@ -442,6 +455,10 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC void FAST_CODE NOINLINE gyroUpdate() { + if (!gyro.initialized) { + return; + } + if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { return; } @@ -492,6 +509,10 @@ void FAST_CODE NOINLINE gyroUpdate() bool gyroReadTemperature(void) { + if (!gyro.initialized) { + return false; + } + // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10] if (gyroDev[0].temperatureFn) { return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]); @@ -502,18 +523,31 @@ bool gyroReadTemperature(void) int16_t gyroGetTemperature(void) { + if (!gyro.initialized) { + return 0; + } + return gyroTemperature[0]; } int16_t gyroRateDps(int axis) { + if (!gyro.initialized) { + return 0; + } + return lrintf(gyro.gyroADCf[axis]); } bool gyroSyncCheckUpdate(void) { - if (!gyroDev[0].intStatusFn) + if (!gyro.initialized) { return false; + } + + if (!gyroDev[0].intStatusFn) { + return false; + } return gyroDev[0].intStatusFn(&gyroDev[0]); } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 51e7fda3af..414d9042cc 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -50,6 +50,7 @@ typedef enum { #define DYN_NOTCH_RANGE_HZ_LOW 1000 typedef struct gyro_s { + bool initialized; uint32_t targetLooptime; float gyroADCf[XYZ_AXIS_COUNT]; } gyro_t; @@ -80,7 +81,6 @@ typedef struct gyroConfig_s { PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); -void gyroInitFilters(void); void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF); void gyroUpdate(void); void gyroStartCalibration(void);