diff --git a/docs/Settings.md b/docs/Settings.md index 537f8239e30..47c551e6961 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -158,7 +158,6 @@ | gyro_notch_hz | 0 | | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | | gyro_stage2_lowpass_type | BIQUAD | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| gyro_sync | ON | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | | gyro_to_use | 0 | | | gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 46ed815100b..25ecb435af0 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -294,10 +294,6 @@ void validateAndFixConfig(void) } #endif -#if !defined(USE_MPU_DATA_READY_SIGNAL) - gyroConfigMutable()->gyroSync = false; -#endif - // Call target-specific validation function validateAndFixTargetConfig(); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f4b14791430..d6a6019fc64 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -101,9 +101,6 @@ enum { ALIGN_MAG = 2 }; -#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro -#define GYRO_SYNC_MAX_CONSECUTIVE_FAILURES 100 // After this many consecutive missed interrupts disable gyro sync and fall back to scheduled updates - #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 100 @@ -129,7 +126,6 @@ int16_t headFreeModeHold; uint8_t motorControlEnable = false; static bool isRXDataNew; -static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; static emergencyArmingState_t emergencyArming; @@ -809,36 +805,17 @@ void processRx(timeUs_t currentTimeUs) // Function for loop trigger void FAST_CODE taskGyro(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); - timeUs_t gyroUpdateUs = currentTimeUs; - - if (gyroConfig()->gyroSync) { - while (true) { - gyroUpdateUs = micros(); - if (gyroSyncCheckUpdate()) { - gyroSyncFailureCount = 0; - break; - } - else if ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (timeDelta_t)(getLooptime() + GYRO_WATCHDOG_DELAY)) { - gyroSyncFailureCount++; - break; - } - } - - // If we detect gyro sync failure - disable gyro sync - if (gyroSyncFailureCount > GYRO_SYNC_MAX_CONSECUTIVE_FAILURES) { - gyroConfigMutable()->gyroSync = false; - } - } /* Update actual hardware readings */ gyroUpdate(); #ifdef USE_OPFLOW if (sensors(SENSOR_OPFLOW)) { - opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs)); + opflowGyroUpdateCallback(currentDeltaTime); } #endif } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5fdcebc9e11..8f968440b52 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1229,7 +1229,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU16(dst, motorConfig()->motorPwmRate); sbufWriteU16(dst, servoConfig()->servoPwmRate); - sbufWriteU8(dst, gyroConfig()->gyroSync); + sbufWriteU8(dst, 0); break; case MSP_FILTER_CONFIG : @@ -2163,7 +2163,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) motorConfigMutable()->motorPwmProtocol = sbufReadU8(src); motorConfigMutable()->motorPwmRate = sbufReadU16(src); servoConfigMutable()->servoPwmRate = sbufReadU16(src); - gyroConfigMutable()->gyroSync = sbufReadU8(src); + sbufReadU8(src); //Was gyroSync } else return MSP_RESULT_ERROR; break; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 95b3df1bfc9..c6dff3e7ad4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -179,11 +179,6 @@ groups: description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible." default_value: 1000 max: 9000 - - name: gyro_sync - description: "This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf" - default_value: ON - field: gyroSync - type: bool - name: align_gyro description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP." default_value: "DEFAULT" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6a8710bff25..0d32b49bfd1 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -104,7 +104,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -113,7 +113,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_align = SETTING_ALIGN_GYRO_DEFAULT, .gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT, .looptime = SETTING_LOOPTIME_DEFAULT, - .gyroSync = SETTING_GYRO_SYNC_DEFAULT, #ifdef USE_DUAL_GYRO .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, #endif @@ -318,7 +317,7 @@ 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 = gyroDev[0].sampleRateIntervalUs; // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record // If configuration says different - override @@ -518,19 +517,6 @@ int16_t gyroRateDps(int axis) return lrintf(gyro.gyroADCf[axis]); } -bool gyroSyncCheckUpdate(void) -{ - if (!gyro.initialized) { - return false; - } - - if (!gyroDev[0].intStatusFn) { - return false; - } - - return gyroDev[0].intStatusFn(&gyroDev[0]); -} - void gyroUpdateDynamicLpf(float cutoffFreq) { if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 6aa3c11414f..b84477d6863 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -61,7 +61,6 @@ 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. - bool 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; @@ -95,5 +94,4 @@ bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); -bool gyroSyncCheckUpdate(void); void gyroUpdateDynamicLpf(float cutoffFreq); diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index d862359d49e..0428b0a2660 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -59,7 +59,6 @@ void targetConfiguration(void) gyroConfigMutable()->looptime = 1000; - gyroConfigMutable()->gyroSync = 1; gyroConfigMutable()->gyro_lpf = 0; // 256 Hz gyroConfigMutable()->gyro_soft_lpf_hz = 90; gyroConfigMutable()->gyro_notch_hz = 150;