2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
25 #include "common/axis.h"
26 #include "common/maths.h"
27 #include "common/filter.h"
29 #include "drivers/sensor.h"
30 #include "drivers/accgyro.h"
31 #include "drivers/gyro_sync.h"
32 #include "sensors/sensors.h"
33 #include "io/beeper.h"
34 #include "io/statusindicator.h"
35 #include "sensors/boardalignment.h"
37 #include "sensors/gyro.h"
39 uint16_t calibratingG
= 0;
40 int16_t gyroADC
[XYZ_AXIS_COUNT
];
41 int16_t gyroZero
[FLIGHT_DYNAMICS_INDEX_COUNT
] = { 0, 0, 0 };
43 static gyroConfig_t
*gyroConfig
;
44 static biquad_t gyroBiQuadState
[3];
45 static bool gyroFilterStateIsSet
;
46 static uint8_t gyroLpfCutFreq
;
49 gyro_t gyro
; // gyro access functions
50 sensor_align_e gyroAlign
= 0;
52 void useGyroConfig(gyroConfig_t
*gyroConfigToUse
, uint8_t *gyro_lpf_hz
)
54 gyroConfig
= gyroConfigToUse
;
55 gyroLpfCutFreq
= *gyro_lpf_hz
;
58 void initGyroFilterCoefficients(void) {
59 if (gyroLpfCutFreq
&& targetLooptime
) { /* Initialisation needs to happen once samplingrate is known */
60 for (axis
= 0; axis
< 3; axis
++) gyroBiQuadState
[axis
] = *(biquad_t
*)BiQuadNewLpf(gyroLpfCutFreq
);
61 gyroFilterStateIsSet
= true;
65 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired
)
67 calibratingG
= calibrationCyclesRequired
;
70 bool isGyroCalibrationComplete(void)
72 return calibratingG
== 0;
75 bool isOnFinalGyroCalibrationCycle(void)
77 return calibratingG
== 1;
80 bool isOnFirstGyroCalibrationCycle(void)
82 return calibratingG
== CALIBRATING_GYRO_CYCLES
;
85 static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold
)
89 static stdev_t var
[3];
91 for (axis
= 0; axis
< 3; axis
++) {
93 // Reset g[axis] at start of calibration
94 if (isOnFirstGyroCalibrationCycle()) {
99 // Sum up CALIBRATING_GYRO_CYCLES readings
100 g
[axis
] += gyroADC
[axis
];
101 devPush(&var
[axis
], gyroADC
[axis
]);
103 // Reset global variables to prevent other code from using un-calibrated data
107 if (isOnFinalGyroCalibrationCycle()) {
108 float dev
= devStandardDeviation(&var
[axis
]);
109 // check deviation and startover in case the model was moved
110 if (gyroMovementCalibrationThreshold
&& dev
> gyroMovementCalibrationThreshold
) {
111 gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES
);
114 gyroZero
[axis
] = (g
[axis
] + (CALIBRATING_GYRO_CYCLES
/ 2)) / CALIBRATING_GYRO_CYCLES
;
118 if (isOnFinalGyroCalibrationCycle()) {
119 beeper(BEEPER_GYRO_CALIBRATED
);
125 static void applyGyroZero(void)
128 for (axis
= 0; axis
< 3; axis
++) {
129 gyroADC
[axis
] -= gyroZero
[axis
];
133 void gyroUpdate(void)
135 // range: +/- 8192; +/- 2000 deg/sec
136 if (!gyro
.read(gyroADC
)) {
140 alignSensors(gyroADC
, gyroADC
, gyroAlign
);
142 if (gyroLpfCutFreq
) {
143 if (!gyroFilterStateIsSet
) {
144 initGyroFilterCoefficients();
146 for (axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) gyroADC
[axis
] = lrintf(applyBiQuadFilter((float) gyroADC
[axis
], &gyroBiQuadState
[axis
]));
150 if (!isGyroCalibrationComplete()) {
151 performAcclerationCalibration(gyroConfig
->gyroMovementCalibrationThreshold
);