Skip to content

Commit

Permalink
Use separate RPM filters banks for RPY
Browse files Browse the repository at this point in the history
  • Loading branch information
@pmattila
pmattila committed Oct 3, 2024
1 parent b7ae36c commit f68d7b6
Show file tree
Hide file tree
Showing 3 changed files with 160 additions and 149 deletions.
8 changes: 4 additions & 4 deletions src/main/cli/settings.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1685,10 +1685,10 @@ const clivalue_t valueTable[] = {
#endif

#ifdef USE_RPM_FILTER
{ "gyro_rpm_filter_bank_rpm_source", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_source) },
{ "gyro_rpm_filter_bank_rpm_ratio", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_ratio) },
{ "gyro_rpm_filter_bank_rpm_limit", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_limit) },
{ "gyro_rpm_filter_bank_notch_q", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_BANK_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_notch_q) },
{ "gyro_rpm_filter_bank_rpm_source", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_PARAM_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_source) },
{ "gyro_rpm_filter_bank_rpm_ratio", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_PARAM_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_ratio) },
{ "gyro_rpm_filter_bank_rpm_limit", VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_PARAM_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_rpm_limit) },
{ "gyro_rpm_filter_bank_notch_q", VAR_UINT8 | MASTER_VALUE | MODE_ARRAY, .config.array.length = RPM_FILTER_PARAM_COUNT, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, filter_bank_notch_q) },
#endif

#ifdef USE_RX_FLYSKY
Expand Down
290 changes: 150 additions & 140 deletions src/main/flight/rpm_filter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@
#include "rpm_filter.h"

// Number of banks to update in one cycle
#define RPM_UPDATE_BANK_COUNT 1
#define RPM_UPDATE_BANK_COUNT 4

typedef struct rpmFilterBank_s
typedef struct
{
uint8_t motor;

Expand All @@ -51,15 +51,17 @@ typedef struct rpmFilterBank_s
float maxHz;
float notchQ;

biquadFilter_t notch[XYZ_AXIS_COUNT];
biquadFilter_t notch;

} rpmFilterBank_t;


FAST_DATA_ZERO_INIT static rpmFilterBank_t filterBank[RPM_FILTER_BANK_COUNT];
FAST_DATA_ZERO_INIT static rpmFilterBank_t filterBank[XYZ_AXIS_COUNT][RPM_FILTER_BANK_COUNT];

FAST_DATA_ZERO_INIT static uint8_t activeBankCount;
FAST_DATA_ZERO_INIT static uint8_t updateAxisNumber;
FAST_DATA_ZERO_INIT static uint8_t updateBankNumber;
FAST_DATA_ZERO_INIT static uint8_t updateBankCount;
FAST_DATA_ZERO_INIT static uint8_t totalBankCount;


INIT_CODE void rpmFilterInit(void)
Expand All @@ -75,178 +77,186 @@ INIT_CODE void rpmFilterInit(void)
const bool enable10 = mainGearRatio != 1.0f;
const bool enable20 = tailGearRatio != 1.0f && mixerMotorizedTail();

int bankNumber = 0;
bool useRPYconfig = true;

/* Check if a separate R/P/Y config is used */
for (int index = RPM_FILTER_BANK_COUNT; index < RPM_FILTER_PARAM_COUNT; index++) {
if (config->filter_bank_rpm_source[index] != 0 &&
config->filter_bank_rpm_ratio[index] != 0 &&
config->filter_bank_notch_q[index] != 0) {
useRPYconfig = false;
break;
}
}

#define CHECK_SOURCE(motor) if (!isMotorFastRpmSourceActive(motor)) goto error

for (int index = 0; index < RPM_FILTER_BANK_COUNT; index++)
{
if (config->filter_bank_rpm_source[index] == 0 ||
config->filter_bank_rpm_ratio[index] == 0 ||
config->filter_bank_notch_q[index] == 0)
continue;

/*
* NOTE! rpm_limit has different meaning depending on rpm_source
*
* 1-4 Minimum RPM of the MOTOR
* 10-18 Minimum RPM of the Main ROTOR
* 20-28 Minimum RPM of the Tail ROTOR
*/

rpmFilterBank_t *bank = &filterBank[bankNumber];

// RPM source for this bank
const unsigned source = config->filter_bank_rpm_source[index];

// Ratio converts RPM to Hz
const float ratio = 10000.0f / (constrain(config->filter_bank_rpm_ratio[index], 1, 50000) * 60.0f);

// Absolute limits
const float minHzLimit = 0.40f * gyro.filterRateHz;
const float maxHzLimit = 0.45f * gyro.filterRateHz;

// Q value
const float notchQ = constrainf(config->filter_bank_notch_q[index], 5, 100) / 10;

// Motor RPM based notches
if (source >= 1 && source <= getMotorCount()) {
CHECK_SOURCE(source - 1);
bank->motor = source - 1;
bank->ratio = ratio;
bank->minHz = constrainf(config->filter_bank_rpm_limit[index] * ratio, 10, minHzLimit);
bank->maxHz = maxHzLimit;
bank->notchQ = notchQ;
bankNumber++;
}
// Main Motor (M1)
else if (source == 10) {
if (enable10) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int bank = 0; bank < RPM_FILTER_BANK_COUNT; bank++)
{
const int index = useRPYconfig ? bank : axis * RPM_FILTER_BANK_COUNT + bank;

if (config->filter_bank_rpm_source[index] == 0 ||
config->filter_bank_rpm_ratio[index] == 0 ||
config->filter_bank_notch_q[index] == 0)
continue;

/*
* NOTE! rpm_limit has different meaning depending on rpm_source
*
* 1-4 Minimum RPM of the MOTOR
* 10-18 Minimum RPM of the Main ROTOR
* 20-28 Minimum RPM of the Tail ROTOR
*/

rpmFilterBank_t *filter = &filterBank[axis][bank];

// RPM source for this bank
const unsigned source = config->filter_bank_rpm_source[index];

// Ratio converts RPM to Hz
const float ratio = 10000.0f / (constrain(config->filter_bank_rpm_ratio[index], 1, 50000) * 60.0f);

// Absolute limits
const float minHzLimit = 0.40f * gyro.filterRateHz;
const float maxHzLimit = 0.45f * gyro.filterRateHz;

// Q value
const float notchQ = constrainf(config->filter_bank_notch_q[index], 5, 100) / 10;

// Motor RPM based notches
if (source >= 1 && source <= getMotorCount()) {
CHECK_SOURCE(source - 1);
filter->motor = source - 1;
filter->ratio = ratio;
filter->minHz = constrainf(config->filter_bank_rpm_limit[index] * ratio, 10, minHzLimit);
filter->maxHz = maxHzLimit;
filter->notchQ = notchQ;
}
// Main Motor (M1)
else if (source == 10) {
if (enable10) {
CHECK_SOURCE(mainMotorIndex);
filter->motor = mainMotorIndex;
filter->ratio = ratio;
filter->minHz = constrainf((config->filter_bank_rpm_limit[index] / mainGearRatio) * ratio, 10, minHzLimit);
filter->maxHz = maxHzLimit;
filter->notchQ = notchQ;
}
}
// Main Rotor harmonics
else if (source >= 11 && source <= 18) {
CHECK_SOURCE(mainMotorIndex);
bank->motor = mainMotorIndex;
bank->ratio = ratio;
bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / mainGearRatio) * ratio, 10, minHzLimit);
bank->maxHz = maxHzLimit;
bank->notchQ = notchQ;
bankNumber++;
const int harmonic = source - 10;
filter->motor = mainMotorIndex;
filter->ratio = mainGearRatio * harmonic * ratio;
filter->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit);
filter->maxHz = maxHzLimit;
filter->notchQ = notchQ;
}
}
// Main Rotor harmonics
else if (source >= 11 && source <= 18) {
CHECK_SOURCE(mainMotorIndex);
const int harmonic = source - 10;
bank->motor = mainMotorIndex;
bank->ratio = mainGearRatio * harmonic * ratio;
bank->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit);
bank->maxHz = maxHzLimit;
bank->notchQ = notchQ;
bankNumber++;
}
// Tail Motor (M2)
else if (source == 20) {
if (enable20) {
// Tail Motor (M2)
else if (source == 20) {
if (enable20) {
CHECK_SOURCE(tailMotorIndex);
filter->motor = tailMotorIndex;
filter->ratio = ratio;
filter->minHz = constrainf((config->filter_bank_rpm_limit[index] / tailGearRatio) * ratio, 10, minHzLimit);
filter->maxHz = maxHzLimit;
filter->notchQ = notchQ;
}
}
// Tail Rotor harmonics
else if (source >= 21 && source <= 28) {
CHECK_SOURCE(tailMotorIndex);
bank->motor = tailMotorIndex;
bank->ratio = ratio;
bank->minHz = constrainf((config->filter_bank_rpm_limit[index] / tailGearRatio) * ratio, 10, minHzLimit);
bank->maxHz = maxHzLimit;
bank->notchQ = notchQ;
bankNumber++;
const int harmonic = source - 20;
filter->motor = tailMotorIndex;
filter->ratio = tailGearRatio * harmonic * ratio;
filter->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit);
filter->maxHz = maxHzLimit;
filter->notchQ = notchQ;
}
else {
goto error;
}
}
// Tail Rotor harmonics
else if (source >= 21 && source <= 28) {
CHECK_SOURCE(tailMotorIndex);
const int harmonic = source - 20;
bank->motor = tailMotorIndex;
bank->ratio = tailGearRatio * harmonic * ratio;
bank->minHz = constrainf((config->filter_bank_rpm_limit[index] * harmonic) * ratio, 10, minHzLimit);
bank->maxHz = maxHzLimit;
bank->notchQ = notchQ;
bankNumber++;
}
else {
goto error;
}
}

// Set activeBankCount to the number of configured notches
activeBankCount = bankNumber;

// Init all filters @minHz. As soon as the motor is running, the filters are updated to the real RPM.
for (int index = 0; index < activeBankCount; index++) {
rpmFilterBank_t *bank = &filterBank[index];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&bank->notch[axis], bank->minHz, gyro.filterRateHz, bank->notchQ, BIQUAD_NOTCH);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int bank = 0; bank < RPM_FILTER_BANK_COUNT; bank++) {
rpmFilterBank_t *filter = &filterBank[axis][bank];
if (filter->notchQ) {
biquadFilterInit(&filter->notch, filter->minHz, gyro.filterRateHz, filter->notchQ, BIQUAD_NOTCH);
totalBankCount++;
}
}
}

// Number of banks to update in one update cycle
updateBankCount = MIN(totalBankCount, RPM_UPDATE_BANK_COUNT);

return;

error:
activeBankCount = 0;

setArmingDisabled(ARMING_DISABLED_RPMFILTER);
}

FAST_CODE float rpmFilterGyro(int axis, float value)
{
for (int index = 0; index < activeBankCount; index++) {
value = biquadFilterApplyDF1(&filterBank[index].notch[axis], value);
for (int bank = 0; bank < RPM_FILTER_BANK_COUNT; bank++) {
rpmFilterBank_t *filter = &filterBank[axis][bank];
if (filter->notchQ) {
value = biquadFilterApplyDF1(&filter->notch, value);
}
}
return value;
}

void rpmFilterUpdate()
{
if (activeBankCount > 0) {

if (totalBankCount)
{
// Actual update rate - allow ±25% variation
const float updateRate = gyro.filterRateHz * constrainf(schedulerGetCycleTimeMultiplier(), 0.75f, 1.25f);

// Number of banks to update in one update cycle
for (int i = 0; i < RPM_UPDATE_BANK_COUNT; i++) {

// Current filter bank
rpmFilterBank_t *bank = &filterBank[updateBankNumber];

// Calculate notch filter center frequency
const float rpm = getMotorRPMf(bank->motor);
const float freq = rpm * bank->ratio;
const float notch = constrainf(freq, bank->minHz, bank->maxHz);

// Notch filters for Roll,Pitch,Yaw
biquadFilter_t *R = &bank->notch[0];
biquadFilter_t *P = &bank->notch[1];
biquadFilter_t *Y = &bank->notch[2];

// Update the filter coefficients
biquadFilterUpdate(R, notch, updateRate, bank->notchQ, BIQUAD_NOTCH);

// Transfer the filter coefficients from Roll axis filter into Pitch and Yaw
P->b0 = Y->b0 = R->b0;
P->b1 = Y->b1 = R->b1;
P->b2 = Y->b2 = R->b2;
P->a1 = Y->a1 = R->a1;
P->a2 = Y->a2 = R->a2;

// Set debug if bank number matches
if (updateBankNumber == debugAxis) {
DEBUG(RPM_FILTER, 0, rpm);
DEBUG(RPM_FILTER, 1, freq * 10);
DEBUG(RPM_FILTER, 2, notch * 10);
DEBUG(RPM_FILTER, 3, updateRate * 10);
DEBUG(RPM_FILTER, 4, bank->motor);
DEBUG(RPM_FILTER, 5, bank->minHz * 10);
DEBUG(RPM_FILTER, 6, bank->maxHz * 10);
DEBUG(RPM_FILTER, 7, bank->notchQ * 10);
for (int count = 0; count < updateBankCount;) {

// Current filter
rpmFilterBank_t *filter = &filterBank[updateAxisNumber][updateBankNumber];

if (filter->notchQ) {

// Calculate notch filter center frequency
const float rpm = getMotorRPMf(filter->motor);
const float freq = rpm * filter->ratio;
const float notch = constrainf(freq, filter->minHz, filter->maxHz);

// Update the filter coefficients
biquadFilterUpdate(&filter->notch, notch, updateRate, filter->notchQ, BIQUAD_NOTCH);

// Set debug if bank number matches
if (debugAxis == updateAxisNumber * RPM_FILTER_BANK_COUNT + updateBankNumber) {
DEBUG(RPM_FILTER, 0, rpm);
DEBUG(RPM_FILTER, 1, freq * 10);
DEBUG(RPM_FILTER, 2, notch * 10);
DEBUG(RPM_FILTER, 3, updateRate * 10);
DEBUG(RPM_FILTER, 4, filter->motor);
DEBUG(RPM_FILTER, 5, filter->minHz * 10);
DEBUG(RPM_FILTER, 6, filter->maxHz * 10);
DEBUG(RPM_FILTER, 7, filter->notchQ * 10);
}

// Bank updated
count++;
}

// Next bank
updateBankNumber = (updateBankNumber + 1) % activeBankCount;
updateBankNumber = (updateBankNumber + 1) % RPM_FILTER_BANK_COUNT;
if (updateBankNumber == 0)
updateAxisNumber = (updateAxisNumber + 1) % XYZ_AXIS_COUNT;
}
}
}

#endif

11 changes: 6 additions & 5 deletions src/main/pg/rpm_filter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,14 +19,15 @@

#include "pg/pg.h"

#define RPM_FILTER_BANK_COUNT 16
#define RPM_FILTER_BANK_COUNT 16
#define RPM_FILTER_PARAM_COUNT (3 * RPM_FILTER_BANK_COUNT)

typedef struct rpmFilteConfig_s
{
uint8_t filter_bank_rpm_source[RPM_FILTER_BANK_COUNT]; // RPM source
uint16_t filter_bank_rpm_ratio[RPM_FILTER_BANK_COUNT]; // RPM ratio *1000
uint16_t filter_bank_rpm_limit[RPM_FILTER_BANK_COUNT]; // RPM minimum limit
uint8_t filter_bank_notch_q[RPM_FILTER_BANK_COUNT]; // Notch Q *10
uint8_t filter_bank_rpm_source[RPM_FILTER_PARAM_COUNT]; // RPM source index
uint16_t filter_bank_rpm_ratio[RPM_FILTER_PARAM_COUNT]; // RPM ratio *1000
uint16_t filter_bank_rpm_limit[RPM_FILTER_PARAM_COUNT]; // RPM low limit
uint8_t filter_bank_notch_q[RPM_FILTER_PARAM_COUNT]; // Notch Q *10

} rpmFilterConfig_t;

Expand Down

0 comments on commit f68d7b6

Please sign in to comment.