Skip to content

Commit b034b67

Browse files
rtlopezrtlopez
committed
AccelSensor split
1 parent b866282 commit b034b67

File tree

2 files changed

+121
-103
lines changed

2 files changed

+121
-103
lines changed

‎lib/Espfc/src/Sensor/AccelSensor.cpp

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
#include "Sensor/AccelSensor.h"
2+
#include "Utils/FilterHelper.h"
3+
4+
namespace Espfc {
5+
6+
namespace Sensor {
7+
8+
AccelSensor::AccelSensor(Model& model): _model(model) {}
9+
10+
int AccelSensor::begin()
11+
{
12+
_model.state.accel.adc.z = ACCEL_G;
13+
14+
_gyro = _model.state.gyro.dev;
15+
if(!_gyro) return 0;
16+
17+
_model.state.accel.scale = 16.f * ACCEL_G / 32768.f;
18+
19+
for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
20+
{
21+
_filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate);
22+
}
23+
24+
_model.state.accel.biasAlpha = _model.state.accel.timer.rate > 0 ? 5.0f / _model.state.accel.timer.rate : 0;
25+
_model.state.accel.calibrationState = CALIBRATION_IDLE;
26+
27+
_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);
28+
29+
return 1;
30+
}
31+
32+
int FAST_CODE_ATTR AccelSensor::update()
33+
{
34+
int status = read();
35+
36+
if (status) filter();
37+
38+
return status;
39+
}
40+
41+
int FAST_CODE_ATTR AccelSensor::read()
42+
{
43+
if(!_model.accelActive()) return 0;
44+
45+
//if(!_model.state.accel.timer.check()) return 0;
46+
47+
Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ);
48+
_gyro->readAccel(_model.state.accel.raw);
49+
50+
return 1;
51+
}
52+
53+
int FAST_CODE_ATTR AccelSensor::filter()
54+
{
55+
if(!_model.accelActive()) return 0;
56+
57+
Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER);
58+
59+
_model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale;
60+
61+
align(_model.state.accel.adc, _model.config.gyro.align);
62+
_model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc);
63+
64+
for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
65+
{
66+
if(_model.config.debug.mode == DEBUG_ACCELEROMETER)
67+
{
68+
_model.state.debug[i] = _model.state.accel.raw[i];
69+
}
70+
_model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i]));
71+
_model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i]));
72+
}
73+
74+
calibrate();
75+
76+
return 1;
77+
}
78+
79+
void FAST_CODE_ATTR AccelSensor::calibrate()
80+
{
81+
switch(_model.state.accel.calibrationState)
82+
{
83+
case CALIBRATION_IDLE:
84+
_model.state.accel.adc -= _model.state.accel.bias;
85+
break;
86+
case CALIBRATION_START:
87+
_model.state.accel.bias = VectorFloat(0, 0, ACCEL_G);
88+
_model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate;
89+
_model.state.accel.calibrationState = CALIBRATION_UPDATE;
90+
break;
91+
case CALIBRATION_UPDATE:
92+
_model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha;
93+
_model.state.accel.biasSamples--;
94+
if(_model.state.accel.biasSamples <= 0) _model.state.accel.calibrationState = CALIBRATION_APPLY;
95+
break;
96+
case CALIBRATION_APPLY:
97+
_model.state.accel.bias.z -= ACCEL_G;
98+
_model.state.accel.calibrationState = CALIBRATION_SAVE;
99+
break;
100+
case CALIBRATION_SAVE:
101+
_model.finishCalibration();
102+
_model.state.accel.calibrationState = CALIBRATION_IDLE;
103+
break;
104+
default:
105+
_model.state.accel.calibrationState = CALIBRATION_IDLE;
106+
break;
107+
}
108+
}
109+
110+
}
111+
112+
}

‎lib/Espfc/src/Sensor/AccelSensor.h

Lines changed: 9 additions & 103 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
1-
#ifndef _ESPFC_SENSOR_ACCEL_SENSOR_H_
2-
#define _ESPFC_SENSOR_ACCEL_SENSOR_H_
1+
#pragma once
32

3+
#include "Model.h"
44
#include "BaseSensor.h"
55
#include "Device/GyroDevice.h"
66

@@ -11,115 +11,21 @@ namespace Sensor {
1111
class AccelSensor: public BaseSensor
1212
{
1313
public:
14-
AccelSensor(Model& model): _model(model) {}
14+
AccelSensor(Model& model);
1515

16-
int begin()
17-
{
18-
_model.state.accel.adc.z = ACCEL_G;
19-
20-
_gyro = _model.state.gyro.dev;
21-
if(!_gyro) return 0;
22-
23-
_model.state.accel.scale = 16.f * ACCEL_G / 32768.f;
24-
25-
for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
26-
{
27-
_filter[i].begin(FilterConfig(FILTER_FIR2, 1), _model.state.accel.timer.rate);
28-
}
29-
30-
_model.state.accel.biasAlpha = _model.state.accel.timer.rate > 0 ? 5.0f / _model.state.accel.timer.rate : 0;
31-
_model.state.accel.calibrationState = CALIBRATION_IDLE;
32-
33-
_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);
34-
35-
return 1;
36-
}
37-
38-
int update()
39-
{
40-
int status = read();
41-
42-
if (status) filter();
43-
44-
return status;
45-
}
46-
47-
int read()
48-
{
49-
if(!_model.accelActive()) return 0;
50-
51-
//if(!_model.state.accel.timer.check()) return 0;
52-
53-
Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_READ);
54-
_gyro->readAccel(_model.state.accel.raw);
55-
56-
return 1;
57-
}
58-
59-
int filter()
60-
{
61-
if(!_model.accelActive()) return 0;
62-
63-
Stats::Measure measure(_model.state.stats, COUNTER_ACCEL_FILTER);
64-
65-
_model.state.accel.adc = (VectorFloat)_model.state.accel.raw * _model.state.accel.scale;
66-
67-
align(_model.state.accel.adc, _model.config.gyro.align);
68-
_model.state.accel.adc = _model.state.boardAlignment.apply(_model.state.accel.adc);
69-
70-
for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
71-
{
72-
if(_model.config.debug.mode == DEBUG_ACCELEROMETER)
73-
{
74-
_model.state.debug[i] = _model.state.accel.raw[i];
75-
}
76-
_model.state.accel.adc.set(i, _filter[i].update(_model.state.accel.adc[i]));
77-
_model.state.accel.adc.set(i, _model.state.accel.filter[i].update(_model.state.accel.adc[i]));
78-
}
79-
80-
calibrate();
81-
82-
return 1;
83-
}
16+
int begin();
17+
int update();
18+
int read();
19+
int filter();
8420

8521
private:
86-
void calibrate()
87-
{
88-
switch(_model.state.accel.calibrationState)
89-
{
90-
case CALIBRATION_IDLE:
91-
_model.state.accel.adc -= _model.state.accel.bias;
92-
break;
93-
case CALIBRATION_START:
94-
_model.state.accel.bias = VectorFloat(0, 0, ACCEL_G);
95-
_model.state.accel.biasSamples = 2 * _model.state.accel.timer.rate;
96-
_model.state.accel.calibrationState = CALIBRATION_UPDATE;
97-
break;
98-
case CALIBRATION_UPDATE:
99-
_model.state.accel.bias += (_model.state.accel.adc - _model.state.accel.bias) * _model.state.accel.biasAlpha;
100-
_model.state.accel.biasSamples--;
101-
if(_model.state.accel.biasSamples <= 0) _model.state.accel.calibrationState = CALIBRATION_APPLY;
102-
break;
103-
case CALIBRATION_APPLY:
104-
_model.state.accel.bias.z -= ACCEL_G;
105-
_model.state.accel.calibrationState = CALIBRATION_SAVE;
106-
break;
107-
case CALIBRATION_SAVE:
108-
_model.finishCalibration();
109-
_model.state.accel.calibrationState = CALIBRATION_IDLE;
110-
break;
111-
default:
112-
_model.state.accel.calibrationState = CALIBRATION_IDLE;
113-
break;
114-
}
115-
}
22+
void calibrate();
11623

11724
Model& _model;
11825
Device::GyroDevice * _gyro;
119-
Filter _filter[3];
26+
Filter _filter[AXIS_COUNT_RPY];
12027
};
12128

12229
}
12330

12431
}
125-
#endif

0 commit comments

Comments
 (0)