piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

WiP: support for LSM6DSxx

Browse Source
This commit is contained in:
Raphael Coeffic
2022-06-25 16:09:04 +02:00
committed by Raphael Coeffic
parent df83a82285
commit 0b8d93ca88
44 changed files with 356 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/gyro.cpp
+21 -6
View File
@@ -27,20 +27,20 @@
Gyro gyro;
int GyroBuffer::read(int32_t values[GYRO_VALUES_COUNT])
int GyroBuffer::read(int32_t values[IMU_VALUES_COUNT])
{
index = (index + 1) & (GYRO_SAMPLES_COUNT - 1);
index = (index + 1) & (IMU_SAMPLES_COUNT - 1);
for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
for (uint8_t i = 0; i < IMU_VALUES_COUNT; i++) {
sums[i] -= samples[index].values[i];
}
if (gyroRead(samples[index].raw) < 0)
return -1;
for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
for (uint8_t i = 0; i < IMU_VALUES_COUNT; i++) {
sums[i] += samples[index].values[i];
values[i] = sums[i] >> GYRO_SAMPLES_EXPONENT;
values[i] = sums[i] >> IMU_SAMPLES_EXPONENT;
}
return 0;
@@ -61,7 +61,7 @@ void Gyro::wakeup()
gyroWakeupTime = now + 1; /* 10ms default */
int32_t values[GYRO_VALUES_COUNT];
int32_t values[IMU_VALUES_COUNT];
if (gyroBuffer.read(values) < 0)
++errors;
@@ -75,3 +75,18 @@ void Gyro::wakeup()
// TRACE("%d %d", values[0], values[1]);
}
int16_t Gyro::scaledX()
{
return limit(-RESX,
(int)(outputs[0] - g_eeGeneral.imuOffset * RESX / 180) *
(180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
RESX);
}
int16_t Gyro::scaledY()
{
return limit(-RESX,
outputs[1] * (180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
RESX);
}