radio/src/gyro.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22 #define _USE_MATH_DEFINES
  23 #include <math.h>
  24
  25 #define ACC_LSB_VALUE   0.000488  // 0.488 mg/LSB
  26
  27 Gyro gyro;
  28
  29 int GyroBuffer::read(int32_t values[GYRO_VALUES_COUNT])
  30 {
  31   index = (index + 1) & (GYRO_SAMPLES_COUNT - 1);
  32
  33   for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
  34     sums[i] -= samples[index].values[i];
  35   }
  36
  37   if (gyroRead(samples[index].raw) < 0)
  38     return -1;
  39
  40   for (uint8_t i = 0; i < GYRO_VALUES_COUNT; i++) {
  41     sums[i] += samples[index].values[i];
  42     values[i] = sums[i] >> GYRO_SAMPLES_EXPONENT;
  43   }
  44
  45   return 0;
  46 }
  47
  48 float rad2RESX(float rad)
  49 {
  50   return (rad * float(RESX)) / M_PI;
  51 }
  52
  53 void Gyro::wakeup()
  54 {
  55   static tmr10ms_t gyroWakeupTime = 0;
  56
  57   tmr10ms_t now = get_tmr10ms();
  58   if (errors >= 100 || now < gyroWakeupTime)
  59     return;
  60
  61   gyroWakeupTime = now + 1; /* 10ms default */
  62
  63   int32_t values[GYRO_VALUES_COUNT];
  64   if (gyroBuffer.read(values) < 0)
  65     ++errors;
  66
  67   float accValues[3]; // m^2 / s
  68   accValues[0] = -9.81 * float(values[3]) * ACC_LSB_VALUE;
  69   accValues[1] = 9.81 * float(values[4]) * ACC_LSB_VALUE;
  70   accValues[2] = 9.81 * float(values[5]) * ACC_LSB_VALUE;
  71
  72   outputs[0] = rad2RESX(atan2f(accValues[1], accValues[2]));
  73   outputs[1] = rad2RESX(atan2f(-accValues[0], accValues[2]));
  74
  75   // TRACE("%d %d", values[0], values[1]);
  76 }