Skip to content

Commit 232310c

Browse files
rtlopezrtlopez
committed
notch filter rate compensation
1 parent d44487e commit 232310c

File tree

6 files changed

+42
-17
lines changed

6 files changed

+42
-17
lines changed

‎lib/Espfc/src/Sensor/GyroSensor.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,7 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate()
180180
{
181181
weight *= freqMargin * _rpm_fade_inv;
182182
}
183-
_model.state.gyro.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight);
183+
_model.state.gyro.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight, lrintf(_model.state.loopTimer.realRate));
184184
for (size_t i = 1; i < AXIS_COUNT_RPY; ++i)
185185
{
186186
// copy coefs from roll to pitch and yaw
@@ -237,7 +237,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate()
237237
float freq = _fft[i].peaks[p].freq;
238238
if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq)
239239
{
240-
_model.state.gyro.dynNotchFilter[p][i].reconfigure(freq, freq, q);
240+
_model.state.gyro.dynNotchFilter[p][i].reconfigure(freq, freq, q, 1.0f, lrintf(_model.state.loopTimer.realRate));
241241
}
242242
}
243243
}
@@ -267,7 +267,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate()
267267
size_t x = (p + i) % 3;
268268
int harmonic = (p / 3) + 1;
269269
int16_t f = Utils::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, _model.config.gyro.dynamicFilter.max_freq);
270-
_model.state.gyro.dynNotchFilter[p][x].reconfigure(f, f, q);
270+
_model.state.gyro.dynNotchFilter[p][x].reconfigure(f, f, q, 1.0f, lrintf(_model.state.loopTimer.realRate));
271271
}
272272
}
273273
}

‎lib/Espfc/src/Utils/Filter.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -359,6 +359,11 @@ void FAST_CODE_ATTR Filter::reconfigure(int16_t freq, int16_t cutoff, float q, f
359359
reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q, weight);
360360
}
361361

362+
void FAST_CODE_ATTR Filter::reconfigure(int16_t freq, int16_t cutoff, float q, float weight, int rate)
363+
{
364+
reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), rate, q, weight);
365+
}
366+
362367
void FAST_CODE_ATTR Filter::reconfigure(const FilterConfig& config, int rate)
363368
{
364369
_rate = rate;

‎lib/Espfc/src/Utils/Filter.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -147,10 +147,13 @@ class Filter
147147
void reset();
148148

149149
void reconfigure(int16_t freq, int16_t cutoff = 0);
150-
void reconfigure(int16_t freq, int16_t cutoff, float q, float weight = 1.0f);
150+
void reconfigure(int16_t freq, int16_t cutoff, float q, float weight);
151+
void reconfigure(int16_t freq, int16_t cutoff, float q, float weight, int rate);
152+
151153
void reconfigure(const FilterConfig& config, int rate);
152154
void reconfigure(const FilterConfig& config, int rate, float q, float weight);
153155
void reconfigure(const Filter& filter);
156+
154157
void setWeight(float weight);
155158
float getNotchQApprox(float freq, float cutoff);
156159
float getNotchQ(float freq, float cutoff);

‎lib/Espfc/src/Utils/Timer.cpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -6,17 +6,14 @@ namespace Espfc {
66

77
namespace Utils {
88

9-
Timer::Timer(): interval(0), last(0), next(0), iteration(0), delta(0)
10-
{
11-
}
12-
139
int Timer::setInterval(uint32_t interval)
1410
{
1511
this->interval = interval;
1612
this->rate = (1000000UL + interval / 2) / interval;
1713
this->denom = 1;
1814
this->delta = this->interval;
1915
this->intervalf = this->interval * 0.000001f;
16+
this->realRate = (float)this->rate;
2017
iteration = 0;
2118
return 1;
2219
}
@@ -28,6 +25,7 @@ int Timer::setRate(uint32_t rate, uint32_t denom)
2825
this->denom = denom;
2926
this->delta = this->interval;
3027
this->intervalf = this->interval * 0.000001f;
28+
this->realRate = (float)this->rate;
3129
iteration = 0;
3230
return 1;
3331
}
@@ -55,6 +53,7 @@ int FAST_CODE_ATTR Timer::update(uint32_t now)
5553
delta = now - last;
5654
last = now;
5755
iteration++;
56+
realRate += 0.1f * ((1000000.0f / delta) - realRate);
5857
return 1;
5958
}
6059

‎lib/Espfc/src/Utils/Timer.h‎

Lines changed: 10 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ namespace Utils {
99
class Timer
1010
{
1111
public:
12-
Timer();
12+
Timer() = default;
1313
int setInterval(uint32_t interval);
1414
int setRate(uint32_t rate, uint32_t denom = 1u);
1515

@@ -19,15 +19,16 @@ class Timer
1919
int update(uint32_t now);
2020
bool syncTo(const Timer& t, uint32_t slot = 0u);
2121

22-
uint32_t interval;
23-
uint32_t rate;
24-
uint32_t denom;
22+
uint32_t interval = 0;
23+
uint32_t rate = 0;
24+
uint32_t denom = 0;
2525

26-
uint32_t last;
27-
uint32_t next;
28-
volatile uint32_t iteration;
29-
uint32_t delta;
30-
float intervalf;
26+
uint32_t last = 0;
27+
uint32_t next = 0;
28+
volatile uint32_t iteration = 0;
29+
uint32_t delta = 0;
30+
float intervalf = 0.f;
31+
float realRate = 0.f;
3132
};
3233

3334
}

‎test/test_fc/test_fc.cpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,30 +60,47 @@ void test_timer_check()
6060

6161
TEST_ASSERT_EQUAL_UINT32(1000, timer.rate);
6262
TEST_ASSERT_EQUAL_UINT32(1000, timer.interval);
63+
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);
6364

6465
TEST_ASSERT_TRUE( timer.check(1000));
6566
TEST_ASSERT_EQUAL_UINT32(1, timer.iteration);
6667
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
68+
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);
6769

6870
TEST_ASSERT_FALSE(timer.check(1500));
6971
TEST_ASSERT_EQUAL_UINT32(1, timer.iteration);
7072
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
73+
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);
7174

7275
TEST_ASSERT_TRUE( timer.check(2000));
7376
TEST_ASSERT_EQUAL_UINT32(2, timer.iteration);
7477
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
78+
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);
7579

7680
TEST_ASSERT_TRUE( timer.check(3000));
7781
TEST_ASSERT_EQUAL_UINT32(3, timer.iteration);
7882
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
83+
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);
7984

8085
TEST_ASSERT_FALSE(timer.check(3999));
8186
TEST_ASSERT_EQUAL_UINT32(3, timer.iteration);
8287
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
88+
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);
8389

8490
TEST_ASSERT_TRUE( timer.check(4050));
8591
TEST_ASSERT_EQUAL_UINT32(4, timer.iteration);
8692
TEST_ASSERT_EQUAL_UINT32(1050, timer.delta);
93+
TEST_ASSERT_FLOAT_WITHIN(1.f, 995.f, timer.realRate);
94+
95+
TEST_ASSERT_TRUE( timer.check(5100));
96+
TEST_ASSERT_EQUAL_UINT32(5, timer.iteration);
97+
TEST_ASSERT_EQUAL_UINT32(1050, timer.delta);
98+
TEST_ASSERT_FLOAT_WITHIN(1.f, 990.f, timer.realRate);
99+
100+
TEST_ASSERT_TRUE( timer.check(6150));
101+
TEST_ASSERT_EQUAL_UINT32(6, timer.iteration);
102+
TEST_ASSERT_EQUAL_UINT32(1050, timer.delta);
103+
TEST_ASSERT_FLOAT_WITHIN(1.f, 987.f, timer.realRate);
87104
}
88105

89106
void test_timer_check_micros()

0 commit comments

Comments
 (0)