Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-12 19:10:27 +03:00
Code Issues Wiki Activity

Add accelerometer

Browse source
This commit is contained in:
breadoven 2024-10-06 13:45:25 +01:00 • committed by Ray Morris
parent 5d423b80f3
commit b9db8e00c3
10 changed files with 157 additions and 80 deletions
Download patch file Download diff file Expand all files Collapse all files

10
docs/Settings.md
View file

@ -122,6 +122,16 @@ Frequency of the software notch filter to remove mechanical vibrations from the
---
### acc_temp_correction
Accelerometer temperature correction factor to compensate for acceleromter drift with changes in acceleromter temperature [cm/s2 per Degs C]. Internally limited to between -50 and 50. Typical setting for MPU6000 acceleromter is around 2.5. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm.
| Default | Min | Max |
| --- | --- | --- |
| 0 | -50 | 51 |
---
### accgain_x
Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page.

3
src/main/CMakeLists.txt
View file

@ -216,7 +216,7 @@ main_sources(COMMON_SRC
drivers/pitotmeter/pitotmeter_ms4525.c
drivers/pitotmeter/pitotmeter_ms4525.h
drivers/pitotmeter/pitotmeter_dlvr_l10d.c
drivers/pitotmeter/pitotmeter_dlvr_l10d.h
drivers/pitotmeter/pitotmeter_dlvr_l10d.h
drivers/pitotmeter/pitotmeter_msp.c
drivers/pitotmeter/pitotmeter_msp.h
drivers/pitotmeter/pitotmeter_virtual.c
@ -460,6 +460,7 @@ main_sources(COMMON_SRC
sensors/esc_sensor.h
sensors/irlock.c
sensors/irlock.h
sensors/sensors.c
sensors/temperature.c
sensors/temperature.h

6
src/main/fc/settings.yaml
View file

@ -467,6 +467,12 @@ groups:
default_value: "BIQUAD"
field: acc_soft_lpf_type
table: filter_type
- name: acc_temp_correction
description: "Accelerometer temperature correction factor to compensate for acceleromter drift with changes in acceleromter temperature [cm/s2 per Degs C]. Internally limited to between -50 and 50. Typical setting for MPU6000 acceleromter is around 2.5. Setting to 51 initiates auto calibration which ends after 5 minutes or on first Arm."
field: acc_temp_correction
min: -50
max: 51
default_value: 0
- name: acczero_x
description: "Calculated value after '6 position avanced calibration'. See Wiki page."
default_value: 0

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

@ -50,8 +50,9 @@
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/pitotmeter.h"
#include "sensors/opflow.h"
#include "sensors/pitotmeter.h"
#include "sensors/sensors.h"
navigationPosEstimator_t posEstimator;
static float initialBaroAltitudeOffset = 0.0f;
@ -283,12 +284,12 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
{
float newBaroAlt = baroCalculateAltitude();
/* If we are required - keep altitude at zero */
if (shouldResetReferenceAltitude()) {
initialBaroAltitudeOffset = newBaroAlt;
}
if (sensors(SENSOR_BARO) && baroIsCalibrationComplete()) {
/* If required - keep altitude at zero */
if (shouldResetReferenceAltitude()) {
initialBaroAltitudeOffset = newBaroAlt;
}
const timeUs_t baroDtUs = currentTimeUs - posEstimator.baro.lastUpdateTime;
posEstimator.baro.alt = newBaroAlt - initialBaroAltitudeOffset;
@ -434,6 +435,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
}
#endif
posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
posEstimator.imu.accelNEU.z += applySensorTempCompensation(10 * gyroGetTemperature(), imuMeasuredAccelBF.z, SENSOR_INDEX_ACC);
}
else { // If calibration is incomplete - report zero acceleration
posEstimator.imu.accelNEU.x = 0.0f;

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

@ -85,7 +85,7 @@ static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM float fAccZero[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM float fAccGain[XYZ_AXIS_COUNT];
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5);
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 6);
void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
{
@ -94,7 +94,8 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
.acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT,
.acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT,
.acc_notch_cutoff = SETTING_ACC_NOTCH_CUTOFF_DEFAULT,
.acc_soft_lpf_type = SETTING_ACC_LPF_TYPE_DEFAULT
.acc_soft_lpf_type = SETTING_ACC_LPF_TYPE_DEFAULT,
.acc_temp_correction = SETTING_ACC_TEMP_CORRECTION_DEFAULT
);
RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero,
.raw[X] = SETTING_ACCZERO_X_DEFAULT,
@ -557,8 +558,8 @@ void accUpdate(void)
if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
performAcclerationCalibration();
applyAccelerationZero();
}
applyAccelerationZero();
}
applySensorAlignment(accADC, accADC, acc.dev.accAlign);
applyBoardAlignment(accADC);
@ -637,7 +638,7 @@ bool accIsClipped(void)
void accSetCalibrationValues(void)
{
if (!ARMING_FLAG(SIMULATOR_MODE_SITL) &&
if (!ARMING_FLAG(SIMULATOR_MODE_SITL) &&
((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&
(accelerometerConfig()->accGain.raw[X] == 4096) && (accelerometerConfig()->accGain.raw[Y] == 4096) &&(accelerometerConfig()->accGain.raw[Z] == 4096))) {
DISABLE_STATE(ACCELEROMETER_CALIBRATED);
@ -648,12 +649,12 @@ void accSetCalibrationValues(void)
}
void accInitFilters(void)
{
{
accSoftLpfFilterApplyFn = nullFilterApply;
if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) {
switch (accelerometerConfig()->acc_soft_lpf_type)
switch (accelerometerConfig()->acc_soft_lpf_type)
{
case FILTER_PT1:
accSoftLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;

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

@ -74,7 +74,8 @@ typedef struct accelerometerConfig_s {
flightDynamicsTrims_t accGain; // Accelerometer gain to read exactly 1G
uint8_t acc_notch_hz; // Accelerometer notch filter frequency
uint8_t acc_notch_cutoff; // Accelerometer notch filter cutoff frequency
uint8_t acc_soft_lpf_type; // Accelerometer LPF type
uint8_t acc_soft_lpf_type; // Accelerometer LPF type
float acc_temp_correction; // Accelerometer temperature compensation factor
} accelerometerConfig_t;
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);

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

@ -60,7 +60,7 @@ baro_t baro; // barometer access functions
#ifdef USE_BARO
PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 4);
PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 5);
PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig,
.baro_hardware = SETTING_BARO_HARDWARE_DEFAULT,
@ -312,62 +312,6 @@ void baroStartCalibration(void)
zeroCalibrationStartS(&zeroCalibration, CALIBRATING_BARO_TIME_MS, acceptedPressureVariance, false);
}
float processBaroTempCorrection(void)
{
float setting = barometerConfig()->baro_temp_correction;
if (setting == 0.0f) {
return 0.0f;
}
static float correctionFactor = 0.0f;
static baroTempCalState_e calibrationState = BARO_TEMP_CAL_INITIALISE;
static int16_t baroTemp1 = 0.0f;
static timeMs_t startTimeMs = 0;
if (calibrationState == BARO_TEMP_CAL_COMPLETE) {
return correctionFactor * CENTIDEGREES_TO_DEGREES(baroTemp1 - baro.baroTemperature);
}
if (!ARMING_FLAG(WAS_EVER_ARMED)) {
static float baroAlt1 = 0.0f;
static int16_t baroTemp2 = 0.0f;
float newBaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
if (calibrationState == BARO_TEMP_CAL_INITIALISE) { // set initial correction reference temps/pressures
baroTemp1 = baroTemp2 = baro.baroTemperature;
baroAlt1 = newBaroAlt;
calibrationState = BARO_TEMP_CAL_IN_PROGRESS;
startTimeMs = millis();
}
if (setting == 51.0f) { // Auto calibration triggered with setting = 51
/* Min 3 deg reference temperature difference required for valid calibration.
* Correction adjusted only if temperature difference to reference temperature increasing */
float referenceDeltaTemp = ABS(baro.baroTemperature - baroTemp1);
if (referenceDeltaTemp > 300 && referenceDeltaTemp > ABS(baroTemp2 - baroTemp1)) {
baroTemp2 = baro.baroTemperature;
correctionFactor = 0.8f * correctionFactor + 0.2f * (newBaroAlt - baroAlt1) / CENTIDEGREES_TO_DEGREES(baroTemp2 - baroTemp1);
correctionFactor = constrainf(correctionFactor, -50.0f, 50.0f);
}
} else {
correctionFactor = setting;
calibrationState = BARO_TEMP_CAL_COMPLETE;
}
}
// Calibration ends on first Arm or after 5 min timeout
if (calibrationState == BARO_TEMP_CAL_IN_PROGRESS && (ARMING_FLAG(WAS_EVER_ARMED) || millis() > startTimeMs + 300000)) {
barometerConfigMutable()->baro_temp_correction = correctionFactor;
calibrationState = BARO_TEMP_CAL_COMPLETE;
if (!ARMING_FLAG(WAS_EVER_ARMED)) {
beeper(correctionFactor ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
}
}
return 0.0f;
}
int32_t baroCalculateAltitude(void)
{
if (!baroIsCalibrationComplete()) {
@ -383,7 +327,8 @@ int32_t baroCalculateAltitude(void)
}
else {
// calculates height from ground via baro readings
baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude + processBaroTempCorrection();
baro.BaroAlt = pressureToAltitude(baro.baroPressure) - baroGroundAltitude;
baro.BaroAlt += applySensorTempCompensation(baro.baroTemperature, baro.BaroAlt, SENSOR_INDEX_BARO);
}
return baro.BaroAlt;

7
src/main/sensors/barometer.h
View file

@ -38,12 +38,6 @@ typedef enum {
BARO_MAX = BARO_FAKE
} baroSensor_e;
typedef enum {
BARO_TEMP_CAL_INITIALISE,
BARO_TEMP_CAL_IN_PROGRESS,
BARO_TEMP_CAL_COMPLETE,
} baroTempCalState_e;
typedef struct baro_s {
baroDev_t dev;
int32_t BaroAlt;
@ -63,7 +57,6 @@ typedef struct barometerConfig_s {
PG_DECLARE(barometerConfig_t, barometerConfig);
bool baroInit(void);
bool baroIsCalibrationComplete(void);
void baroStartCalibration(void);

105
src/main/sensors/sensors.c Normal file
View file

@ -0,0 +1,105 @@
/*
* This file is part of INAV.
*
* INAV is free software: you can redistribute it 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.
*
* INAV 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 INAV. If not, see <http://www.gnu.org/licenses/>.
*/
#include "platform.h"
#include "build/debug.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/time.h"
#include "fc/runtime_config.h"
#include "io/beeper.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/sensors.h"
sensor_compensation_t sensor_comp_data[SENSOR_INDEX_COUNT];
float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, sensorIndex_e sensorType)
{
float setting = 0.0f;
if (sensorType == SENSOR_INDEX_ACC) {
setting = accelerometerConfig()->acc_temp_correction;
} else if (sensorType == SENSOR_INDEX_BARO) {
setting = barometerConfig()->baro_temp_correction;
}
if (!setting) {
return 0.0f;
}
if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_COMPLETE) {
return sensor_comp_data[sensorType].correctionFactor * CENTIDEGREES_TO_DEGREES(sensor_comp_data[sensorType].referenceTemp - sensorTemp);
}
static timeMs_t startTimeMs = 0;
if (!ARMING_FLAG(WAS_EVER_ARMED)) {
if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_INITIALISE) {
sensor_comp_data[sensorType].referenceTemp = sensorTemp;
sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_IN_PROGRESS;
}
if (setting == 51.0f) { // initiate auto calibration
static float referenceMeasurement = 0.0f;
static int16_t lastTemp = 0.0f;
if (sensor_comp_data[sensorType].referenceTemp == sensorTemp) {
referenceMeasurement = sensorMeasurement;
lastTemp = sensorTemp;
startTimeMs = millis();
}
float referenceDeltaTemp = ABS(sensorTemp - sensor_comp_data[sensorType].referenceTemp); // centidegrees
if (referenceDeltaTemp > 300 && referenceDeltaTemp > ABS(lastTemp - sensor_comp_data[sensorType].referenceTemp)) {
/* Min 3 deg reference temperature difference required for valid calibration.
* Correction adjusted only if temperature difference to reference temperature increasing
* Calibration assumes a simple linear relationship */
lastTemp = sensorTemp;
sensor_comp_data[sensorType].correctionFactor = 0.9f * sensor_comp_data[sensorType].correctionFactor + 0.1f * (sensorMeasurement - referenceMeasurement) / CENTIDEGREES_TO_DEGREES(lastTemp - sensor_comp_data[sensorType].referenceTemp);
sensor_comp_data[sensorType].correctionFactor = constrainf(sensor_comp_data[sensorType].correctionFactor, -50.0f, 50.0f);
}
} else {
sensor_comp_data[sensorType].correctionFactor = setting;
sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_COMPLETE;
}
}
// Calibration ends on first Arm or after 5 min timeout
if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_IN_PROGRESS && (ARMING_FLAG(WAS_EVER_ARMED) || millis() > startTimeMs + 300000)) {
if (!ARMING_FLAG(WAS_EVER_ARMED)) {
beeper(sensor_comp_data[sensorType].correctionFactor ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
}
if (sensorType == SENSOR_INDEX_ACC) {
accelerometerConfigMutable()->acc_temp_correction = sensor_comp_data[sensorType].correctionFactor;
} else if (sensorType == SENSOR_INDEX_BARO) {
barometerConfigMutable()->baro_temp_correction = sensor_comp_data[sensorType].correctionFactor;
}
sensor_comp_data[sensorType].calibrationState = SENSOR_TEMP_CAL_COMPLETE;
startTimeMs = 0;
}
return 0.0f;
}

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

@ -59,5 +59,18 @@ typedef enum {
SENSOR_TEMP = 1 << 9
} sensors_e;
typedef enum {
SENSOR_TEMP_CAL_INITIALISE,
SENSOR_TEMP_CAL_IN_PROGRESS,
SENSOR_TEMP_CAL_COMPLETE,
} sensorTempCalState_e;
typedef struct sensor_compensation_s {
float correctionFactor;
int16_t referenceTemp;
sensorTempCalState_e calibrationState;
} sensor_compensation_t;
float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, sensorIndex_e sensorType);
extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];