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/>.
23 #include "common/axis.h"
24 #include "common/maths.h"
26 #include "drivers/sensor.h"
27 #include "drivers/accgyro.h"
28 #include "flight/flight.h"
29 #include "sensors/sensors.h"
30 #include "io/statusindicator.h"
31 #include "sensors/boardalignment.h"
33 #include "sensors/gyro.h"
35 uint16_t calibratingG
= 0;
37 static gyroConfig_t
*gyroConfig
;
39 gyro_t gyro
; // gyro access functions
40 sensor_align_e gyroAlign
= 0;
42 void useGyroConfig(gyroConfig_t
*gyroConfigToUse
)
44 gyroConfig
= gyroConfigToUse
;
47 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired
)
49 calibratingG
= calibrationCyclesRequired
;
52 bool isGyroCalibrationComplete(void)
54 return calibratingG
== 0;
57 bool isOnFinalGyroCalibrationCycle(void)
59 return calibratingG
== 1;
62 bool isOnFirstGyroCalibrationCycle(void)
64 return calibratingG
== CALIBRATING_GYRO_CYCLES
;
67 static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold
)
71 static stdev_t var
[3];
73 for (axis
= 0; axis
< 3; axis
++) {
75 // Reset g[axis] at start of calibration
76 if (isOnFirstGyroCalibrationCycle()) {
81 // Sum up CALIBRATING_GYRO_CYCLES readings
82 g
[axis
] += gyroADC
[axis
];
83 devPush(&var
[axis
], gyroADC
[axis
]);
85 // Reset global variables to prevent other code from using un-calibrated data
89 if (isOnFinalGyroCalibrationCycle()) {
90 float dev
= devStandardDeviation(&var
[axis
]);
91 // check deviation and startover in case the model was moved
92 if (gyroMovementCalibrationThreshold
&& dev
> gyroMovementCalibrationThreshold
) {
93 gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES
);
96 gyroZero
[axis
] = (g
[axis
] + (CALIBRATING_GYRO_CYCLES
/ 2)) / CALIBRATING_GYRO_CYCLES
;
97 blinkLedAndSoundBeeper(10, 15, 1);
103 static void applyGyroZero(void)
106 for (axis
= 0; axis
< 3; axis
++) {
107 gyroADC
[axis
] -= gyroZero
[axis
];
111 void gyroUpdate(void)
113 // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time.
115 // range: +/- 8192; +/- 2000 deg/sec
117 alignSensors(gyroADC
, gyroADC
, gyroAlign
);
119 if (!isGyroCalibrationComplete()) {
120 performAcclerationCalibration(gyroConfig
->gyroMovementCalibrationThreshold
);