Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Improved gyro initialisation (#456)

Browse source 
* Improved gyro initialisation
* Fixed acc filter initialisation. Improved detection routines
* Fixed biquad filter initialisation
* Changed gyroSoftLpfHz from int8_t to uint8_t
This commit is contained in:
Martin Budden 2016-08-29 11:44:49 +01:00 • committed by Konstantin Sharlaimov
parent 30cd4183ec
commit b45737d3b2
19 changed files with 116 additions and 114 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/blackbox/blackbox.c
View file

@ -379,9 +379,6 @@ extern uint32_t currentTime;
//From rx.c:
extern uint16_t rssi;
//From gyro.c
extern uint32_t targetLooptime;
static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED;
static uint32_t blackboxLastArmingBeep = 0;
@ -1303,7 +1300,7 @@ static bool blackboxWriteSysinfo()
}
);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcExpo8);
BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcYawExpo8);
BLACKBOX_PRINT_HEADER_LINE("thrMid:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].thrMid8);

2
src/main/blackbox/blackbox_io.c
View file

@ -618,7 +618,7 @@ bool blackboxDeviceOpen(void)
* = floor((looptime_ns * 3) / 500.0)
* = (looptime_ns * 3) / 500
*/
blackboxMaxHeaderBytesPerIteration = constrain((targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
blackboxMaxHeaderBytesPerIteration = constrain((gyro.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
return blackboxPort != NULL;
}

49
src/main/common/filter.c
View file

@ -29,23 +29,17 @@
#define BIQUAD_Q (1.0f / 1.41421356f) /* quality factor - butterworth (1 / sqrt(2)) */
#define M_PI_FLOAT 3.14159265358979323846f
/* sets up a biquad Filter */
void biquadFilterInit(biquadFilter_t *newState, uint8_t filterCutFreq, int16_t samplingRate)
void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate)
{
float omega, sn, cs, alpha;
// setup variables
const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f);
const float omega = 2 * M_PIf * ((float)filterCutFreq) / sampleRate;
const float sn = sin_approx(omega);
const float cs = cos_approx(omega);
const float alpha = sn / (2 * BIQUAD_Q);
float a0, a1, a2, b0, b1, b2;
/* If sampling rate == 0 - use main loop target rate */
if (!samplingRate) {
samplingRate = 1000000 / targetLooptime;
}
/* setup variables */
omega = 2 * M_PIf * (float)filterCutFreq / (float)samplingRate;
sn = sin_approx(omega);
cs = cos_approx(omega);
alpha = sn / (2 * BIQUAD_Q);
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
@ -53,26 +47,23 @@ void biquadFilterInit(biquadFilter_t *newState, uint8_t filterCutFreq, int16_t s
a1 = -2 * cs;
a2 = 1 - alpha;
/* precompute the coefficients */
newState->b0 = b0 / a0;
newState->b1 = b1 / a0;
newState->b2 = b2 / a0;
newState->a1 = a1 / a0;
newState->a2 = a2 / a0;
// precompute the coefficients
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
/* zero initial samples */
newState->d1 = newState->d2 = 1;
// zero initial samples
filter->d1 = filter->d2 = 1;
}
/* Computes a biquad_t filter on a sample */
float biquadFilterApply(biquadFilter_t *state, float sample)
float biquadFilterApply(biquadFilter_t *filter, float input)
{
float result;
result = state->b0 * sample + state->d1;
state->d1 = state->b1 * sample - state->a1 * result + state->d2;
state->d2 = state->b2 * sample - state->a2 * result;
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
filter->d2 = filter->b2 * input - filter->a2 * result;
return result;
}

2
src/main/common/filter.h
View file

@ -47,7 +47,7 @@ void pt1FilterReset(pt1Filter_t *filter, float input);
void rateLimitFilterInit(rateLimitFilter_t *filter);
float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);
void biquadFilterInit(biquadFilter_t *filter, uint8_t filterCutFreq, int16_t samplingRate);
void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate);
float biquadFilterApply(biquadFilter_t *filter, float sample);
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);

2
src/main/config/config.c
View file

@ -801,7 +801,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
gyroUseConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

1
src/main/drivers/accgyro.h
View file

@ -27,6 +27,7 @@ typedef struct gyro_s {
sensorReadFuncPtr temperature; // read temperature if available
sensorInterruptFuncPtr intStatus;
float scale; // scalefactor
uint32_t targetLooptime;
} gyro_t;
typedef struct acc_s {

7
src/main/drivers/gyro_sync.c
View file

@ -35,7 +35,6 @@
extern gyro_t gyro;
uint32_t targetLooptime;
static uint8_t mpuDividerDrops;
bool getMpuDataStatus(gyro_t *gyro)
@ -48,7 +47,7 @@ bool gyroSyncCheckUpdate(void)
return getMpuDataStatus(&gyro);
}
void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)
uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)
{
if (gyroSync) {
int gyroSamplePeriod;
@ -60,11 +59,11 @@ void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t
}
mpuDividerDrops = gyroSyncDenominator - 1;
targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
looptime = gyroSyncDenominator * gyroSamplePeriod;
} else {
mpuDividerDrops = 0;
targetLooptime = looptime;
}
return looptime;
}
uint8_t gyroMPU6xxxCalculateDivider(void)

4
src/main/drivers/gyro_sync.h
View file

@ -17,8 +17,6 @@
#define INTERRUPT_WAIT_TIME 10
extern uint32_t targetLooptime;
bool gyroSyncCheckUpdate(void);
uint8_t gyroMPU6xxxCalculateDivider(void);
void gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);
uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);

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

@ -498,7 +498,7 @@ void filterRc(bool isRXDataNew)
// Calculate average cycle time (1Hz LPF on cycle time)
if (!filterInitialised) {
biquadFilterInit(&filteredCycleTimeState, 1, 0);
biquadFilterInit(&filteredCycleTimeState, 1, gyro.targetLooptime);
filterInitialised = true;
}
@ -641,7 +641,7 @@ void taskMainPidLoopChecker(void) {
if (masterConfig.gyroSync) {
while (1) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
break;
}
}

3
src/main/flight/mixer.c
View file

@ -47,6 +47,7 @@
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "flight/mixer.h"
#include "flight/failsafe.h"
@ -922,7 +923,7 @@ void filterServos(void)
// Initialize servo lowpass filter (servos are calculated at looptime rate)
if (!servoFilterIsSet) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, 0);
biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
}
servoFilterIsSet = true;

12
src/main/main.c
View file

@ -422,15 +422,15 @@ void init(void)
}
#endif
// Set gyro sampling rate divider before initialization
gyroSetSampleRate(masterConfig.looptime, masterConfig.gyro_lpf, masterConfig.gyroSync, masterConfig.gyroSyncDenominator);
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
masterConfig.gyro_lpf,
masterConfig.acc_hardware,
masterConfig.mag_hardware,
masterConfig.baro_hardware,
currentProfile->mag_declination)) {
currentProfile->mag_declination,
masterConfig.looptime,
masterConfig.gyro_lpf,
masterConfig.gyroSync,
masterConfig.gyroSyncDenominator)) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
@ -585,7 +585,7 @@ int main(void)
/* Setup scheduler */
schedulerInit();
rescheduleTask(TASK_GYROPID, targetLooptime);
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
setTaskEnabled(TASK_GYROPID, true);
setTaskEnabled(TASK_SERIAL, true);

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

@ -42,6 +42,7 @@
acc_t acc; // acc access functions
int32_t accADC[XYZ_AXIS_COUNT];
sensor_align_e accAlign = 0;
uint32_t accTargetLooptime;
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 int16_t accADCRaw[XYZ_AXIS_COUNT];
@ -49,9 +50,18 @@ static int16_t accADCRaw[XYZ_AXIS_COUNT];
static flightDynamicsTrims_t * accZero;
static flightDynamicsTrims_t * accGain;
static int8_t accLpfCutHz = 0;
static biquadFilter_t accFilterState[XYZ_AXIS_COUNT];
static bool accFilterInitialised = false;
static uint8_t accLpfCutHz = 0;
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
void accInit(uint32_t targetLooptime)
{
accTargetLooptime = targetLooptime;
if (accLpfCutHz) {
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
}
}
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
{
@ -166,7 +176,7 @@ void performAcclerationCalibration(void)
calibratingA--;
}
void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain)
static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
{
accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
@ -179,23 +189,13 @@ void updateAccelerationReadings(void)
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = accADCRaw[axis];
}
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis]));
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float) accADC[axis]));
}
}
@ -218,7 +218,12 @@ void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
accGain = accGainToUse;
}
void setAccelerationFilter(int8_t initialAccLpfCutHz)
void setAccelerationFilter(uint8_t initialAccLpfCutHz)
{
accLpfCutHz = initialAccLpfCutHz;
if (accTargetLooptime) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
}
}

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

@ -37,9 +37,11 @@ extern acc_t acc;
extern int32_t accADC[XYZ_AXIS_COUNT];
void accInit(uint32_t accTargetLooptime);
bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void updateAccelerationReadings(void);
void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse);
void setAccelerationGain(flightDynamicsTrims_t * accGainToUse);
void setAccelerationFilter(int8_t initialAccLpfCutHz);
union flightDynamicsTrims_u;
void setAccelerationZero(union flightDynamicsTrims_u * accZeroToUse);
void setAccelerationGain(union flightDynamicsTrims_u * accGainToUse);
void setAccelerationFilter(uint8_t initialAccLpfCutHz);

6
src/main/sensors/boardalignment.c
View file

@ -30,12 +30,12 @@
static bool standardBoardAlignment = true; // board orientation correction
static float boardRotation[3][3]; // matrix
static bool isBoardAlignmentStandard(boardAlignment_t *boardAlignment)
static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
{
return !boardAlignment->rollDeciDegrees && !boardAlignment->pitchDeciDegrees && !boardAlignment->yawDeciDegrees;
}
void initBoardAlignment(boardAlignment_t *boardAlignment)
void initBoardAlignment(const boardAlignment_t *boardAlignment)
{
if (isBoardAlignmentStandard(boardAlignment)) {
standardBoardAlignment = true;
@ -75,7 +75,7 @@ static void alignBoard(int32_t *vec)
vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
}
void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation)
void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation)
{
static uint32_t swap[3];
memcpy(swap, src, sizeof(swap));

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

@ -23,6 +23,6 @@ typedef struct boardAlignment_s {
int16_t yawDeciDegrees;
} boardAlignment_t;
void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation);
void initBoardAlignment(boardAlignment_t *boardAlignment);
void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation);
void initBoardAlignment(const boardAlignment_t *boardAlignment);
void updateBoardAlignment(boardAlignment_t *boardAlignment, int16_t roll, int16_t pitch);

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

@ -41,20 +41,27 @@ sensor_align_e gyroAlign = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
static gyroConfig_t *gyroConfig;
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static uint16_t calibratingG = 0;
static int16_t gyroADCRaw[XYZ_AXIS_COUNT];
static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static int8_t gyroLpfCutHz = 0;
static biquadFilter_t gyroFilterState[XYZ_AXIS_COUNT];
static bool gyroFilterInitialised = false;
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfHz = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
{
gyroConfig = gyroConfigToUse;
gyroLpfCutHz = initialGyroLpfCutHz;
gyroSoftLpfHz = gyro_soft_lpf_hz;
}
void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@ -125,29 +132,21 @@ static void applyGyroZero(void)
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
// Prepare a copy of int32_t gyroADC for mangling to prevent overflow
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = gyroADCRaw[axis];
}
if (gyroLpfCutHz) {
if (!gyroFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterState[axis], gyroLpfCutHz, 0);
}
gyroFilterInitialised = true;
}
}
if (gyroFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterState[axis], (float) gyroADC[axis]));
}
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]));
}
}

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

@ -32,7 +32,6 @@ typedef enum {
} gyroSensor_e;
extern gyro_t gyro;
extern sensor_align_e gyroAlign;
extern int32_t gyroADC[XYZ_AXIS_COUNT];
@ -40,7 +39,9 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz);
void gyroInit(void);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t initialGyroLpfCutHz);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);

19
src/main/sensors/initialisation.c
View file

@ -82,6 +82,7 @@
extern baro_t baro;
extern mag_t mag;
extern sensor_align_e gyroAlign;
uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
@ -702,12 +703,15 @@ static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentC
}
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
uint8_t gyroLpf,
uint8_t accHardwareToUse,
uint8_t magHardwareToUse,
uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig) {
int16_t magDeclinationFromConfig,
uint32_t looptime,
uint8_t gyroLpf,
uint8_t gyroSync,
uint8_t gyroSyncDenominator)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
@ -719,11 +723,15 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
if (!detectGyro()) {
return false;
}
gyro.init(gyroLpf);
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.targetLooptime = gyroSetSampleRate(looptime, gyroLpf, gyroSync, gyroSyncDenominator); // Set gyro sample rate before initialisation
gyro.init(gyroLpf); // driver initialisation
gyroInit(); // sensor initialisation
if (detectAcc(accHardwareToUse)) {
acc.acc_1G = 256; // set default
acc.init(&acc);
accInit(gyro.targetLooptime); // acc and gyro updated at same frequency in taskMainPidLoop in mw.c
}
#ifdef BARO
@ -746,12 +754,11 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
UNUSED(magDeclinationFromConfig);
#endif
reconfigureAlignment(sensorAlignmentConfig);
#ifdef SONAR
const rangefinderType_e rangefinderType = detectRangefinder();
rangefinderInit(rangefinderType);
#endif
reconfigureAlignment(sensorAlignmentConfig);
return true;
}

5
src/main/sensors/initialisation.h
View file

@ -17,6 +17,7 @@
#pragma once
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t gyroLpf,
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
uint8_t accHardwareToUse, uint8_t magHardwareToUse, uint8_t baroHardwareToUse,
int16_t magDeclinationFromConfig);
int16_t magDeclinationFromConfig,
uint32_t looptime, uint8_t gyroLpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);