Skip to content

notch filter rate compensation #145

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

Draft
rtlopez wants to merge 1 commit into
base: master
Choose a base branch
from
Draft

notch filter rate compensation #145

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
6 changes: 3 additions & 3 deletions lib/Espfc/src/Sensor/GyroSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ void FAST_CODE_ATTR GyroSensor::rpmFilterUpdate()
{
weight *= freqMargin * _rpm_fade_inv;
}
_model.state.gyro.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight);
_model.state.gyro.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight, lrintf(_model.state.loopTimer.realRate));
for (size_t i = 1; i < AXIS_COUNT_RPY; ++i)
{
// copy coefs from roll to pitch and yaw
Expand Down Expand Up @@ -237,7 +237,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate()
float freq = _fft[i].peaks[p].freq;
if (freq >= _model.config.gyro.dynamicFilter.min_freq && freq <= _model.config.gyro.dynamicFilter.max_freq)
{
_model.state.gyro.dynNotchFilter[p][i].reconfigure(freq, freq, q);
_model.state.gyro.dynNotchFilter[p][i].reconfigure(freq, freq, q, 1.0f, lrintf(_model.state.loopTimer.realRate));
}
}
}
Expand Down Expand Up @@ -267,7 +267,7 @@ void FAST_CODE_ATTR GyroSensor::dynNotchFilterUpdate()
size_t x = (p + i) % 3;
int harmonic = (p / 3) + 1;
int16_t f = Utils::clamp((int16_t)lrintf(freq * harmonic), _model.config.gyro.dynamicFilter.min_freq, _model.config.gyro.dynamicFilter.max_freq);
_model.state.gyro.dynNotchFilter[p][x].reconfigure(f, f, q);
_model.state.gyro.dynNotchFilter[p][x].reconfigure(f, f, q, 1.0f, lrintf(_model.state.loopTimer.realRate));
}
}
}
Expand Down
5 changes: 5 additions & 0 deletions lib/Espfc/src/Utils/Filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -359,6 +359,11 @@ void FAST_CODE_ATTR Filter::reconfigure(int16_t freq, int16_t cutoff, float q, f
reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q, weight);
}

void FAST_CODE_ATTR Filter::reconfigure(int16_t freq, int16_t cutoff, float q, float weight, int rate)
{
reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), rate, q, weight);
}

void FAST_CODE_ATTR Filter::reconfigure(const FilterConfig& config, int rate)
{
_rate = rate;
Expand Down
5 changes: 4 additions & 1 deletion lib/Espfc/src/Utils/Filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,10 +147,13 @@ class Filter
void reset();

void reconfigure(int16_t freq, int16_t cutoff = 0);
void reconfigure(int16_t freq, int16_t cutoff, float q, float weight = 1.0f);
void reconfigure(int16_t freq, int16_t cutoff, float q, float weight);
void reconfigure(int16_t freq, int16_t cutoff, float q, float weight, int rate);

void reconfigure(const FilterConfig& config, int rate);
void reconfigure(const FilterConfig& config, int rate, float q, float weight);
void reconfigure(const Filter& filter);

void setWeight(float weight);
float getNotchQApprox(float freq, float cutoff);
float getNotchQ(float freq, float cutoff);
Expand Down
7 changes: 3 additions & 4 deletions lib/Espfc/src/Utils/Timer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,17 +6,14 @@ namespace Espfc {

namespace Utils {

Timer::Timer(): interval(0), last(0), next(0), iteration(0), delta(0)
{
}

int Timer::setInterval(uint32_t interval)
{
this->interval = interval;
this->rate = (1000000UL + interval / 2) / interval;
this->denom = 1;
this->delta = this->interval;
this->intervalf = this->interval * 0.000001f;
this->realRate = (float)this->rate;
iteration = 0;
return 1;
}
Expand All @@ -28,6 +25,7 @@ int Timer::setRate(uint32_t rate, uint32_t denom)
this->denom = denom;
this->delta = this->interval;
this->intervalf = this->interval * 0.000001f;
this->realRate = (float)this->rate;
iteration = 0;
return 1;
}
Expand Down Expand Up @@ -55,6 +53,7 @@ int FAST_CODE_ATTR Timer::update(uint32_t now)
delta = now - last;
last = now;
iteration++;
realRate += 0.1f * ((1000000.0f / delta) - realRate);
return 1;
}

Expand Down
19 changes: 10 additions & 9 deletions lib/Espfc/src/Utils/Timer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ namespace Utils {
class Timer
{
public:
Timer();
Timer() = default;
int setInterval(uint32_t interval);
int setRate(uint32_t rate, uint32_t denom = 1u);

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

uint32_t interval;
uint32_t rate;
uint32_t denom;
uint32_t interval = 0;
uint32_t rate = 0;
uint32_t denom = 0;

uint32_t last;
uint32_t next;
volatile uint32_t iteration;
uint32_t delta;
float intervalf;
uint32_t last = 0;
uint32_t next = 0;
volatile uint32_t iteration = 0;
uint32_t delta = 0;
float intervalf = 0.f;
float realRate = 0.f;
};

}
Expand Down
17 changes: 17 additions & 0 deletions test/test_fc/test_fc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,30 +60,47 @@ void test_timer_check()

TEST_ASSERT_EQUAL_UINT32(1000, timer.rate);
TEST_ASSERT_EQUAL_UINT32(1000, timer.interval);
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);

TEST_ASSERT_TRUE( timer.check(1000));
TEST_ASSERT_EQUAL_UINT32(1, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);

TEST_ASSERT_FALSE(timer.check(1500));
TEST_ASSERT_EQUAL_UINT32(1, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);

TEST_ASSERT_TRUE( timer.check(2000));
TEST_ASSERT_EQUAL_UINT32(2, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);

TEST_ASSERT_TRUE( timer.check(3000));
TEST_ASSERT_EQUAL_UINT32(3, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);

TEST_ASSERT_FALSE(timer.check(3999));
TEST_ASSERT_EQUAL_UINT32(3, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1000, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 1000.f, timer.realRate);

TEST_ASSERT_TRUE( timer.check(4050));
TEST_ASSERT_EQUAL_UINT32(4, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1050, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 995.f, timer.realRate);

TEST_ASSERT_TRUE( timer.check(5100));
TEST_ASSERT_EQUAL_UINT32(5, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1050, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 990.f, timer.realRate);

TEST_ASSERT_TRUE( timer.check(6150));
TEST_ASSERT_EQUAL_UINT32(6, timer.iteration);
TEST_ASSERT_EQUAL_UINT32(1050, timer.delta);
TEST_ASSERT_FLOAT_WITHIN(1.f, 987.f, timer.realRate);
}

void test_timer_check_micros()
Expand Down