mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
fix build errors
This commit is contained in:
rav
parent
d9909b91d3
commit
75089ea24c
5 changed files with 32 additions and 39 deletions
|
|
@ -150,7 +150,7 @@ static const char * const featureNames[] = {
|
|||
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
|
||||
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
|
||||
"UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
|
||||
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", NULL
|
||||
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL
|
||||
};
|
||||
|
||||
// sync this with rxFailsafeChannelMode_e
|
||||
|
|
|
|||
|
|
@ -204,7 +204,10 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
|
|||
"STACK",
|
||||
"ESC_SENSOR_RPM",
|
||||
"ESC_SENSOR_TMP",
|
||||
"ALTITUDE"
|
||||
"ALTITUDE",
|
||||
"FFT",
|
||||
"FFT_TIME",
|
||||
"FFT_FREQ"
|
||||
};
|
||||
|
||||
#ifdef OSD
|
||||
|
|
|
|||
|
|
@ -96,15 +96,14 @@ typedef struct gyroSensor_s {
|
|||
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
|
||||
filterApplyFnPtr notchFilter2ApplyFn;
|
||||
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
||||
filterApplyFnPtr notchFilterDynApplyFn;
|
||||
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
|
||||
} gyroSensor_t;
|
||||
|
||||
static gyroSensor_t gyroSensor0;
|
||||
|
||||
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
|
||||
|
||||
static filterApplyFnPtr notchFilterDynApplyFn;
|
||||
biquadFilter_t *notchFilterDyn[3];
|
||||
|
||||
#define DEBUG_GYRO_CALIBRATION 3
|
||||
|
||||
#ifdef STM32F10X
|
||||
|
|
@ -410,7 +409,6 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
|
|||
|
||||
void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
|
||||
{
|
||||
|
||||
gyroSensor->notchFilter2ApplyFn = nullFilterApply;
|
||||
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
|
||||
if (notchHz && notchHz <= gyroFrequencyNyquist) {
|
||||
|
|
@ -422,30 +420,26 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
|
|||
}
|
||||
}
|
||||
|
||||
void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
|
||||
{
|
||||
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; // must be this function, not DF2
|
||||
const float notchQ = filterGetNotchQ(400, 390); //just any init value
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
}
|
||||
}
|
||||
|
||||
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
|
||||
{
|
||||
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
|
||||
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
}
|
||||
|
||||
void gyroInitFilterDynamicNotch()
|
||||
{
|
||||
static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
|
||||
|
||||
notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; //must be this function, do not change
|
||||
|
||||
const float notchQ = filterGetNotchQ(400, 390); //just any init value
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
notchFilterDyn[axis] = &gyroFilterNotch[axis];
|
||||
biquadFilterInit(notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
}
|
||||
gyroInitFilterDynamicNotch(gyroSensor);
|
||||
}
|
||||
|
||||
void gyroInitFilters(void)
|
||||
{
|
||||
gyroInitSensorFilters(&gyroSensor0);
|
||||
gyroInitFilterDynamicNotch();
|
||||
}
|
||||
|
||||
bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
||||
|
|
@ -545,6 +539,10 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
|
|||
return;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
|
||||
#endif
|
||||
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
// scale gyro output to degrees per second
|
||||
float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
|
||||
|
|
@ -555,7 +553,7 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
|
|||
DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
|
||||
|
||||
if (isDynamicFilterActive())
|
||||
gyroADCf = notchFilterDynApplyFn(notchFilterDyn[axis], gyroADCf);
|
||||
gyroADCf = gyroSensor->notchFilterDynApplyFn(&gyroSensor->notchFilterDyn[axis], gyroADCf);
|
||||
|
||||
if (axis == 0)
|
||||
DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
|
||||
|
|
@ -572,10 +570,6 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
|
|||
|
||||
gyro.gyroADCf[axis] = gyroADCf;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_DATA_ANALYSE
|
||||
gyroDataAnalyse(&gyroSensor->gyroDev, &gyro);
|
||||
#endif
|
||||
}
|
||||
|
||||
void gyroUpdate(void)
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@
|
|||
#define FFT_SAMPLING_RATE 1000 // allows analysis up to 500Hz which is more than motors create
|
||||
#define FFT_BPF_HZ 200 // use a bandpass on gyro data to ignore extreme low and extreme high frequencies
|
||||
#define DYN_NOTCH_WIDTH 100 // just an orientation and start value
|
||||
#define DYN_NOTCH_CHANGERATE 40 // lower cut does not improve the performance much, higher cut makes it worse...
|
||||
#define DYN_NOTCH_CHANGERATE 60 // lower cut does not improve the performance much, higher cut makes it worse...
|
||||
#define DYN_NOTCH_MIN_CUTOFF 120 // don't cut too deep into low frequencies
|
||||
#define DYN_NOTCH_MAX_CUTOFF 200 // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
|
||||
|
||||
|
|
@ -118,7 +118,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
|
|||
// at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
|
||||
float looptime = targetLooptimeUs * 4 * 3;
|
||||
for (int axis = 0; axis < 3; ++axis) {
|
||||
fftResult[axis].centerFreq = 200;
|
||||
fftResult[axis].centerFreq = 200; // any init value
|
||||
biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
|
||||
biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
|
||||
}
|
||||
|
|
@ -136,15 +136,14 @@ bool isDynamicFilterActive(void) {
|
|||
/*
|
||||
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
|
||||
*/
|
||||
void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) {
|
||||
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) {
|
||||
if (!isDynamicFilterActive()) {
|
||||
return;
|
||||
}
|
||||
UNUSED(gyro);
|
||||
|
||||
// if gyro sampling is > 1kHz, accumulate multiple samples
|
||||
for (int axis = 0; axis < 3; ++axis) {
|
||||
fftAcc[axis] += gyroDev->gyroADC[axis] * gyroDev->scale;
|
||||
fftAcc[axis] += gyroDev->gyroADC[axis];
|
||||
}
|
||||
fftAccCount++;
|
||||
|
||||
|
|
@ -158,7 +157,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) {
|
|||
sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample);
|
||||
gyroData[axis][fftIdx] = sample;
|
||||
if (axis == 0)
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
|
||||
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
|
||||
fftAcc[axis] = 0;
|
||||
}
|
||||
|
||||
|
|
@ -166,7 +165,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) {
|
|||
}
|
||||
|
||||
// calculate FFT and update filters
|
||||
gyroDataAnalyseUpdate();
|
||||
gyroDataAnalyseUpdate(notchFilterDyn);
|
||||
}
|
||||
|
||||
void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut);
|
||||
|
|
@ -189,7 +188,7 @@ typedef enum {
|
|||
/*
|
||||
* Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
|
||||
*/
|
||||
void gyroDataAnalyseUpdate() {
|
||||
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
|
||||
static int axis = 0;
|
||||
static int step = 0;
|
||||
arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
|
||||
|
|
@ -295,7 +294,7 @@ void gyroDataAnalyseUpdate() {
|
|||
// calculate new filter coefficients
|
||||
float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF);
|
||||
float notchQ = filterGetNotchQApprox(fftResult[axis].centerFreq, cutoffFreq);
|
||||
biquadFilterUpdate(notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
biquadFilterUpdate(¬chFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
|
||||
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
|
||||
|
||||
axis = (axis + 1) % 3;
|
||||
|
|
|
|||
|
|
@ -30,9 +30,6 @@ typedef struct gyroFftData_s {
|
|||
void gyroDataAnalyseInit(uint32_t targetLooptime);
|
||||
const gyroFftData_t *gyroFftData(int axis);
|
||||
struct gyroDev_s;
|
||||
struct gyro_s;
|
||||
void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro);
|
||||
void gyroDataAnalyseUpdate();
|
||||
void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn);
|
||||
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn);
|
||||
bool isDynamicFilterActive();
|
||||
|
||||
extern biquadFilter_t *notchFilterDyn[3];
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue