[X-LiteS] Gyro added

radio/src/gyro.cpp

@@ -0,0 +1,70 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Based on code named
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "opentx.h"
+
+#define ACC_LSB_VALUE	0.000488  // 0.488 mg/LSB
+
+Gyro gyro;
+
+void GyroBuffer::read(int32_t values[GYRO_SAMPLES_COUNT])
+{
+  for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
+    sums[index] -= samples[index].values[i];
+  }
+
+  index = (index + 1) & (GYRO_SAMPLES_COUNT - 1);
+
+  gyroRead(samples[index].raw);
+
+  for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
+    sums[index] += samples[index].values[i];
+    values[i] = sums[index] >> GYRO_SAMPLES_EXPONENT;
+  }
+}
+
+float rad2RESX(float rad)
+{
+  return (rad * float(RESX)) / M_PI;
+}
+
+void Gyro::wakeup()
+{
+  static tmr10ms_t gyroWakeupTime = 0;
+
+  tmr10ms_t now = get_tmr10ms();
+  if (now < gyroWakeupTime)
+    return;
+
+  gyroWakeupTime = now + 1; /* 10ms default */
+
+  int32_t values[GYRO_VALUES_COUNT];
+  gyroBuffer.read(values);
+
+  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;
+
+  outputs[0] = rad2RESX(atan2f(accValues[1], accValues[2]));
+  outputs[1] = rad2RESX(atan2f(-accValues[0], accValues[2]));
+
+  // TRACE("%d %d", values[0], values[1]);
+}