Skip to content

Commit

Permalink
Merge branch 'master' of ssh://github.com/rotorflight/rotorflight-fir…
Browse files Browse the repository at this point in the history 
…mware into esc_sensor_fly

* 'master' of ssh://github.com/rotorflight/rotorflight-firmware:
  Fix HPF bug and cross-coupling scale (rotorflight#158)
  feat: add MSP_PILOT_CONFIG with user definable parameters  (rotorflight#156)
  Use HPF instead of derivative in Cyclic Cross-Coupling
  Add firstOrderHPF in filter.c
  Improved PID defaults (rotorflight#140)
  Refactor RC response & acceleration filter (rotorflight#153)
  • Loading branch information
@bob01
bob01 committed Sep 17, 2024
2 parents d0f2410 + 6229abf commit cc3c8be
Show file tree
Hide file tree
Showing 12 changed files with 161 additions and 101 deletions.
11 changes: 11 additions & 0 deletions src/main/cli/settings.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -501,6 +501,10 @@ const char * const lookupTableEdgeMode[] = {
"FALLING", "RISING"
};

const char * const lookupTableParamType[] = {
"NONE", "TIMER1", "TIMER2", "TIMER3", "GV1", "GV2", "GV3", "GV4", "GV5", "GV6", "GV7", "GV8", "GV9"
};

#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }

const lookupTableEntry_t lookupTables[] = {
Expand Down Expand Up @@ -614,6 +618,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableTelemMode),
LOOKUP_TABLE_ENTRY(lookupTablePullMode),
LOOKUP_TABLE_ENTRY(lookupTableEdgeMode),
LOOKUP_TABLE_ENTRY(lookupTableParamType),
};

#undef LOOKUP_TABLE_ENTRY
Expand Down Expand Up @@ -1701,6 +1706,12 @@ const clivalue_t valueTable[] = {
{ "display_name", VAR_UINT8 | MASTER_VALUE | MODE_STRING, .config.string = { 1, MAX_NAME_LENGTH, STRING_FLAGS_NONE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, displayName) },
#endif
{ "model_id", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 99 }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, modelId) },
{ "model_param1_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PARAM_TYPE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, modelParam1Type) },
{ "model_param1_value", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, modelParam1Value) },
{ "model_param2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PARAM_TYPE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, modelParam2Type) },
{ "model_param2_value", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, modelParam2Value) },
{ "model_param3_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PARAM_TYPE }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, modelParam3Type) },
{ "model_param3_value", VAR_INT16 | MASTER_VALUE, .config.minmax = { INT16_MIN, INT16_MAX }, PG_PILOT_CONFIG, offsetof(pilotConfig_t, modelParam3Value) },

// PG_POSITION
{ "position_alt_source", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, alt_source) },
Expand Down
2 changes: 1 addition & 1 deletion src/main/cli/settings.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,9 +129,9 @@ typedef enum {
TABLE_SWASH_TYPE,
TABLE_DTERM_MODE,
TABLE_TELEM_MODE,

TABLE_INPUT_PULL_MODE,
TABLE_INPUT_EDGE_MODE,
TABLE_PARAM_TYPE,

LOOKUP_TABLE_COUNT
} lookupTableIndex_e;
Expand Down
68 changes: 37 additions & 31 deletions src/main/common/filter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -419,7 +419,7 @@ FAST_CODE float ewma3FilterApply(ewma3Filter_t *filter, float input)
* a₀ + a₁⋅z⁻¹
*
* Where
* b₀ = Wc / (1 + K)
* b₀ = 2*Fs * K / (1 + K)
* b₁ = -b₀
* a₀ = 1
* a₁ = (1 - K) / (1 + K)
Expand All @@ -439,24 +439,20 @@ void difFilterInit(difFilter_t *filter, float cutoff, float sampleRate)

void difFilterUpdate(difFilter_t *filter, float cutoff, float sampleRate)
{
cutoff = limitCutoff(cutoff, sampleRate);
cutoff = limitCutoff(cutoff, sampleRate / 2);

const float K = tan_approx(M_PIf * cutoff / sampleRate);
const float Wc = M_2PIf * cutoff;
const float W = tan_approx(M_PIf * cutoff / sampleRate);

filter->a = (K - 1) / (K + 1);
filter->b = Wc / (K + 1);
filter->a = (W - 1) / (W + 1);
filter->b = 2 * sampleRate * W / (W + 1);
}

FAST_CODE float difFilterApply(difFilter_t *filter, float input)
{
const float output =
filter->b * input -
filter->b * filter->x1 -
filter->a * filter->y1;
const float output = filter->b * (input - filter->x1) - filter->a * filter->y1;

filter->y1 = output;
filter->x1 = input;
filter->y1 = output;

return output;
}
Expand Down Expand Up @@ -622,44 +618,54 @@ FAST_CODE float filterStackApply(biquadFilter_t *filter, float input, int count)
}


// First order filter
// First order filters

void firstOrderFilterInit(order1Filter_t *filter, float cutoff, float sampleRate)
void firstOrderLPFInit(order1Filter_t *filter, float cutoff, float sampleRate)
{
filter->x1 = 0;
filter->y1 = 0;

firstOrderFilterUpdate(filter, cutoff, sampleRate);
firstOrderLPFUpdate(filter, cutoff, sampleRate);
}

FAST_CODE void firstOrderFilterUpdate(order1Filter_t *filter, float cutoff, float sampleRate)
void firstOrderHPFInit(order1Filter_t *filter, float cutoff, float sampleRate)
{
filter->x1 = 0;
filter->y1 = 0;

firstOrderHPFUpdate(filter, cutoff, sampleRate);
}

FAST_CODE void firstOrderLPFUpdate(order1Filter_t *filter, float cutoff, float sampleRate)
{
cutoff = limitCutoff(cutoff, sampleRate);

const float W = tan_approx(M_PIf * cutoff / sampleRate);

filter->a1 = (W - 1) / (W + 1);
filter->b1 = filter->b0 = W / (W + 1);
filter->b0 = W / (W + 1);
filter->b1 = filter->b0;
}

FAST_CODE float firstOrderFilterApplyDF1(order1Filter_t *filter, float input)
FAST_CODE void firstOrderHPFUpdate(order1Filter_t *filter, float cutoff, float sampleRate)
{
const float output =
filter->b0 * input +
filter->b1 * filter->x1 -
filter->a1 * filter->y1;
cutoff = limitCutoff(cutoff, sampleRate / 2);

filter->x1 = input;
filter->y1 = output;
const float W = tan_approx(M_PIf * cutoff / sampleRate);

return output;
filter->a1 = (W - 1) / (W + 1);
filter->b0 = 1 / (W + 1);
filter->b1 = -filter->b0;
}

FAST_CODE float firstOrderFilterApplyTF2(order1Filter_t *filter, float input)
FAST_CODE float firstOrderFilterApply(order1Filter_t *filter, float input)
{
const float output = filter->b0 * input + filter->x1;
const float output =
filter->b0 * input +
filter->b1 * filter->x1 -
filter->a1 * filter->y1;

filter->x1 = filter->b1 * input - filter->a1 * output;
filter->x1 = input;
filter->y1 = output;

return output;
Expand Down Expand Up @@ -742,13 +748,13 @@ void lowpassFilterInit(filter_t *filter, uint8_t type, float cutoff, float sampl

case LPF_1ST_ORDER:
case LPF_ORDER1:
filter->init = (filterInitFn)firstOrderFilterInit;
filter->init = (filterInitFn)firstOrderLPFInit;
if (flags & LPF_UPDATE) {
filter->apply = (filterApplyFn)firstOrderFilterApplyDF1;
filter->update = (filterUpdateFn)firstOrderFilterUpdate;
filter->apply = (filterApplyFn)firstOrderFilterApply;
filter->update = (filterUpdateFn)firstOrderLPFUpdate;
}
else {
filter->apply = (filterApplyFn)firstOrderFilterApplyTF2;
filter->apply = (filterApplyFn)firstOrderFilterApply;
filter->update = (filterUpdateFn)nilFilterUpdate;
}
break;
Expand Down
9 changes: 5 additions & 4 deletions src/main/common/filter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -267,10 +267,11 @@ float biquadFilterApply(biquadFilter_t *filter, float input);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApplyDF2(biquadFilter_t *filter, float input);

void firstOrderFilterInit(order1Filter_t *filter, float cutoff, float sampleRate);
void firstOrderFilterUpdate(order1Filter_t *filter, float cutoff, float sampleRate);
float firstOrderFilterApplyDF1(order1Filter_t *filter, float input);
float firstOrderFilterApplyTF2(order1Filter_t *filter, float input);
void firstOrderLPFInit(order1Filter_t *filter, float cutoff, float sampleRate);
void firstOrderLPFUpdate(order1Filter_t *filter, float cutoff, float sampleRate);
void firstOrderHPFInit(order1Filter_t *filter, float cutoff, float sampleRate);
void firstOrderHPFUpdate(order1Filter_t *filter, float cutoff, float sampleRate);
float firstOrderFilterApply(order1Filter_t *filter, float input);

float filterStackApply(biquadFilter_t *filter, float input, int count);

Expand Down
12 changes: 6 additions & 6 deletions src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,9 +236,9 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile)
pid.cyclicCrossCouplingGain[FD_PITCH] = pidProfile->cyclic_cross_coupling_gain * mixerRotationSign() * -CROSS_COUPLING_SCALE;
pid.cyclicCrossCouplingGain[FD_ROLL] = pid.cyclicCrossCouplingGain[FD_PITCH] * pidProfile->cyclic_cross_coupling_ratio / -100.0f;

// Cross-coupling derivative filters
difFilterInit(&pid.crossCouplingFilter[FD_PITCH], pidProfile->cyclic_cross_coupling_cutoff / 10.0f, pid.freq);
difFilterInit(&pid.crossCouplingFilter[FD_ROLL], pidProfile->cyclic_cross_coupling_cutoff / 10.0f, pid.freq);
// Cross-coupling filters
firstOrderHPFInit(&pid.crossCouplingFilter[FD_PITCH], pidProfile->cyclic_cross_coupling_cutoff / 10.0f, pid.freq);
firstOrderHPFInit(&pid.crossCouplingFilter[FD_ROLL], pidProfile->cyclic_cross_coupling_cutoff / 10.0f, pid.freq);

// Initialise sub-profiles
governorInitProfile(pidProfile);
Expand Down Expand Up @@ -444,9 +444,9 @@ static void pidApplyPrecomp(void)

static void pidApplyCyclicCrossCoupling(void)
{
// Setpoint derivative filter
const float pitchDeriv = difFilterApply(&pid.crossCouplingFilter[FD_PITCH], pid.data[FD_PITCH].setPoint);
const float rollDeriv = difFilterApply(&pid.crossCouplingFilter[FD_ROLL], pid.data[FD_ROLL].setPoint);
// Cross-coupling filters
const float pitchDeriv = firstOrderFilterApply(&pid.crossCouplingFilter[FD_PITCH], pid.data[FD_PITCH].setPoint);
const float rollDeriv = firstOrderFilterApply(&pid.crossCouplingFilter[FD_ROLL], pid.data[FD_ROLL].setPoint);
const float pitchComp = rollDeriv * pid.cyclicCrossCouplingGain[FD_ROLL];
const float rollComp = pitchDeriv * pid.cyclicCrossCouplingGain[FD_PITCH];

Expand Down
4 changes: 2 additions & 2 deletions src/main/flight/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@
#define YAW_F_TERM_SCALE 0.000025f
#define YAW_B_TERM_SCALE 1.0e-6f

#define CROSS_COUPLING_SCALE 1.0e-6f
#define CROSS_COUPLING_SCALE 10.0e-6f

typedef struct {
float P;
Expand Down Expand Up @@ -135,7 +135,7 @@ typedef struct pid_s {
difFilter_t dtermFilter[PID_AXIS_COUNT];
difFilter_t btermFilter[PID_AXIS_COUNT];

difFilter_t crossCouplingFilter[XY_AXIS_COUNT];
order1Filter_t crossCouplingFilter[XY_AXIS_COUNT];

} pid_t;

Expand Down
Loading

0 comments on commit cc3c8be

Please sign in to comment.