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

configurable Acc notch filter

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2017-03-11 13:07:40 +01:00
parent a19cdefd11
commit 878360e33e
5 changed files with 41 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/fc/cli.c
View file

@ -536,6 +536,11 @@ static const clivalue_t valueTable[] = {
{ "current_adc_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = {ADC_CHN_NONE, ADC_CHN_MAX}, PG_ADC_CHANNEL_CONFIG, offsetof(adcChannelConfig_t, adcFunctionChannel[ADC_CURRENT]) },
{ "airspeed_adc_channel", VAR_UINT8 | MASTER_VALUE, .config.minmax = {ADC_CHN_NONE, ADC_CHN_MAX}, PG_ADC_CHANNEL_CONFIG, offsetof(adcChannelConfig_t, adcFunctionChannel[ADC_AIRSPEED]) },
#ifdef USE_ACC_NOTCH
{ "acc_notch_hz", VAR_UINT8 | MASTER_VALUE, .config.minmax = {0, 255 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t,acc_notch_hz) },
{ "acc_notch_cutoff", VAR_UINT8 | MASTER_VALUE, .config.minmax = {1, 255 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_notch_cutoff) },
#endif
// PG_ACCELEROMETER_CONFIG
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },

7
src/main/fc/config.c
View file

@ -214,6 +214,13 @@ void validateAndFixConfig(void)
pidProfileMutable()->dterm_soft_notch_hz = 0;
}
#endif
#ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
accelerometerConfigMutable()->acc_notch_hz = 0;
}
#endif
// Disable unused features
featureClear(FEATURE_UNUSED_1 | FEATURE_UNUSED_2);

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

@ -70,7 +70,9 @@ static uint16_t calibratingA = 0; // the calibration is done is the main lo
static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
#ifdef USE_ACC_NOTCH
static void *accNotchFilter[XYZ_AXIS_COUNT];
#endif
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
@ -79,7 +81,9 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
RESET_CONFIG_2(accelerometerConfig_t, instance,
.acc_align = ALIGN_DEFAULT,
.acc_hardware = ACC_AUTODETECT,
.acc_lpf_hz = 15
.acc_lpf_hz = 15,
.acc_notch_hz = 0,
.acc_notch_cutoff = 0
);
RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero,
.raw[X] = 0,
@ -444,7 +448,6 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
void accUpdate(void)
{
float temp;
if (!acc.dev.read(&acc.dev)) {
return;
}
@ -455,11 +458,18 @@ void accUpdate(void)
if (accelerometerConfig()->acc_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
temp = biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis]);
acc.accADC[axis] = lrintf(biquadFilterApply(accNotchFilter[axis], temp));
acc.accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis]));
}
}
#ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADC[axis] = lrintf(biquadFilterApply(accNotchFilter[axis], (float)acc.accADC[axis]));
}
}
#endif
if (!accIsCalibrationComplete()) {
performAcclerationCalibration();
}
@ -482,16 +492,23 @@ void accSetCalibrationValues(void)
void accInitFilters(void)
{
static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime);
accNotchFilter[axis] = &accFilterNotch[axis];
biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, 75, 30);
}
}
#ifdef USE_ACC_NOTCH
static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
if (acc.accTargetLooptime && accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accNotchFilter[axis] = &accFilterNotch[axis];
biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
}
}
#endif
}
bool accIsHealthy(void)

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

@ -52,6 +52,8 @@ typedef struct accelerometerConfig_s {
uint16_t acc_lpf_hz; // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
flightDynamicsTrims_t accZero; // Accelerometer offset
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
} accelerometerConfig_t;
PG_DECLARE(accelerometerConfig_t, accelerometerConfig);

1
src/main/target/common.h
View file

@ -59,6 +59,7 @@
#define USE_GYRO_NOTCH_1
#define USE_GYRO_NOTCH_2
#define USE_DTERM_NOTCH
#define USE_ACC_NOTCH
#define CMS
#define USE_DASHBOARD
#define USE_MSP_DISPLAYPORT