Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-13 19:40:20 +03:00
Code Issues Wiki Activity

If Gyro errors > 100 we stop spending time with I2C

Browse source
This commit is contained in:
Bertrand Songis 2019-05-10 15:33:03 +02:00
parent 0af764bfab
commit bff7d5e0bd
2 changed files with 10 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

12
radio/src/gyro.cpp
View file

@ -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;