esp-fc/lib/Espfc/src/Sensor/AccelSensor.cpp at e03199601585a20b58a9ba5083dba26ae9b70647 · rtlopez/esp-fc · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
#include "Sensor/AccelSensor.h"
#include "Utils/FilterHelper.h"

namespace Espfc::Sensor {

static constexpr float ESPFC_FUZZY_ACCEL_ZERO = 0.05f;
static constexpr float ESPFC_FUZZY_GYRO_ZERO = 0.20f;

AccelSensor::AccelSensor(Model& model): _model(model) {}

int AccelSensor::begin()
{
  _model.state.accel.adc.z = ACCEL_G;

  _gyro = _model.state.gyro.dev;
  if(!_gyro) return 0;

  _model.state.accel.scale = 16.f * ACCEL_G / 32768.f;

  for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
  {
    _filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate);
    _model.state.accel.filter[i].begin(_model.config.accel.filter, _model.state.accel.timer.rate);
  }

  _model.state.accel.biasAlpha = 5.0f / _model.state.accel.timer.rate;
  _model.state.accel.calibrationState = CALIBRATION_IDLE;

  _model.logger.info().log(F("ACCEL INIT")).log(FPSTR(Device::GyroDevice::getName(_gyro->getType()))).log(_gyro->getAddress()).log(_model.state.accel.timer.rate).log(_model.state.accel.timer.interval).logln(_model.state.accel.present);

  return 1;
}

int FAST_CODE_ATTR AccelSensor::update()
{
  int status = read();

  if (status) filter();

  return status;
}

int FAST_CODE_ATTR AccelSensor::read()
{
  if(!_model.accelActive()) return 0;

  Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ);
  _gyro->readAccel(_model.state.accel.raw);

  return 1;
}

int FAST_CODE_ATTR AccelSensor::filter()
{
  if(!_model.accelActive()) return 0;

  Utils::Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER);

  _model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale;

  align(_model.state.accel.adc, _model.config.gyro.align);
  _model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc);

  for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
  {
    if(_model.config.debug.mode == DEBUG_ACCELEROMETER)
    {
      _model.state.debug[i] = _model.state.accel.raw[i];
    }
    _model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i]));
    _model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i]));
  }

  calibrate();

  if(_model.state.accel.calibrationState == CALIBRATION_IDLE)
  {
    const float trimPitch = Utils::toRad(_model.config.accel.trim[0] * 0.1f);
    const float trimRoll = Utils::toRad(_model.config.accel.trim[1] * 0.1f);
    if(trimPitch != 0.f || trimRoll != 0.f)
    {
      RotationMatrixFloat trimRotation;
      trimRotation.init(VectorFloat(trimRoll, trimPitch, 0.f));
      _model.state.accel.adc = trimRotation.apply(_model.state.accel.adc);
    }
  }

  return 1;
}

void FAST_CODE_ATTR AccelSensor::calibrate()
{
  switch(_model.state.accel.calibrationState)
  {
    case CALIBRATION_IDLE:
      _model.state.accel.adc -= _model.state.accel.bias;
      break;
    case CALIBRATION_START:
      _model.state.accel.bias = VectorFloat(0, 0, ACCEL_G);
      _model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate;
      _model.state.accel.calibrationState = CALIBRATION_UPDATE;
      break;
    case CALIBRATION_UPDATE:
      _model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha;
      _model.state.accel.biasSamples--;
      if(_model.state.accel.biasSamples <= 0) _model.state.accel.calibrationState = CALIBRATION_APPLY;
      break;
    case CALIBRATION_APPLY:
      _model.state.accel.bias.z -= ACCEL_G;
      _model.state.accel.calibrationState = CALIBRATION_SAVE;
      break;
    case CALIBRATION_SAVE:
      _model.finishCalibration();
      _model.state.accel.calibrationState = CALIBRATION_IDLE;
      break;
    default:
      _model.state.accel.calibrationState = CALIBRATION_IDLE;
      break;
  }
}

}