[4.4.2] Remove 15 m/s limit on estimated vario (#12788)
[betaflight.git] / src / main / sensors / acceleration.c
blob1ee9cc9149e21efcb7ea2aa8534136e1bfcb6ea3
1 /*
2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <math.h>
25 #include "platform.h"
27 #ifdef USE_ACC
29 #include "build/debug.h"
31 #include "common/axis.h"
32 #include "common/filter.h"
33 #include "common/utils.h"
35 #include "config/feature.h"
37 #include "sensors/acceleration_init.h"
38 #include "sensors/boardalignment.h"
40 #include "acceleration.h"
42 FAST_DATA_ZERO_INIT acc_t acc; // acc access functions
44 static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
46 acc.accADC[X] -= accelerationTrims->raw[X];
47 acc.accADC[Y] -= accelerationTrims->raw[Y];
48 acc.accADC[Z] -= accelerationTrims->raw[Z];
51 void accUpdate(timeUs_t currentTimeUs)
53 UNUSED(currentTimeUs);
55 if (!acc.dev.readFn(&acc.dev)) {
56 return;
58 acc.isAccelUpdatedAtLeastOnce = true;
60 for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
61 const int16_t val = acc.dev.ADCRaw[axis];
62 acc.accADC[axis] = val;
65 if (acc.dev.accAlign == ALIGN_CUSTOM) {
66 alignSensorViaMatrix(acc.accADC, &acc.dev.rotationMatrix);
67 } else {
68 alignSensorViaRotation(acc.accADC, acc.dev.accAlign);
71 if (!accIsCalibrationComplete()) {
72 performAcclerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
75 if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) {
76 performInflightAccelerationCalibration(&accelerometerConfigMutable()->accelerometerTrims);
79 applyAccelerationTrims(accelerationRuntime.accelerationTrims);
81 for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
82 const int16_t val = acc.accADC[axis];
83 acc.accADC[axis] = accelerationRuntime.accLpfCutHz ? pt2FilterApply(&accelerationRuntime.accFilter[axis], val) : val;
87 #endif