rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

2khz Support Added

This commit is contained in:
borisbstyle 2015-12-22 17:42:57 +01:00
parent 27c6db51fb
commit a3fe45d774
8 changed files with 79 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
src/main/common/filter.c
View File

@ -1,8 +1,18 @@
/*
* filter.c
* This file is part of Cleanflight.
*
* Created on: 24 jun. 2015
* Author: borisb
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
@ -10,8 +20,9 @@
#include <stdlib.h>
#include <math.h>
#include "common/filter.h"
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
@ -27,23 +38,37 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
return filter->state;
}
// 7 Tap FIR filter as described here:
// Thanks to Qcopter
void filterApplyFIR(int16_t data[]) {
int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 }; // looptime=500;
static int8_t gyroFIRCoeff_1000[FILTER_TAPS] = { 0, 0, 0, 0, 0, 0, 12, 23, 40, 51, 52, 40, 38 }; // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime)
{
if (filter_level == 0) {
return NULL;
}
// filter for 2kHz looptime
if (targetLooptime == 500) {
return gyroFIRCoeff_500;
} else { // filter for 1kHz looptime
return gyroFIRCoeff_1000;
}
}
// Thanks to Qcopter & BorisB & DigitalEntity
void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS])
{
int32_t FIRsum;
int axis, i;
// 7 tap FIR, <-20dB at >170Hz with looptime 1ms, groupdelay = 2.5ms
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
FIRsum = 0;
for (i = 0; i <= 5; i++) {
gyro_delay[axis][i] = gyro_delay[axis][i + 1];
FIRsum += gyro_delay[axis][i] * FIRcoeff[i];
for (i = 0; i <= 7; i++) {
state[axis][i] = state[axis][i + 1];
FIRsum += state[axis][i] * (int16_t)coeff[i];
}
gyro_delay[axis][6] = data[axis];
FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
state[axis][FILTER_TAPS-1] = data[axis];
FIRsum += state[axis][FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
data[axis] = FIRsum / 256;
}
}

22
src/main/common/filter.h
View File

@ -1,16 +1,28 @@
/*
* filter.h
* This file is part of Cleanflight.
*
* Created on: 24 jun. 2015
* Author: borisb
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#define FILTER_TAPS 13
typedef struct filterStatePt1_s {
float state;
float RC;
float constdT;
} filterStatePt1_t;
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
void filterApplyFIR(int16_t data[3]);
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime);
void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS]);

3
src/main/config/config.c
View File

@ -36,6 +36,7 @@
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
#include "drivers/serial.h"
#include "drivers/gyro_sync.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
@ -751,7 +752,7 @@ void activateConfig(void)
);
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf); // Leave this for more coefficients in the future
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, targetLooptime));
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

12
src/main/drivers/gyro_sync.c
View File

@ -46,10 +46,18 @@ void gyroUpdateSampleRate(uint8_t lpf) {
if (!lpf) {
gyroSamplePeriod = 125;
gyroSyncDenominator = 8; // Sample every 8th gyro measurement
#ifdef STM32F303xC
gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
#else
if (!sensors(SENSOR_ACC) && !sensors(SENSOR_BARO) && !sensors(SENSOR_MAG)) {
gyroSyncDenominator = 4; // Sample every 4th gyro measurement 2khz
} else {
gyroSyncDenominator = 8; // Sample every 8th gyro measurement 1khz
}
#endif
} else {
gyroSamplePeriod = 1000;
gyroSyncDenominator = 1; // Full Sampling
gyroSyncDenominator = 1; // Full Sampling 1khz
}
// calculate gyro divider and targetLooptime (expected cycleTime)

2
src/main/mw.c
View File

@ -91,7 +91,7 @@ enum {
ALIGN_MAG = 2
};
//#define JITTER_DEBUG 0 // Specify debug value for jitter debug
#define JITTER_DEBUG 0 // Specify debug value for jitter debug
/* VBAT monitoring interval (in microseconds) - 1s*/
#define VBATINTERVAL (6 * 3500)

2
src/main/scheduler.c
View File

@ -27,7 +27,7 @@
#include "drivers/system.h"
//#define SCHEDULER_DEBUG
#define SCHEDULER_DEBUG
cfTaskId_e currentTaskId = TASK_NONE;

12
src/main/sensors/gyro.c
View File

@ -38,15 +38,17 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
static uint8_t gyroFIRFilter;
static int8_t * gyroFIRTable = 0L;
static int16_t gyroFIRState[3][FILTER_TAPS];
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t filter)
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
{
gyroConfig = gyroConfigToUse;
gyroFIRFilter = filter;
gyroFIRTable = filterTableToUse;
}
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@ -124,8 +126,8 @@ void gyroUpdate(void)
return;
}
if (gyroFIRFilter) {
filterApplyFIR(gyroADC);
if (gyroFIRTable) {
filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable);
}
alignSensors(gyroADC, gyroADC, gyroAlign);

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

@ -39,7 +39,7 @@ 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 filter);
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
void gyroUpdate(void);
bool isGyroCalibrationComplete(void);