Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Merge pull request #4202 from iNavFlight/de_runtime_cal_refactor

Browse source 
Refactor run-time calibration
This commit is contained in:
Konstantin Sharlaimov 2019-02-07 19:15:13 +01:00 • committed by GitHub
commit d2c60a60e5
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
19 changed files with 431 additions and 186 deletions
Download patch file Download diff file Expand all files Collapse all files

1
make/source.mk
View file

@ -11,6 +11,7 @@ COMMON_SRC = \
common/encoding.c \
common/filter.c \
common/maths.c \
common/calibration.c \
common/memory.c \
common/olc.c \
common/printf.c \

189
src/main/common/calibration.c Normal file
View file

@ -0,0 +1,189 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "build/debug.h"
#include "drivers/time.h"
#include "common/calibration.h"
void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure)
{
// Reset parameters and state
s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
s->params.startTimeMs = millis();
s->params.windowSizeMs = window;
s->params.stdDevThreshold = threshold;
s->params.allowFailure = allowFailure;
s->params.sampleCount = 0;
s->val.accumulatedValue = 0;
devClear(&s->val.stdDev);
}
bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s)
{
return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
}
bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s)
{
return (s->params.state == ZERO_CALIBRATION_DONE);
}
void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v)
{
if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
return;
}
// Add value
s->val.accumulatedValue += v;
s->params.sampleCount++;
devPush(&s->val.stdDev, v);
// Check if calibration is complete
if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
const float stddev = devStandardDeviation(&s->val.stdDev);
if (stddev > s->params.stdDevThreshold) {
if (!s->params.allowFailure) {
// If deviation is too big - restart calibration
s->params.startTimeMs = millis();
s->params.sampleCount = 0;
s->val.accumulatedValue = 0;
devClear(&s->val.stdDev);
}
else {
// We are allowed to fail
s->params.state = ZERO_CALIBRATION_FAIL;
}
}
else {
// All seems ok - calculate average value
s->val.accumulatedValue = s->val.accumulatedValue / s->params.sampleCount;
s->params.state = ZERO_CALIBRATION_DONE;
}
}
}
void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v)
{
if (s->params.state != ZERO_CALIBRATION_DONE) {
*v = 0.0f;
}
else {
*v = s->val.accumulatedValue;
}
}
void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure)
{
// Reset parameters and state
s->params.state = ZERO_CALIBRATION_IN_PROGRESS;
s->params.startTimeMs = millis();
s->params.windowSizeMs = window;
s->params.stdDevThreshold = threshold;
s->params.allowFailure = allowFailure;
s->params.sampleCount = 0;
for (int i = 0; i < 3; i++) {
s->val[i].accumulatedValue = 0;
devClear(&s->val[i].stdDev);
}
}
bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s)
{
return !(s->params.state == ZERO_CALIBRATION_IN_PROGRESS);
}
bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s)
{
return (s->params.state == ZERO_CALIBRATION_DONE);
}
void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v)
{
if (s->params.state != ZERO_CALIBRATION_IN_PROGRESS) {
return;
}
// Add value
for (int i = 0; i < 3; i++) {
s->val[i].accumulatedValue += v->v[i];
devPush(&s->val[i].stdDev, v->v[i]);
}
s->params.sampleCount++;
// Check if calibration is complete
if ((millis() - s->params.startTimeMs) > s->params.windowSizeMs) {
bool needRecalibration = false;
for (int i = 0; i < 3 && !needRecalibration; i++) {
const float stddev = devStandardDeviation(&s->val[i].stdDev);
//debug[i] = stddev;
if (stddev > s->params.stdDevThreshold) {
needRecalibration = true;
}
}
if (needRecalibration) {
if (!s->params.allowFailure) {
// If deviation is too big - restart calibration
s->params.startTimeMs = millis();
s->params.sampleCount = 0;
for (int i = 0; i < 3; i++) {
s->val[i].accumulatedValue = 0;
devClear(&s->val[i].stdDev);
}
}
else {
// We are allowed to fail
s->params.state = ZERO_CALIBRATION_FAIL;
}
}
else {
// All seems ok - calculate average value
s->val[0].accumulatedValue = s->val[0].accumulatedValue / s->params.sampleCount;
s->val[1].accumulatedValue = s->val[1].accumulatedValue / s->params.sampleCount;
s->val[2].accumulatedValue = s->val[2].accumulatedValue / s->params.sampleCount;
s->params.state = ZERO_CALIBRATION_DONE;
}
}
}
void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v)
{
if (s->params.state != ZERO_CALIBRATION_DONE) {
vectorZero(v);
}
else {
v->v[0] = s->val[0].accumulatedValue;
v->v[1] = s->val[1].accumulatedValue;
v->v[2] = s->val[2].accumulatedValue;
}
}

76
src/main/common/calibration.h Normal file
View file

@ -0,0 +1,76 @@
/*
* This file is part of INAV Project.
*
* This Source Code Form is subject to the terms of the Mozilla Public
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
* You can obtain one at http://mozilla.org/MPL/2.0/.
*
* Alternatively, the contents of this file may be used under the terms
* of the GNU General Public License Version 3, as described below:
*
* This file is free software: you may copy, redistribute and/or modify
* it under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or (at your
* option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
* Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*/
#pragma once
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "common/maths.h"
#include "common/time.h"
#include "common/vector.h"
typedef enum {
ZERO_CALIBRATION_NONE,
ZERO_CALIBRATION_IN_PROGRESS,
ZERO_CALIBRATION_DONE,
ZERO_CALIBRATION_FAIL,
} zeroCalibrationState_e;
typedef struct {
zeroCalibrationState_e state;
timeMs_t startTimeMs;
timeMs_t windowSizeMs;
bool allowFailure;
unsigned sampleCount;
float stdDevThreshold;
} zeroCalibrationParams_t;
typedef struct {
float accumulatedValue;
stdev_t stdDev;
} zeroCalibrationValue_t;
typedef struct {
zeroCalibrationParams_t params;
zeroCalibrationValue_t val;
} zeroCalibrationScalar_t;
typedef struct {
zeroCalibrationParams_t params;
zeroCalibrationValue_t val[3];
} zeroCalibrationVector_t;
void zeroCalibrationStartS(zeroCalibrationScalar_t * s, timeMs_t window, float threshold, bool allowFailure);
bool zeroCalibrationIsCompleteS(zeroCalibrationScalar_t * s);
bool zeroCalibrationIsSuccessfulS(zeroCalibrationScalar_t * s);
void zeroCalibrationAddValueS(zeroCalibrationScalar_t * s, const float v);
void zeroCalibrationGetZeroS(zeroCalibrationScalar_t * s, float * v);
void zeroCalibrationStartV(zeroCalibrationVector_t * s, timeMs_t window, float threshold, bool allowFailure);
bool zeroCalibrationIsCompleteV(zeroCalibrationVector_t * s);
bool zeroCalibrationIsSuccessfulV(zeroCalibrationVector_t * s);
void zeroCalibrationAddValueV(zeroCalibrationVector_t * s, const fpVector3_t * v);
void zeroCalibrationGetZeroV(zeroCalibrationVector_t * s, fpVector3_t * v);

3
src/main/fc/fc_init.c
View file

@ -629,7 +629,8 @@ void init(void)
blackboxInit();
#endif
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroStartCalibration();
#ifdef USE_BARO
baroStartCalibration();
#endif

2
src/main/fc/fc_msp.c
View file

@ -2068,7 +2068,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_ACC_CALIBRATION:
if (!ARMING_FLAG(ARMED))
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
accStartCalibration();
else
return MSP_RESULT_ERROR;
break;

4
src/main/fc/rc_controls.c
View file

@ -235,7 +235,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// GYRO calibration
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroStartCalibration();
return;
}
@ -317,7 +317,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
// Calibrating Acc
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
accStartCalibration();
return;
}

40
src/main/navigation/navigation_pos_estimator.c
View file

@ -342,18 +342,24 @@ void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs)
* Update IMU topic
* Function is called at main loop rate
*/
static void restartGravityCalibration(void)
{
zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false);
}
static bool gravityCalibrationComplete(void)
{
return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
}
static void updateIMUTopic(void)
{
static float calibratedGravityCMSS = GRAVITY_CMSS;
static timeMs_t gravityCalibrationTimeout = 0;
if (!isImuReady()) {
posEstimator.imu.accelNEU.x = 0;
posEstimator.imu.accelNEU.y = 0;
posEstimator.imu.accelNEU.z = 0;
gravityCalibrationTimeout = millis();
posEstimator.imu.gravityCalibrationComplete = false;
restartGravityCalibration();
}
else {
fpVector3_t accelBF;
@ -377,24 +383,18 @@ static void updateIMUTopic(void)
posEstimator.imu.accelNEU.z = accelBF.z;
/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
if (!ARMING_FLAG(ARMED) && !posEstimator.imu.gravityCalibrationComplete) {
// Slowly converge on calibrated gravity while level
const float gravityOffsetError = posEstimator.imu.accelNEU.z - calibratedGravityCMSS;
calibratedGravityCMSS += gravityOffsetError * 0.0025f;
if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);
if (fabsf(gravityOffsetError) < positionEstimationConfig()->gravity_calibration_tolerance) { // Error should be within 0.5% of calibrated gravity
if ((millis() - gravityCalibrationTimeout) > 250) {
posEstimator.imu.gravityCalibrationComplete = true;
}
}
else {
gravityCalibrationTimeout = millis();
if (gravityCalibrationComplete()) {
zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
DEBUG_TRACE_SYNC("Gravity calibration complete (%d)", lrintf(posEstimator.imu.calibratedGravityCMSS));
}
}
/* If calibration is incomplete - report zero acceleration */
if (posEstimator.imu.gravityCalibrationComplete) {
posEstimator.imu.accelNEU.z -= calibratedGravityCMSS;
if (gravityCalibrationComplete()) {
posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
}
else {
posEstimator.imu.accelNEU.x = 0;
@ -768,7 +768,7 @@ void initializePositionEstimator(void)
posEstimator.est.flowCoordinates[X] = 0;
posEstimator.est.flowCoordinates[Y] = 0;
posEstimator.imu.gravityCalibrationComplete = false;
restartGravityCalibration();
for (axis = 0; axis < 3; axis++) {
posEstimator.imu.accelBias.v[axis] = 0;
@ -807,7 +807,7 @@ void updatePositionEstimator(void)
bool navIsCalibrationComplete(void)
{
return posEstimator.imu.gravityCalibrationComplete;
return gravityCalibrationComplete();
}
#endif

10
src/main/navigation/navigation_pos_estimator_private.h
View file

@ -23,6 +23,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "common/calibration.h"
#include "sensors/sensors.h"
@ -44,6 +45,8 @@
#define INAV_SURFACE_TIMEOUT_MS 400 // Surface timeout (missed 3 readings in a row)
#define INAV_FLOW_TIMEOUT_MS 200
#define CALIBRATING_GRAVITY_TIME_MS 2000
// Time constants for calculating Baro/Sonar averages. Should be the same value to impose same amount of group delay
#define INAV_BARO_AVERAGE_HZ 1.0f
#define INAV_SURFACE_AVERAGE_HZ 1.0f
@ -123,9 +126,10 @@ typedef struct {
} navPositionEstimatorESTIMATE_t;
typedef struct {
fpVector3_t accelNEU;
fpVector3_t accelBias;
bool gravityCalibrationComplete;
fpVector3_t accelNEU;
fpVector3_t accelBias;
float calibratedGravityCMSS;
zeroCalibrationScalar_t gravityCalibration;
} navPosisitonEstimatorIMU_t;
typedef enum {

82
src/main/sensors/acceleration.c
View file

@ -27,6 +27,7 @@
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
#include "common/calibration.h"
#include "config/config_reset.h"
#include "config/parameter_group.h"
@ -70,7 +71,7 @@
FASTRAM acc_t acc; // acc access functions
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.
STATIC_FASTRAM zeroCalibrationVector_t zeroCalibration;
STATIC_FASTRAM int32_t accADC[XYZ_AXIS_COUNT];
@ -341,26 +342,6 @@ bool accInit(uint32_t targetLooptime)
return true;
}
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
calibratingA = calibrationCyclesRequired;
}
bool accIsCalibrationComplete(void)
{
return calibratingA == 0;
}
static bool isOnFinalAccelerationCalibrationCycle(void)
{
return calibratingA == 1;
}
static bool isOnFirstAccelerationCalibrationCycle(void)
{
return calibratingA == CALIBRATING_ACC_CYCLES;
}
static bool calibratedPosition[6];
static int32_t accSamples[6][3];
static int calibratedAxisCount = 0;
@ -406,17 +387,21 @@ static int getPrimaryAxisIndex(int32_t accADCData[3])
return -1;
}
static void performAcclerationCalibration(void)
bool accIsCalibrationComplete(void)
{
return zeroCalibrationIsCompleteV(&zeroCalibration);
}
void accStartCalibration(void)
{
int positionIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (positionIndex < 0) {
return;
}
// Top-up and first calibration cycle, reset everything
if (positionIndex == 0 && isOnFirstAccelerationCalibrationCycle()) {
// Top+up and first calibration cycle, reset everything
if (positionIndex == 0) {
for (int axis = 0; axis < 6; axis++) {
calibratedPosition[axis] = false;
accSamples[axis][X] = 0;
@ -428,19 +413,40 @@ static void performAcclerationCalibration(void)
DISABLE_STATE(ACCELEROMETER_CALIBRATED);
}
zeroCalibrationStartV(&zeroCalibration, CALIBRATING_ACC_TIME_MS, acc.dev.acc_1G * 0.01f, true);
}
static void performAcclerationCalibration(void)
{
fpVector3_t v;
int positionIndex = getPrimaryAxisIndex(accADC);
// Check if sample is usable
if (positionIndex < 0) {
return;
}
if (!calibratedPosition[positionIndex]) {
accSamples[positionIndex][X] += accADC[X];
accSamples[positionIndex][Y] += accADC[Y];
accSamples[positionIndex][Z] += accADC[Z];
v.v[0] = accADC[0];
v.v[1] = accADC[1];
v.v[2] = accADC[2];
if (isOnFinalAccelerationCalibrationCycle()) {
calibratedPosition[positionIndex] = true;
zeroCalibrationAddValueV(&zeroCalibration, &v);
accSamples[positionIndex][X] = accSamples[positionIndex][X] / CALIBRATING_ACC_CYCLES;
accSamples[positionIndex][Y] = accSamples[positionIndex][Y] / CALIBRATING_ACC_CYCLES;
accSamples[positionIndex][Z] = accSamples[positionIndex][Z] / CALIBRATING_ACC_CYCLES;
if (zeroCalibrationIsCompleteV(&zeroCalibration)) {
if (zeroCalibrationIsSuccessfulV(&zeroCalibration)) {
zeroCalibrationGetZeroV(&zeroCalibration, &v);
calibratedAxisCount++;
accSamples[positionIndex][X] = v.v[X];
accSamples[positionIndex][Y] = v.v[Y];
accSamples[positionIndex][Z] = v.v[Z];
calibratedPosition[positionIndex] = true;
calibratedAxisCount++;
}
else {
calibratedPosition[positionIndex] = false;
}
beeperConfirmationBeeps(2);
}
@ -459,9 +465,9 @@ static void performAcclerationCalibration(void)
sensorCalibrationSolveForOffset(&calState, accTmp);
for (int axis = 0; axis < 3; axis++) {
accelerometerConfigMutable()->accZero.raw[axis] = lrintf(accTmp[axis]);
}
accelerometerConfigMutable()->accZero.raw[X] = lrintf(accTmp[X]);
accelerometerConfigMutable()->accZero.raw[Y] = lrintf(accTmp[Y]);
accelerometerConfigMutable()->accZero.raw[Z] = lrintf(accTmp[Z]);
/* Not we can offset our accumulated averages samples and calculate scale factors and calculate gains */
sensorCalibrationResetState(&calState);
@ -484,8 +490,6 @@ static void performAcclerationCalibration(void)
saveConfigAndNotify();
}
calibratingA--;
}
static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)

2
src/main/sensors/acceleration.h
View file

@ -73,7 +73,7 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
bool accInit(uint32_t accTargetLooptime);
bool accIsCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void accStartCalibration(void);
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
void accGetVibrationLevels(fpVector3_t *accVibeLevels);
float accGetVibrationLevel(void);

49
src/main/sensors/barometer.c
View file

@ -26,6 +26,7 @@
#include "common/maths.h"
#include "common/time.h"
#include "common/utils.h"
#include "common/calibration.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
@ -66,8 +67,7 @@ PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
#ifdef USE_BARO
static timeMs_t baroCalibrationTimeout = 0;
static bool baroCalibrationFinished = false;
static zeroCalibrationScalar_t zeroCalibration;
static float baroGroundAltitude = 0;
static float baroGroundPressure = 101325.0f; // 101325 pascal, 1 standard atmosphere
@ -183,17 +183,6 @@ bool baroInit(void)
return true;
}
bool baroIsCalibrationComplete(void)
{
return baroCalibrationFinished;
}
void baroStartCalibration(void)
{
baroCalibrationTimeout = millis();
baroCalibrationFinished = false;
}
#define PRESSURE_SAMPLES_MEDIAN 3
/*
@ -275,27 +264,33 @@ static float pressureToAltitude(const float pressure)
return (1.0f - powf(pressure / 101325.0f, 0.190295f)) * 4433000.0f;
}
static void performBaroCalibrationCycle(void)
static float altitudeToPressure(const float altCm)
{
const float baroGroundPressureError = baro.baroPressure - baroGroundPressure;
baroGroundPressure += baroGroundPressureError * 0.15f;
return powf(1.0f - (altCm / 4433000.0f), 5.254999) * 101325.0f;
}
if (fabsf(baroGroundPressureError) < (baroGroundPressure * 0.00005f)) { // 0.005% calibration error (should give c. 10cm calibration error)
if ((millis() - baroCalibrationTimeout) > 250) {
baroGroundAltitude = pressureToAltitude(baroGroundPressure);
baroCalibrationFinished = true;
DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
}
}
else {
baroCalibrationTimeout = millis();
}
bool baroIsCalibrationComplete(void)
{
return zeroCalibrationIsCompleteS(&zeroCalibration) && zeroCalibrationIsSuccessfulS(&zeroCalibration);
}
void baroStartCalibration(void)
{
const float acceptedPressureVariance = (101325.0f - altitudeToPressure(30.0f)); // max 30cm deviation during calibration (at sea level)
zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false);
}
int32_t baroCalculateAltitude(void)
{
if (!baroIsCalibrationComplete()) {
performBaroCalibrationCycle();
zeroCalibrationAddValueS(&zeroCalibration, baro.baroPressure);
if (zeroCalibrationIsCompleteS(&zeroCalibration)) {
zeroCalibrationGetZeroS(&zeroCalibration, &baroGroundPressure);
baroGroundAltitude = pressureToAltitude(baroGroundPressure);
DEBUG_TRACE_SYNC("Barometer calibration complete (%d)", lrintf(baroGroundAltitude));
}
baro.BaroAlt = 0;
}
else {

77
src/main/sensors/gyro.c
View file

@ -27,6 +27,7 @@
#include "common/axis.h"
#include "common/maths.h"
#include "common/calibration.h"
#include "common/filter.h"
#include "common/utils.h"
@ -74,13 +75,7 @@ FASTRAM gyro_t gyro; // gyro sensor object
STATIC_UNIT_TESTED gyroDev_t gyroDev0; // Not in FASTRAM since it may hold DMA buffers
STATIC_FASTRAM int16_t gyroTemperature0;
typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT];
stdev_t var[XYZ_AXIS_COUNT];
uint16_t calibratingG;
} gyroCalibration_t;
STATIC_FASTRAM_UNIT_TESTED gyroCalibration_t gyroCalibration;
STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration;
STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn;
@ -344,64 +339,42 @@ void gyroInitFilters(void)
#endif
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
void gyroStartCalibration(void)
{
gyroCalibration.calibratingG = calibrationCyclesRequired;
}
STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration)
{
return gyroCalibration->calibratingG == 0;
zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
}
bool gyroIsCalibrationComplete(void)
{
return gyroCalibration.calibratingG == 0;
return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration);
}
static bool isOnFinalGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
{
return gyroCalibration->calibratingG == 1;
}
fpVector3_t v;
static bool isOnFirstGyroCalibrationCycle(gyroCalibration_t *gyroCalibration)
{
return gyroCalibration->calibratingG == CALIBRATING_GYRO_CYCLES;
}
// Consume gyro reading
v.v[0] = dev->gyroADCRaw[0];
v.v[1] = dev->gyroADCRaw[1];
v.v[2] = dev->gyroADCRaw[2];
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold)
{
for (int axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(gyroCalibration)) {
gyroCalibration->g[axis] = 0;
devClear(&gyroCalibration->var[axis]);
}
zeroCalibrationAddValueV(gyroCalibration, &v);
// Sum up CALIBRATING_GYRO_CYCLES readings
gyroCalibration->g[axis] += dev->gyroADCRaw[axis];
devPush(&gyroCalibration->var[axis], dev->gyroADCRaw[axis]);
// Check if calibration is complete after this cycle
if (zeroCalibrationIsCompleteV(gyroCalibration)) {
zeroCalibrationGetZeroV(gyroCalibration, &v);
dev->gyroZero[0] = v.v[0];
dev->gyroZero[1] = v.v[1];
dev->gyroZero[2] = v.v[2];
// gyroZero is set to zero until calibration complete
dev->gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
const float stddev = devStandardDeviation(&gyroCalibration->var[axis]);
// check deviation and start over if the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
return;
}
// calibration complete, so set the gyro zero values
dev->gyroZero[axis] = (gyroCalibration->g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES;
}
}
if (isOnFinalGyroCalibrationCycle(gyroCalibration)) {
DEBUG_TRACE_SYNC("Gyro calibration complete (%d, %d, %d)", dev->gyroZero[0], dev->gyroZero[1], dev->gyroZero[2]);
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
}
gyroCalibration->calibratingG--;
else {
dev->gyroZero[0] = 0;
dev->gyroZero[1] = 0;
dev->gyroZero[2] = 0;
}
}
/*
@ -418,7 +391,7 @@ void gyroUpdate()
{
// range: +/- 8192; +/- 2000 deg/sec
if (gyroDev0.readFn(&gyroDev0)) {
if (isCalibrationComplete(&gyroCalibration)) {
if (zeroCalibrationIsCompleteV(&gyroCalibration)) {
// Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment
gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X];
gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y];
@ -426,7 +399,7 @@ void gyroUpdate()
applySensorAlignment(gyroADC, gyroADC, gyroDev0.gyroAlign);
applyBoardAlignment(gyroADC);
} else {
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold);
performGyroCalibration(&gyroDev0, &gyroCalibration);
// Reset gyro values to zero to prevent other code from using uncalibrated data
gyro.gyroADCf[X] = 0.0f;
gyro.gyroADCf[Y] = 0.0f;

2
src/main/sensors/gyro.h
View file

@ -68,7 +68,7 @@ bool gyroInit(void);
void gyroInitFilters(void);
void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF);
void gyroUpdate();
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroStartCalibration(void);
bool gyroIsCalibrationComplete(void);
bool gyroReadTemperature(void);
int16_t gyroGetTemperature(void);

18
src/main/sensors/pitotmeter.c
View file

@ -153,27 +153,21 @@ bool pitotInit(void)
bool pitotIsCalibrationComplete(void)
{
return pitot.calibrationFinished;
return zeroCalibrationIsCompleteS(&pitot.zeroCalibration) && zeroCalibrationIsSuccessfulS(&pitot.zeroCalibration);
}
void pitotStartCalibration(void)
{
pitot.calibrationTimeoutMs = millis();
pitot.calibrationFinished = false;
zeroCalibrationStartS(&pitot.zeroCalibration, CALIBRATING_PITOT_TIME_MS, P0 * 0.00001f, false);
}
static void performPitotCalibrationCycle(void)
{
const float pitotPressureZeroError = pitot.pressure - pitot.pressureZero;
pitot.pressureZero += pitotPressureZeroError * 0.25f;
zeroCalibrationAddValueS(&pitot.zeroCalibration, pitot.pressure);
if (fabsf(pitotPressureZeroError) < (P0 * 0.000005f)) {
if ((millis() - pitot.calibrationTimeoutMs) > 500) {
pitot.calibrationFinished = true;
}
}
else {
pitot.calibrationTimeoutMs = millis();
if (zeroCalibrationIsCompleteS(&pitot.zeroCalibration)) {
zeroCalibrationGetZeroS(&pitot.zeroCalibration, &pitot.pressureZero);
DEBUG_TRACE_SYNC("Pitot calibration complete (%d)", lrintf(pitot.pressureZero));
}
}

4
src/main/sensors/pitotmeter.h
View file

@ -19,6 +19,7 @@
#include "config/parameter_group.h"
#include "common/filter.h"
#include "common/calibration.h"
#include "drivers/pitotmeter.h"
@ -46,11 +47,10 @@ typedef struct pito_s {
pitotDev_t dev;
float airSpeed;
zeroCalibrationScalar_t zeroCalibration;
pt1Filter_t lpfState;
timeUs_t lastMeasurementUs;
timeMs_t lastSeenHealthyMs;
timeMs_t calibrationTimeoutMs;
bool calibrationFinished;
float pressureZero;
float pressure;

6
src/main/sensors/sensors.h
View file

@ -39,8 +39,10 @@ typedef union flightDynamicsTrims_u {
flightDynamicsTrims_def_t values;
} flightDynamicsTrims_t;
#define CALIBRATING_GYRO_CYCLES 1000
#define CALIBRATING_ACC_CYCLES 400
#define CALIBRATING_BARO_TIME_MS 2000
#define CALIBRATING_PITOT_TIME_MS 2000
#define CALIBRATING_GYRO_TIME_MS 2000
#define CALIBRATING_ACC_TIME_MS 500
// These bits have to be aligned with sensorIndex_e
typedef enum {

10
src/test/Makefile
View file

@ -105,6 +105,15 @@ $(OBJECT_DIR)/common/maths.o : \
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
$(OBJECT_DIR)/common/calibration.o : \
$(USER_DIR)/common/calibration.c \
$(USER_DIR)/common/calibration.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/calibration.c -o $@
$(OBJECT_DIR)/common/bitarray.o : \
$(USER_DIR)/common/bitarray.c \
$(USER_DIR)/common/bitarray.h \
@ -639,6 +648,7 @@ $(OBJECT_DIR)/sensor_gyro_unittest.o : \
$(OBJECT_DIR)/sensor_gyro_unittest : \
$(OBJECT_DIR)/build/debug.o \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/common/calibration.o \
$(OBJECT_DIR)/common/filter.o \
$(OBJECT_DIR)/drivers/accgyro/accgyro_fake.o \
$(OBJECT_DIR)/sensors/gyro.o \

8
src/test/unit/maths_unittest.cc
View file

@ -128,28 +128,28 @@ void expectVectorsAreEqual(fpVector3_t *a, fpVector3_t *b)
TEST(MathsUnittest, TestRotateVectorWithNoAngle)
{
fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
fp_angles_t euler_angles = {.raw={0.0f, 0.0f, 0.0f}};
fpMat3_t rmat;
rotationMatrixFromAngles(&rmat, &euler_angles);
rotationMatrixRotateVector(&vector, &vector, &rmat);
fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
expectVectorsAreEqual(&vector, &expected_result);
}
TEST(MathsUnittest, TestRotateVectorAroundAxis)
{
// Rotate a vector <1, 0, 0> around an each axis x y and z.
fpVector3_t vector = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t vector = { 1.0f, 0.0f, 0.0f};
fp_angles_t euler_angles = {.raw={90.0f, 0.0f, 0.0f}};
fpMat3_t rmat;
rotationMatrixFromAngles(&rmat, &euler_angles);
rotationMatrixRotateVector(&vector, &vector, &rmat);
fpVector3_t expected_result = { .x = 1.0f, .y = 0.0f, .z = 0.0f};
fpVector3_t expected_result = { 1.0f, 0.0f, 0.0f};
expectVectorsAreEqual(&vector, &expected_result);
}

34
src/test/unit/sensor_gyro_unittest.cc
View file

@ -28,6 +28,7 @@ extern "C" {
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/calibration.h"
#include "common/utils.h"
#include "drivers/accgyro/accgyro_fake.h"
#include "drivers/logging_codes.h"
@ -37,17 +38,11 @@ extern "C" {
#include "sensors/acceleration.h"
#include "sensors/sensors.h"
typedef struct gyroCalibration_s {
int32_t g[XYZ_AXIS_COUNT];
stdev_t var[XYZ_AXIS_COUNT];
uint16_t calibratingG;
} gyroCalibration_t;
extern gyroCalibration_t gyroCalibration;
extern zeroCalibrationVector_t gyroCalibration;
extern gyroDev_t gyroDev0;
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware);
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold);
STATIC_UNIT_TESTED bool isCalibrationComplete(gyroCalibration_t *gyroCalibration);
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration);
}
#include "unittest_macros.h"
@ -81,7 +76,7 @@ TEST(SensorGyro, Read)
TEST(SensorGyro, Calibrate)
{
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
gyroStartCalibration();
gyroInit();
fakeGyroSet(5, 6, 7);
const bool read = gyroDev0.readFn(&gyroDev0);
@ -89,17 +84,16 @@ TEST(SensorGyro, Calibrate)
EXPECT_EQ(5, gyroDev0.gyroADCRaw[X]);
EXPECT_EQ(6, gyroDev0.gyroADCRaw[Y]);
EXPECT_EQ(7, gyroDev0.gyroADCRaw[Z]);
static const int gyroMovementCalibrationThreshold = 32;
gyroDev0.gyroZero[X] = 8;
gyroDev0.gyroZero[Y] = 9;
gyroDev0.gyroZero[Z] = 10;
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold);
performGyroCalibration(&gyroDev0, &gyroCalibration);
EXPECT_EQ(0, gyroDev0.gyroZero[X]);
EXPECT_EQ(0, gyroDev0.gyroZero[Y]);
EXPECT_EQ(0, gyroDev0.gyroZero[Z]);
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
while (!isCalibrationComplete(&gyroCalibration)) {
performGyroCalibration(&gyroDev0, &gyroCalibration, gyroMovementCalibrationThreshold);
EXPECT_EQ(false, gyroIsCalibrationComplete());
while (!gyroIsCalibrationComplete()) {
performGyroCalibration(&gyroDev0, &gyroCalibration);
}
EXPECT_EQ(5, gyroDev0.gyroZero[X]);
EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
@ -108,16 +102,16 @@ TEST(SensorGyro, Calibrate)
TEST(SensorGyro, Update)
{
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
gyroStartCalibration();
EXPECT_EQ(false, gyroIsCalibrationComplete());
gyroInit();
fakeGyroSet(5, 6, 7);
gyroUpdate();
EXPECT_EQ(false, isCalibrationComplete(&gyroCalibration));
while (!isCalibrationComplete(&gyroCalibration)) {
EXPECT_EQ(false, gyroIsCalibrationComplete());
while (!gyroIsCalibrationComplete()) {
gyroUpdate();
}
EXPECT_EQ(true, isCalibrationComplete(&gyroCalibration));
EXPECT_EQ(true, gyroIsCalibrationComplete());
EXPECT_EQ(5, gyroDev0.gyroZero[X]);
EXPECT_EQ(6, gyroDev0.gyroZero[Y]);
EXPECT_EQ(7, gyroDev0.gyroZero[Z]);
@ -140,7 +134,9 @@ TEST(SensorGyro, Update)
// STUBS
extern "C" {
static timeMs_t milliTime = 0;
timeMs_t millis(void) {return milliTime++;}
uint32_t micros(void) {return 0;}
void beeper(beeperMode_e) {}
uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE };