Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-14 20:10:08 +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; 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); 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]; 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++) { for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
sums[i] += samples[index].values[i]; sums[i] += samples[index].values[i];
values[i] = sums[i] >> GYRO_SAMPLES_EXPONENT; values[i] = sums[i] >> GYRO_SAMPLES_EXPONENT;
} }
return 0;
} }
float rad2RESX(float rad) float rad2RESX(float rad)
@ -52,13 +55,14 @@ void Gyro::wakeup()
static tmr10ms_t gyroWakeupTime = 0; static tmr10ms_t gyroWakeupTime = 0;
tmr10ms_t now = get_tmr10ms(); tmr10ms_t now = get_tmr10ms();
if (now < gyroWakeupTime) if (errors >= 100 || now < gyroWakeupTime)
return; return;
gyroWakeupTime = now + 1; /* 10ms default */ gyroWakeupTime = now + 1; /* 10ms default */
int32_t values[GYRO_VALUES_COUNT]; int32_t values[GYRO_VALUES_COUNT];
gyroBuffer.read(values); if (gyroBuffer.read(values) < 0)
++errors;
float accValues[3]; // m^2 / s float accValues[3]; // m^2 / s
accValues[0] = -9.81 * float(values[3]) * ACC_LSB_VALUE; accValues[0] = -9.81 * float(values[3]) * ACC_LSB_VALUE;

3
radio/src/gyro.h
View file

@ -36,12 +36,13 @@ class GyroBuffer {
uint8_t index; uint8_t index;
public: public:
void read(int32_t values[GYRO_VALUES_COUNT]); int read(int32_t values[GYRO_VALUES_COUNT]);
}; };
class Gyro { class Gyro {
protected: protected:
GyroBuffer gyroBuffer; GyroBuffer gyroBuffer;
uint8_t errors = 0;
public: public:
int16_t outputs[2]; int16_t outputs[2];