diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp index a3c6a6244..9f40f0994 100644 --- a/radio/src/gyro.cpp +++ b/radio/src/gyro.cpp @@ -26,7 +26,7 @@ Gyro gyro; -void GyroBuffer::read(int32_t values[GYRO_VALUES_COUNT]) +int GyroBuffer::read(int32_t values[GYRO_VALUES_COUNT]) { index = (index + 1) & (GYRO_SAMPLES_COUNT - 1); @@ -34,12 +34,15 @@ void GyroBuffer::read(int32_t values[GYRO_VALUES_COUNT]) sums[i] -= samples[index].values[i]; } - gyroRead(samples[index].raw); + if (gyroRead(samples[index].raw) < 0) + return -1; for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) { sums[i] += samples[index].values[i]; values[i] = sums[i] >> GYRO_SAMPLES_EXPONENT; } + + return 0; } float rad2RESX(float rad) @@ -52,13 +55,14 @@ void Gyro::wakeup() static tmr10ms_t gyroWakeupTime = 0; tmr10ms_t now = get_tmr10ms(); - if (now < gyroWakeupTime) + if (errors >= 100 || now < gyroWakeupTime) return; gyroWakeupTime = now + 1; /* 10ms default */ int32_t values[GYRO_VALUES_COUNT]; - gyroBuffer.read(values); + if (gyroBuffer.read(values) < 0) + ++errors; float accValues[3]; // m^2 / s accValues[0] = -9.81 * float(values[3]) * ACC_LSB_VALUE; diff --git a/radio/src/gyro.h b/radio/src/gyro.h index 42f92eb7d..782fcd4ee 100644 --- a/radio/src/gyro.h +++ b/radio/src/gyro.h @@ -36,12 +36,13 @@ class GyroBuffer { uint8_t index; public: - void read(int32_t values[GYRO_VALUES_COUNT]); + int read(int32_t values[GYRO_VALUES_COUNT]); }; class Gyro { protected: GyroBuffer gyroBuffer; + uint8_t errors = 0; public: int16_t outputs[2];