Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Separate RPM filters for RPY #177

Closed
wants to merge 3 commits into from
Closed

Separate RPM filters for RPY #177

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4ddef54
Use separate RPM filters banks for RPY
pmattila Oct 3, 2024
40e692b
Fix RPM_FILTER MSP
pmattila Oct 4, 2024
e27cfb0
Fix RPM filter parameter check
pmattila Oct 11, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions src/main/cli/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1689,10 +1689,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
6 changes: 6 additions & 0 deletions src/main/common/types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,3 +27,9 @@ typedef unsigned char uchar;

typedef uint32_t bitmap_t;
typedef uint64_t bitmap64_t;

typedef struct {
int min;
int max;
} range_t;

3 changes: 2 additions & 1 deletion src/main/config/config.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -668,9 +668,10 @@ void validateAndFixGyroConfig(void)
#ifdef USE_RPM_FILTER
if (featureIsConfigured(FEATURE_RPM_FILTER)) {
rpmFilterConfig_t *config = rpmFilterConfigMutable();
for (int index = 0; index < RPM_FILTER_BANK_COUNT; index++) {
for (int index = 0; index < RPM_FILTER_PARAM_COUNT; index++) {
if (config->filter_bank_rpm_source[index] == 0 ||
config->filter_bank_rpm_ratio[index] == 0 ||
config->filter_bank_rpm_limit[index] == 0 ||
config->filter_bank_notch_q[index] == 0)
{
config->filter_bank_rpm_source[index] = 0;
Expand Down
290 changes: 150 additions & 140 deletions src/main/flight/rpm_filter.c
View file
Open in desktop
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 = false;

/* 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 = true;
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, 250) / 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 ? axis * RPM_FILTER_BANK_COUNT + bank : 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, 250) / 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

Loading