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 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
  34 {
  35     // Reset parameters and state
  36     s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
  37     s->params.startTimeMs = 0;
  38     s->params.windowSizeMs = window;
  39     s->params.stdDevThreshold = threshold;
  40     s->params.allowFailure = allowFailure;
  41
  42     s->params.sampleCount = 0;
  43     s->val.accumulatedValue = 0;
  44     devClear(&s->val.stdDev);
  45 }
  46
  47 bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s)
  48 {
  49     return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
  50 }
  51
  52 bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s)
  53 {
  54     return (s->params.state == ZERO_CALIBRATION_DONE);
  55 }
  56
  57 void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
  58 {
  59     if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
  60         return;
  61     }
  62
  63     // An unknown delay may have passed between `zeroCalibrationStartS` and first sample acquisition
  64     // therefore our window measurement might be incorrect
  65     // To account for that we reset the startTimeMs when acquiring the first sample
  66     if (s->params.sampleCount == 0 && s->params.startTimeMs == 0) {
  67         s->params.startTimeMs = millis();
  68     }
  69
  70     // Add value
  71     s->val.accumulatedValue += v;
  72     s->params.sampleCount++;
  73     devPush(&s->val.stdDev, v);
  74
  75     // Check if calibration is complete
  76     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
  77         const float stddev = devStandardDeviation(&s->val.stdDev);
  78         if (stddev > s->params.stdDevThreshold) {
  79             if (!s->params.allowFailure) {
  80                 // If deviation is too big - restart calibration
  81                 s->params.startTimeMs = millis();
  82                 s->params.sampleCount = 0;
  83                 s->val.accumulatedValue = 0;
  84                 devClear(&s->val.stdDev);
  85             }
  86             else {
  87                 // We are allowed to fail
  88                 s->params.state = ZERO_CALIBRATION_FAIL;
  89             }
  90         }
  91         else {
  92             // All seems ok - calculate average value
  93             s->val.accumulatedValue = s->val.accumulatedValue / s->params.sampleCount;
  94             s->params.state = ZERO_CALIBRATION_DONE;
  95         }
  96     }
  97 }
  98
  99 void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v)
 100 {
 101     if (s->params.state != ZERO_CALIBRATION_DONE) {
 102         *v = 0.0f;
 103     }
 104     else {
 105         *v = s->val.accumulatedValue;
 106     }
 107 }
 108
 109 void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure)
 110 {
 111     // Reset parameters and state
 112     s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
 113     s->params.startTimeMs = millis();
 114     s->params.windowSizeMs = window;
 115     s->params.stdDevThreshold = threshold;
 116     s->params.allowFailure = allowFailure;
 117
 118     s->params.sampleCount = 0;
 119     for (int i = 0; i < 3; i++) {
 120         s->val[i].accumulatedValue = 0;
 121         devClear(&s->val[i].stdDev);
 122     }
 123 }
 124
 125 bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s)
 126 {
 127     return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
 128 }
 129
 130 bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s)
 131 {
 132     return (s->params.state == ZERO_CALIBRATION_DONE);
 133 }
 134
 135 void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v)
 136 {
 137     if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
 138         return;
 139     }
 140
 141     // Add value
 142     for (int i = 0; i < 3; i++) {
 143         s->val[i].accumulatedValue += v->v[i];
 144         devPush(&s->val[i].stdDev, v->v[i]);
 145     }
 146
 147     s->params.sampleCount++;
 148
 149     // Check if calibration is complete
 150     if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
 151         bool needRecalibration = false;
 152
 153         for (int i = 0; i < 3 && !needRecalibration; i++) {
 154             const float stddev = devStandardDeviation(&s->val[i].stdDev);
 155             //debug[i] = stddev;
 156             if (stddev > s->params.stdDevThreshold) {
 157                 needRecalibration = true;
 158             }
 159         }
 160
 161         if (needRecalibration) {
 162             if (!s->params.allowFailure) {
 163                 // If deviation is too big - restart calibration
 164                 s->params.startTimeMs = millis();
 165                 s->params.sampleCount = 0;
 166                 for (int i = 0; i < 3; i++) {
 167                     s->val[i].accumulatedValue = 0;
 168                     devClear(&s->val[i].stdDev);
 169                 }
 170             }
 171             else {
 172                 // We are allowed to fail
 173                 s->params.state = ZERO_CALIBRATION_FAIL;
 174             }
 175         }
 176         else {
 177             // All seems ok - calculate average value
 178             s->val[0].accumulatedValue = s->val[0].accumulatedValue / s->params.sampleCount;
 179             s->val[1].accumulatedValue = s->val[1].accumulatedValue / s->params.sampleCount;
 180             s->val[2].accumulatedValue = s->val[2].accumulatedValue / s->params.sampleCount;
 181             s->params.state = ZERO_CALIBRATION_DONE;
 182         }
 183     }
 184 }
 185
 186 void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v)
 187 {
 188     if (s->params.state != ZERO_CALIBRATION_DONE) {
 189         vectorZero(v);
 190     }
 191     else {
 192         v->v[0] = s->val[0].accumulatedValue;
 193         v->v[1] = s->val[1].accumulatedValue;
 194         v->v[2] = s->val[2].accumulatedValue;
 195     }
 196 }