Improve gyro/acc range and config (#2113)

radio/src/gyro.cpp

@@ -25,30 +25,17 @@
 
 #define ACC_LSB_VALUE	0.000488  // 0.488 mg/LSB
 
+#define ALPHA 0.98
+#define DT    0.01
+#define SCALE_FACT_ACC  0.005
+#define SCALE_FACT_GYRO 0.0078 // 250deg/s / 32000
+
 Gyro gyro;
 
-int GyroBuffer::read(int32_t values[IMU_VALUES_COUNT])
+static float deg2RESX(float deg)
 {
-  index = (index + 1) & (IMU_SAMPLES_COUNT - 1);
-
-  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 < IMU_VALUES_COUNT; i++) {
-    sums[i] += samples[index].values[i];
-    values[i] = sums[i] >> IMU_SAMPLES_EXPONENT;
-  }
-
-  return 0;
-}
-
-float rad2RESX(float rad)
-{
-  return (rad * float(RESX)) / M_PI;
+  // [-90 : 90] -> [-RESX : RESX]
+  return (deg * float(RESX)) / 90.0;
 }
 
 void Gyro::wakeup()
@@ -61,32 +48,53 @@ void Gyro::wakeup()
 
   gyroWakeupTime = now + 1; /* 10ms default */
 
-  int32_t values[IMU_VALUES_COUNT];
-  if (gyroBuffer.read(values) < 0)
+  int16_t values[IMU_VALUES_COUNT];
+  if (gyroRead((uint8_t*)values) < 0) {
     ++errors;
+    return;
+  }
 
-  float accValues[3]; // m^2 / s
-  accValues[0] = -9.81 * float(values[3]) * ACC_LSB_VALUE;
-  accValues[1] = 9.81 * float(values[4]) * ACC_LSB_VALUE;
-  accValues[2] = 9.81 * float(values[5]) * ACC_LSB_VALUE;
+  int16_t gx = values[0];
+  int16_t gy = values[1];
+  // int16_t gz = values[2];
 
-  outputs[0] = rad2RESX(atan2f(accValues[1], accValues[2]));
-  outputs[1] = rad2RESX(atan2f(-accValues[0], accValues[2]));
+  int16_t ax = values[3];
+  int16_t ay = values[4];
+  int16_t az = values[5];
 
-  // TRACE("%d %d", values[0], values[1]);
+  // integrate gyro
+  roll  -= gx * SCALE_FACT_GYRO * DT;
+  pitch += gy * SCALE_FACT_GYRO * DT;
+
+  int32_t magn = abs(ax) + abs(ay) + abs(az);
+  if (magn > 8192 && magn < 32768) {
+
+    if (az < 0) az = -az;
+        
+    float rollAcc  = atan2f((float)ay,(float)az) * 57.3 /* 180/PI */;
+    float pitchAcc = atan2f((float)ax,(float)az) * 57.3 /* 180/PI */;
+
+    roll  = roll  * ALPHA + rollAcc  * (1.0-ALPHA);
+    pitch = pitch * ALPHA + pitchAcc * (1.0-ALPHA);
+  }
+
+  outputs[0] = deg2RESX(roll);
+  outputs[1] = deg2RESX(pitch);
 }
 
 int16_t Gyro::scaledX()
 {
-  return limit(-RESX,
-               (int)(outputs[0] - g_eeGeneral.imuOffset * RESX / 180) *
-                   (180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
-               RESX);
+  // return limit(-RESX,
+  //              (int)(outputs[0] - g_eeGeneral.imuOffset * RESX / 180) *
+  //                  (180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
+  //              RESX);
+  return limit<int16_t>(-RESX, outputs[0], RESX);
 }
 
 int16_t Gyro::scaledY()
 {
-  return limit(-RESX,
-               outputs[1] * (180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
-               RESX);
+  // return limit(-RESX,
+  //              outputs[1] * (180 / (IMU_MAX_DEFAULT + g_eeGeneral.imuMax)),
+  //              RESX);
+  return limit<int16_t>(-RESX, outputs[1], RESX);
 }

radio/src/gyro.h

@@ -33,27 +33,12 @@
 #define IMU_SAMPLES_EXPONENT  3
 #define IMU_SAMPLES_COUNT     (2 ^ IMU_SAMPLES_EXPONENT)
 
-class GyroBuffer
-{
- protected:
-  union {
-    int16_t values[IMU_VALUES_COUNT];
-    uint8_t raw[IMU_BUFFER_LENGTH];
-  } samples[IMU_SAMPLES_COUNT];
-
-  int32_t sums[IMU_VALUES_COUNT];
-
-  uint8_t index;
-
- public:
-  int read(int32_t values[IMU_VALUES_COUNT]);
-};
-
 class Gyro
 {
  protected:
-  GyroBuffer gyroBuffer;
   uint8_t errors = 0;
+  float roll = 0.0;
+  float pitch = 0.0;
 
  public:
   int16_t outputs[2];

radio/src/targets/common/arm/stm32/lsm6ds_driver.cpp

@@ -195,12 +195,12 @@
 #define LSM6DS33TR_ID                           0x69
 
 static const char configure[][2] = {
-  {LSM6DS_ACCEL_AXIS_EN_ADDR, 0x38},
-  {LSM6DS_ACCEL_ODR_ADDR, (0x3 << 4) | (0x1 << 2) | (0x3 << 0)},
-  {LSM6DS_GYRO_AXIS_EN_ADDR, 0x38},
-  {LSM6DS_GYRO_ODR_ADDR, (3 << 4) | (3 << 2) | (0 << 0)},
-  {LSM6DS_INT1_CTRL_ADDR, 0x3},
-  {LSM6DS_INT2_CTRL_ADDR, 0x3},
+  // ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (+/-2 g full scale)
+  {LSM6DS_ACCEL_ODR_ADDR, 0x80},
+  // ODR = 1000 (1.66 kHz (high performance)); FS_XL = 00 (245 dps)
+  {LSM6DS_GYRO_ODR_ADDR, 0x80},
+  // IF_INC = 1 (automatically increment register address)
+  {LSM6DS_BDU_ADDR, 0x04}
 };
 
 #define I2C_TIMEOUT_MAX      10000
@@ -254,11 +254,6 @@ int gyroInit()
   if (id != LSM6DSLTR_ID && id != LSM6DS33TR_ID)
     return -1;
 
-  I2C_LSM6DS_WriteRegister(0x12, 0x05);
-  delay_ms(1);
-  I2C_LSM6DS_WriteRegister(0x12, 0x04);
-  delay_ms(1);
-
   for (uint8_t i = 0; i < DIM(configure); i++) {
     I2C_LSM6DS_WriteRegister(configure[i][0], configure[i][1]);
   }