src/main/sensors/acceleration.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   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.
   8  *
   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.
  13  *
  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/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/axis.h"
  25 #include "common/maths.h"
  26 #include "common/filter.h"
  27
  28 #include "drivers/sensor.h"
  29
  30 #include "sensors/battery.h"
  31 #include "sensors/sensors.h"
  32 #include "io/beeper.h"
  33 #include "sensors/boardalignment.h"
  34 #include "fc/runtime_config.h"
  35
  36 #include "config/config.h"
  37
  38 #include "sensors/acceleration.h"
  39
  40 acc_t acc;                       // acc access functions
  41 int32_t accADC[XYZ_AXIS_COUNT];
  42 sensor_align_e accAlign = 0;
  43 uint32_t accTargetLooptime;
  44
  45 static uint16_t calibratingA = 0;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
  46 static int16_t accADCRaw[XYZ_AXIS_COUNT];
  47
  48 static flightDynamicsTrims_t * accZero;
  49 static flightDynamicsTrims_t * accGain;
  50
  51 static uint8_t accLpfCutHz = 0;
  52 static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
  53
  54 void accInit(uint32_t targetLooptime)
  55 {
  56     accTargetLooptime = targetLooptime;
  57     if (accLpfCutHz) {
  58         for (int axis = 0; axis < 3; axis++) {
  59             biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
  60         }
  61     }
  62 }
  63
  64 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
  65 {
  66     calibratingA = calibrationCyclesRequired;
  67 }
  68
  69 bool isAccelerationCalibrationComplete(void)
  70 {
  71     return calibratingA == 0;
  72 }
  73
  74 bool isOnFinalAccelerationCalibrationCycle(void)
  75 {
  76     return calibratingA == 1;
  77 }
  78
  79 bool isOnFirstAccelerationCalibrationCycle(void)
  80 {
  81     return calibratingA == CALIBRATING_ACC_CYCLES;
  82 }
  83
  84 static sensorCalibrationState_t calState;
  85 static bool calibratedAxis[6];
  86 static int32_t accSamples[6][3];
  87 static int  calibratedAxisCount = 0;
  88
  89 int getPrimaryAxisIndex(int32_t sample[3])
  90 {
  91     if (ABS(sample[Z]) > ABS(sample[X]) && ABS(sample[Z]) > ABS(sample[Y])) {
  92         //Z-axis
  93         return (sample[Z] > 0) ? 0 : 1;
  94     }
  95     else if (ABS(sample[X]) > ABS(sample[Y]) && ABS(sample[X]) > ABS(sample[Z])) {
  96         //X-axis
  97         return (sample[X] > 0) ? 2 : 3;
  98     }
  99     else if (ABS(sample[Y]) > ABS(sample[X]) && ABS(sample[Y]) > ABS(sample[Z])) {
 100         //Y-axis
 101         return (sample[Y] > 0) ? 4 : 5;
 102     }
 103     else
 104         return -1;
 105 }
 106
 107 void performAcclerationCalibration(void)
 108 {
 109     int axisIndex = getPrimaryAxisIndex(accADC);
 110
 111     // Check if sample is usable
 112     if (axisIndex < 0) {
 113         return;
 114     }
 115
 116     // Top-up and first calibration cycle, reset everything
 117     if (axisIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
 118         for (int axis = 0; axis < 6; axis++) {
 119             calibratedAxis[axis] = false;
 120             accSamples[axis][X] = 0;
 121             accSamples[axis][Y] = 0;
 122             accSamples[axis][Z] = 0;
 123         }
 124
 125         calibratedAxisCount = 0;
 126         sensorCalibrationResetState(&calState);
 127     }
 128
 129     if (!calibratedAxis[axisIndex]) {
 130         sensorCalibrationPushSampleForOffsetCalculation(&calState, accADC);
 131         accSamples[axisIndex][X] += accADC[X];
 132         accSamples[axisIndex][Y] += accADC[Y];
 133         accSamples[axisIndex][Z] += accADC[Z];
 134
 135         if (isOnFinalAccelerationCalibrationCycle()) {
 136             calibratedAxis[axisIndex] = true;
 137             calibratedAxisCount++;
 138
 139             beeperConfirmationBeeps(2);
 140         }
 141     }
 142
 143     if (calibratedAxisCount == 6) {
 144         float accTmp[3];
 145         int32_t accSample[3];
 146
 147         /* Calculate offset */
 148         sensorCalibrationSolveForOffset(&calState, accTmp);
 149
 150         for (int axis = 0; axis < 3; axis++) {
 151             accZero->raw[axis] = lrintf(accTmp[axis]);
 152         }
 153
 154         /* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
 155         sensorCalibrationResetState(&calState);
 156
 157         for (int axis = 0; axis < 6; axis++) {
 158             accSample[X] = accSamples[axis][X] / CALIBRATING_ACC_CYCLES - accZero->raw[X];
 159             accSample[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
 160             accSample[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
 161
 162             sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.acc_1G);
 163         }
 164
 165         sensorCalibrationSolveForScale(&calState, accTmp);
 166
 167         for (int axis = 0; axis < 3; axis++) {
 168             accGain->raw[axis] = lrintf(accTmp[axis] * 4096);
 169         }
 170
 171         saveConfigAndNotify();
 172     }
 173
 174     calibratingA--;
 175 }
 176
 177 static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
 178 {
 179     accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
 180     accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
 181     accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
 182 }
 183
 184 void updateAccelerationReadings(void)
 185 {
 186     if (!acc.read(accADCRaw)) {
 187         return;
 188     }
 189
 190     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 191         accADC[axis] = accADCRaw[axis];
 192     }
 193
 194     if (accLpfCutHz) {
 195         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 196             accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float) accADC[axis]));
 197         }
 198     }
 199
 200     if (!isAccelerationCalibrationComplete()) {
 201         performAcclerationCalibration();
 202     }
 203
 204     applyAccelerationZero(accZero, accGain);
 205
 206     alignSensors(accADC, accAlign);
 207 }
 208
 209 void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse)
 210 {
 211     accZero = accZeroToUse;
 212 }
 213
 214 void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
 215 {
 216     accGain = accGainToUse;
 217 }
 218
 219 void setAccelerationFilter(uint8_t initialAccLpfCutHz)
 220 {
 221     accLpfCutHz = initialAccLpfCutHz;
 222     if (accTargetLooptime) {
 223         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 224             biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
 225         }
 226     }
 227 }