src/main/common/calibration.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include <stdint.h>
  26 #include <string.h>
  27 #include <math.h>
  28
  29 #include "build/debug.h"
  30 #include "drivers/time.h"
  31 #include "common/calibration.h"
  32
  33
  34 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
  35 {
  36     // Reset parameters and state
  37     s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
  38     s->params.startTimeMs = 0;
  39     s->params.windowSizeMs = window;
  40     s->params.stdDevThreshold = threshold;
  41     s->params.allowFailure = allowFailure;
  42
  43     s->params.sampleCount = 0;
  44     s->val.accumulatedValue = 0;
  45     devClear(&s->val.stdDev);
  46 }
  47
  48 bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s)
  49 {
  50     return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
  51 }
  52
  53 bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s)
  54 {
  55     return (s->params.state == ZERO_CALIBRATION_DONE);
  56 }
  57
  58 void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
  59 {
  60     if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
  61         return;
  62     }
  63
  64     // An unknown delay may have passed between `zeroCalibrationStartS` and first sample acquisition
  65     // therefore our window measurement might be incorrect
  66     // To account for that we reset the startTimeMs when acquiring the first sample
  67     if (s->params.sampleCount == 0 && s->params.startTimeMs == 0) {
  68         s->params.startTimeMs = millis();
  69     }
  70
  71     // Add value
  72     s->val.accumulatedValue += v;
  73     s->params.sampleCount++;
  74     devPush(&s->val.stdDev, v);
  75
  76     // Check if calibration is complete
  77     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
  78         const float stddev = devStandardDeviation(&s->val.stdDev);
  79
  80         if (stddev > s->params.stdDevThreshold) {
  81             if (!s->params.allowFailure) {
  82                 // If deviation is too big && no failure allowed - restart calibration
  83                 // TODO :: some safeguard should exist which will not allow to keep on calibrating for ever
  84                 s->params.startTimeMs = millis();
  85                 s->params.sampleCount = 0;
  86                 s->val.accumulatedValue = 0;
  87                 devClear(&s->val.stdDev);
  88             }
  89             else {
  90                 // We are allowed to fail
  91                 s->params.state = ZERO_CALIBRATION_FAIL;
  92             }
  93         }
  94         else {
  95             // All seems ok - calculate average value
  96             s->val.accumulatedValue = s->val.accumulatedValue / s->params.sampleCount;
  97             s->params.state = ZERO_CALIBRATION_DONE;
  98         }
  99     }
 100 }
 101
 102 void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v)
 103 {
 104     if (s->params.state != ZERO_CALIBRATION_DONE) {
 105         *v = 0.0f;
 106     }
 107     else {
 108         *v = s->val.accumulatedValue;
 109     }
 110 }
 111
 112 void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure)
 113 {
 114     // Reset parameters and state
 115     s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
 116     s->params.startTimeMs = millis();
 117     s->params.windowSizeMs = window;
 118     s->params.stdDevThreshold = threshold;
 119     s->params.allowFailure = allowFailure;
 120
 121     s->params.sampleCount = 0;
 122     for (int i = 0; i < 3; i++) {
 123         s->val[i].accumulatedValue = 0;
 124         devClear(&s->val[i].stdDev);
 125     }
 126 }
 127
 128 bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s)
 129 {
 130     return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
 131 }
 132
 133 bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s)
 134 {
 135     return (s->params.state == ZERO_CALIBRATION_DONE);
 136 }
 137
 138 void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v)
 139 {
 140     if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
 141         return;
 142     }
 143
 144     // Add value
 145     for (int i = 0; i < 3; i++) {
 146         s->val[i].accumulatedValue += v->v[i];
 147         devPush(&s->val[i].stdDev, v->v[i]);
 148     }
 149
 150     s->params.sampleCount++;
 151
 152     // Check if calibration is complete
 153     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
 154         bool needRecalibration = false;
 155
 156         for (int i = 0; i < 3 && !needRecalibration; i++) {
 157             const float stddev = devStandardDeviation(&s->val[i].stdDev);
 158             //debug[i] = stddev;
 159             if (stddev > s->params.stdDevThreshold) {
 160                 needRecalibration = true;
 161             }
 162         }
 163
 164         if (needRecalibration) {
 165             if (!s->params.allowFailure) {
 166                 // If deviation is too big - restart calibration
 167                 s->params.startTimeMs = millis();
 168                 s->params.sampleCount = 0;
 169                 for (int i = 0; i < 3; i++) {
 170                     s->val[i].accumulatedValue = 0;
 171                     devClear(&s->val[i].stdDev);
 172                 }
 173             }
 174             else {
 175                 // We are allowed to fail
 176                 s->params.state = ZERO_CALIBRATION_FAIL;
 177             }
 178         }
 179         else {
 180             // All seems ok - calculate average value
 181             s->val[0].accumulatedValue = s->val[0].accumulatedValue / s->params.sampleCount;
 182             s->val[1].accumulatedValue = s->val[1].accumulatedValue / s->params.sampleCount;
 183             s->val[2].accumulatedValue = s->val[2].accumulatedValue / s->params.sampleCount;
 184             s->params.state = ZERO_CALIBRATION_DONE;
 185         }
 186     }
 187 }
 188
 189 void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v)
 190 {
 191     if (s->params.state != ZERO_CALIBRATION_DONE) {
 192         vectorZero(v);
 193     }
 194     else {
 195         v->v[0] = s->val[0].accumulatedValue;
 196         v->v[1] = s->val[1].accumulatedValue;
 197         v->v[2] = s->val[2].accumulatedValue;
 198     }
 199 }