diff --git a/lib/Espfc/src/Sensor/GyroSensor.cpp b/lib/Espfc/src/Sensor/GyroSensor.cpp index 6da36b90..3718b5c5 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.cpp +++ b/lib/Espfc/src/Sensor/GyroSensor.cpp @@ -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 @@ -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)); } } } @@ -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)); } } } diff --git a/lib/Espfc/src/Utils/Filter.cpp b/lib/Espfc/src/Utils/Filter.cpp index f81b96c0..76d8ac62 100644 --- a/lib/Espfc/src/Utils/Filter.cpp +++ b/lib/Espfc/src/Utils/Filter.cpp @@ -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; diff --git a/lib/Espfc/src/Utils/Filter.h b/lib/Espfc/src/Utils/Filter.h index d5d6c819..927c149c 100644 --- a/lib/Espfc/src/Utils/Filter.h +++ b/lib/Espfc/src/Utils/Filter.h @@ -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); diff --git a/lib/Espfc/src/Utils/Timer.cpp b/lib/Espfc/src/Utils/Timer.cpp index c82fd599..9d7061c6 100644 --- a/lib/Espfc/src/Utils/Timer.cpp +++ b/lib/Espfc/src/Utils/Timer.cpp @@ -6,10 +6,6 @@ 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; @@ -17,6 +13,7 @@ int Timer::setInterval(uint32_t interval) this->denom = 1; this->delta = this->interval; this->intervalf = this->interval * 0.000001f; + this->realRate = (float)this->rate; iteration = 0; return 1; } @@ -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; } @@ -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; } diff --git a/lib/Espfc/src/Utils/Timer.h b/lib/Espfc/src/Utils/Timer.h index 3de7a958..31d3955a 100644 --- a/lib/Espfc/src/Utils/Timer.h +++ b/lib/Espfc/src/Utils/Timer.h @@ -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); @@ -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; }; } diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 2d240ec7..8daf8941 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -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()