src/main/common/calibration.h

   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 #pragma once
  26
  27 #include <stdint.h>
  28 #include <string.h>
  29 #include <math.h>
  30
  31 #include "common/maths.h"
  32 #include "common/time.h"
  33 #include "common/vector.h"
  34
  35 typedef enum {
  36     ZERO_CALIBRATION_NONE = 0,
  37     ZERO_CALIBRATION_IN_PROGRESS,
  38     ZERO_CALIBRATION_DONE,
  39     ZERO_CALIBRATION_FAIL,
  40 } zeroCalibrationState_e;
  41
  42 typedef struct {
  43     zeroCalibrationState_e  state;
  44     timeMs_t                startTimeMs;
  45     timeMs_t                windowSizeMs;
  46     bool                    allowFailure;
  47     unsigned                sampleCount;
  48     float                   stdDevThreshold;
  49 } zeroCalibrationParams_t;
  50
  51 typedef struct {
  52     float                   accumulatedValue;
  53     stdev_t                 stdDev;
  54 } zeroCalibrationValue_t;
  55
  56 typedef struct {
  57     zeroCalibrationParams_t params;
  58     zeroCalibrationValue_t  val;
  59 } zeroCalibrationScalar_t;
  60
  61 typedef struct {
  62     zeroCalibrationParams_t params;
  63     zeroCalibrationValue_t  val[3];
  64 } zeroCalibrationVector_t;
  65
  66 void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure);
  67 bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s);
  68 bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s);
  69 void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v);
  70 void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v);
  71
  72 void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure);
  73 bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s);
  74 bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s);
  75 void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v);
  76 void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v);