| Directory: | src/ |
|---|---|
| File: | src/LowPassFilter.cpp |
| Date: | 2026-10-06 07:41:07 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 111 | 111 | 100.0% |
| Branches: | 73 | 110 | 66.4% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /***************************************************************************** | ||
| 2 | * | ||
| 3 | * Copyright (C) 2021 - 2022 Jonathan Grahl <jonathan.grahl@igh.de> | ||
| 4 | * 2021 - 2026 Florian Pose <florian.pose@igh.de> | ||
| 5 | * | ||
| 6 | * This file is part of the reta library (realtime-automation). | ||
| 7 | * | ||
| 8 | * The reta library is free software: you can redistribute it and/or modify | ||
| 9 | * it under the terms of the GNU Lesser General Public License as published | ||
| 10 | * by the Free Software Foundation, either version 3 of the License, or (at | ||
| 11 | * your option) any later version. | ||
| 12 | * | ||
| 13 | * The reta library is distributed in the hope that it will be useful, but | ||
| 14 | * WITHOUT ANY WARRANTY; without even the implied warranty of | ||
| 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser | ||
| 16 | * General Public License for more details. | ||
| 17 | * | ||
| 18 | * You should have received a copy of the GNU Lesser General Public License | ||
| 19 | * along with the reta library. If not, see <http://www.gnu.org/licenses/>. | ||
| 20 | * | ||
| 21 | ****************************************************************************/ | ||
| 22 | |||
| 23 | #include "reta/LowPassFilter.h" | ||
| 24 | |||
| 25 | #include "Base.h" | ||
| 26 | |||
| 27 | #include <cmath> // cos(), sin() | ||
| 28 | #include <sstream> | ||
| 29 | #include <stdexcept> | ||
| 30 | |||
| 31 | using std::function; | ||
| 32 | using std::invalid_argument; | ||
| 33 | using std::make_unique; | ||
| 34 | using std::shared_ptr; | ||
| 35 | using std::string; | ||
| 36 | using std::stringstream; | ||
| 37 | using std::vector; | ||
| 38 | |||
| 39 | using namespace Reta; | ||
| 40 | |||
| 41 | /****************************************************************************/ | ||
| 42 | |||
| 43 | 48 | struct RETA_LOCAL LowPassFilter::Impl : public Base | |
| 44 | { | ||
| 45 | // Direct Form I biquad section (RBJ audio cookbook). For order == 1, | ||
| 46 | // a1/b0 are set up as a plain single-pole filter and b1/b2/a2 stay 0, so | ||
| 47 | // the same process() reduces to a first-order filter without any | ||
| 48 | // special-casing in update(). | ||
| 49 | // | ||
| 50 | // Direct Form I is used on purpose: its state consists only of past | ||
| 51 | // inputs and outputs and is independent of the coefficients, so changing | ||
| 52 | // tau at runtime does not make the output jump (unlike the transposed | ||
| 53 | // Direct Form II, whose state is scaled by the coefficients). | ||
| 54 | 73 | struct Biquad | |
| 55 | { | ||
| 56 | double b0 {1.0}, b1 {0.0}, b2 {0.0}, a1 {0.0}, a2 {0.0}; | ||
| 57 | double x1 {0.0}, x2 {0.0}, y1 {0.0}, y2 {0.0}; | ||
| 58 | |||
| 59 | 324268 | double process(double in) | |
| 60 | { | ||
| 61 | 324268 | double out = b0 * in + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2; | |
| 62 | 324268 | x2 = x1; | |
| 63 | 324268 | x1 = in; | |
| 64 | 324268 | y2 = y1; | |
| 65 | 324268 | y1 = out; | |
| 66 | 324268 | return out; | |
| 67 | } | ||
| 68 | |||
| 69 | // Set the state to the steady state for a constant input (every | ||
| 70 | // section has unity DC gain), so that filtering continues smoothly | ||
| 71 | // from this value. | ||
| 72 | 1028 | void setSteadyState(double value) { x1 = x2 = y1 = y2 = value; } | |
| 73 | }; | ||
| 74 | |||
| 75 | Impl(shared_ptr<Task>, | ||
| 76 | const string &, | ||
| 77 | double, | ||
| 78 | unsigned int, | ||
| 79 | unsigned int); | ||
| 80 | |||
| 81 | void | ||
| 82 | update(const vector<double> &, | ||
| 83 | const vector<uint8_t> & = vector<uint8_t>()); | ||
| 84 | void updateCoefficients(unsigned int, double); | ||
| 85 | |||
| 86 | shared_ptr<Task> task; | ||
| 87 | |||
| 88 | // order == 1: single real pole, one section | ||
| 89 | // Even order >= 2: (order / 2) RBJ Butterworth biquad sections | ||
| 90 | unsigned int order; | ||
| 91 | |||
| 92 | vector<double> tau; | ||
| 93 | vector<double> output; | ||
| 94 | |||
| 95 | // Set whenever tau had to be clamped: for order == 1 to the task period, | ||
| 96 | // for order >= 2 to stay clear of the Nyquist frequency, see | ||
| 97 | // updateCoefficients(). | ||
| 98 | vector<uint8_t> tauLimited; | ||
| 99 | |||
| 100 | // tau the cascade coefficients were last computed for, per channel, so | ||
| 101 | // that the (comparatively expensive) trigonometry in | ||
| 102 | // updateCoefficients() only runs when tau actually changes. | ||
| 103 | vector<double> stageTau; | ||
| 104 | vector<vector<Biquad>> stages; | ||
| 105 | }; | ||
| 106 | |||
| 107 | /****************************************************************************/ | ||
| 108 | |||
| 109 | 50 | LowPassFilter::Impl::Impl( | |
| 110 | shared_ptr<Task> task, | ||
| 111 | const string &prefix, | ||
| 112 | double initTau, | ||
| 113 | unsigned int width, | ||
| 114 | 50 | unsigned int filterOrder) : | |
| 115 | Base {prefix}, | ||
| 116 | task {task}, | ||
| 117 | order {filterOrder}, | ||
| 118 | tau(width, initTau), | ||
| 119 | output(width, 0.0), | ||
| 120 | tauLimited(width, 0), | ||
| 121 | stageTau(width, 0.0), | ||
| 122 |
5/10✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 50 times.
✗ Branch 25 not taken.
|
52 | stages(width) |
| 123 | { | ||
| 124 |
2/2✓ Branch 1 taken 49 times.
✓ Branch 2 taken 1 times.
|
50 | checkZeroWidth(width); |
| 125 | |||
| 126 |
5/6✓ Branch 0 taken 48 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 10 times.
|
49 | if (order == 0 || (order > 1 && order % 2 != 0)) { |
| 127 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | stringstream str; |
| 128 | str << __PRETTY_FUNCTION__ << " : " << prefix | ||
| 129 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
1 | << " Invalid filter order " << order |
| 130 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | << " (must be 1, or an even number >= 2)!"; |
| 131 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | throw invalid_argument(str.str()); |
| 132 | } | ||
| 133 | |||
| 134 |
2/2✓ Branch 3 taken 71 times.
✓ Branch 4 taken 48 times.
|
119 | for (auto &s : stages) { |
| 135 |
3/4✓ Branch 0 taken 10 times.
✓ Branch 1 taken 61 times.
✓ Branch 3 taken 71 times.
✗ Branch 4 not taken.
|
71 | s.resize(order == 1 ? 1 : order / 2); |
| 136 | } | ||
| 137 | |||
| 138 |
1/2✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
|
48 | pdserv *pdserv {task->getPdServ()}; |
| 139 |
1/2✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
|
48 | pdtask *pdtask {task->getPdTask()}; |
| 140 | |||
| 141 |
1/2✓ Branch 3 taken 48 times.
✗ Branch 4 not taken.
|
96 | pdserv_parameter( |
| 142 |
1/2✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
|
96 | pdserv, (prefix + "/Tau").c_str(), 0666, pd_double_T, tau.data(), |
| 143 | tau.size(), NULL, NULL, NULL); | ||
| 144 |
1/2✓ Branch 3 taken 48 times.
✗ Branch 4 not taken.
|
96 | pdserv_signal( |
| 145 |
1/2✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
|
96 | pdtask, 1, (prefix + "/Output").c_str(), pd_double_T, |
| 146 | 48 | output.data(), output.size(), NULL); | |
| 147 |
1/2✓ Branch 3 taken 48 times.
✗ Branch 4 not taken.
|
96 | pdserv_signal( |
| 148 |
1/2✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
|
96 | pdtask, 1, (prefix + "/TauLimited").c_str(), pd_boolean_T, |
| 149 | 48 | tauLimited.data(), tauLimited.size(), NULL); | |
| 150 | 48 | } | |
| 151 | |||
| 152 | /****************************************************************************/ | ||
| 153 | |||
| 154 | 32 | void LowPassFilter::Impl::updateCoefficients(unsigned int i, double t) | |
| 155 | { | ||
| 156 | 32 | double period {task->getPeriod()}; | |
| 157 | |||
| 158 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 16 times.
|
32 | if (order == 1) { |
| 159 | // Discretized single-pole filter, equivalent to Euler-integrating | ||
| 160 | // dy/dt = (x - y) / tau: | ||
| 161 | // y[n] = y[n-1] + (period / tau) * (x[n] - y[n-1]). | ||
| 162 | // | ||
| 163 | // For tau < period, the gain exceeds 1 and the output overshoots | ||
| 164 | // (and even diverges for tau < period / 2), so tau is clamped to the | ||
| 165 | // period, where the output simply follows the input. | ||
| 166 | 16 | tauLimited.at(i) = t < period; | |
| 167 |
2/2✓ Branch 1 taken 15 times.
✓ Branch 2 taken 1 times.
|
16 | double k {tauLimited.at(i) ? 1.0 : period / t}; |
| 168 | |||
| 169 | 16 | Biquad &s {stages.at(i).at(0)}; | |
| 170 | 16 | s.b0 = k; | |
| 171 | 16 | s.b1 = 0.0; | |
| 172 | 16 | s.b2 = 0.0; | |
| 173 | 16 | s.a1 = k - 1.0; | |
| 174 | 16 | s.a2 = 0.0; | |
| 175 | |||
| 176 | 16 | stageTau.at(i) = t; | |
| 177 | 16 | return; | |
| 178 | } | ||
| 179 | |||
| 180 | 16 | double sampleRate {1.0 / period}; | |
| 181 | 16 | double nyquist {sampleRate / 2.0}; | |
| 182 | 16 | double cutoffHz {1.0 / (2.0 * M_PI * t)}; | |
| 183 | 16 | tauLimited.at(i) = cutoffHz > nyquist * 0.99; | |
| 184 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 15 times.
|
16 | if (tauLimited.at(i)) { |
| 185 | // stay clear of the Nyquist limit, where cos(w0) approaches -1 | ||
| 186 | 1 | cutoffHz = nyquist * 0.99; | |
| 187 | } | ||
| 188 | |||
| 189 | 16 | double w0 {2.0 * M_PI * cutoffHz / sampleRate}; | |
| 190 | 16 | double cosw0 {cos(w0)}; | |
| 191 | 16 | double sinw0 {sin(w0)}; | |
| 192 | |||
| 193 | 16 | unsigned int nSections {order / 2}; | |
| 194 |
2/2✓ Branch 0 taken 18 times.
✓ Branch 1 taken 16 times.
|
34 | for (unsigned int k = 1; k <= nSections; k++) { |
| 195 | // Per-section Q from the Butterworth pole angles, so that the | ||
| 196 | // cascade of these biquads reproduces the N-th order Butterworth | ||
| 197 | // response (maximally flat passband). | ||
| 198 | 18 | double theta {M_PI * (2.0 * k - 1.0) / (2.0 * order)}; | |
| 199 | 18 | double Q {1.0 / (2.0 * cos(theta))}; | |
| 200 | 18 | double alpha {sinw0 / (2.0 * Q)}; | |
| 201 | |||
| 202 | // Standard bilinear-transform lowpass biquad ("RBJ cookbook"). | ||
| 203 | 18 | double a0 {1.0 + alpha}; | |
| 204 | 18 | Biquad &s {stages.at(i).at(k - 1)}; | |
| 205 | 18 | s.b0 = ((1.0 - cosw0) / 2.0) / a0; | |
| 206 | 18 | s.b1 = (1.0 - cosw0) / a0; | |
| 207 | 18 | s.b2 = s.b0; | |
| 208 | 18 | s.a1 = (-2.0 * cosw0) / a0; | |
| 209 | 18 | s.a2 = (1.0 - alpha) / a0; | |
| 210 | } | ||
| 211 | |||
| 212 | 16 | stageTau.at(i) = t; | |
| 213 | } | ||
| 214 | |||
| 215 | /****************************************************************************/ | ||
| 216 | |||
| 217 | 323271 | void LowPassFilter::Impl::update( | |
| 218 | const vector<double> &input, | ||
| 219 | const vector<uint8_t> &reset) | ||
| 220 | { | ||
| 221 |
2/2✓ Branch 1 taken 325274 times.
✓ Branch 2 taken 323271 times.
|
648545 | for (unsigned int i = 0; i < output.size(); i++) { |
| 222 |
6/6✓ Branch 1 taken 325251 times.
✓ Branch 2 taken 23 times.
✓ Branch 4 taken 324248 times.
✓ Branch 5 taken 1003 times.
✓ Branch 6 taken 324248 times.
✓ Branch 7 taken 1026 times.
|
325274 | if (tau.at(i) > 0.0 && !reset.at(i)) { |
| 223 |
2/2✓ Branch 2 taken 32 times.
✓ Branch 3 taken 324216 times.
|
324248 | if (tau.at(i) != stageTau.at(i)) { |
| 224 | 32 | updateCoefficients(i, tau.at(i)); | |
| 225 | } | ||
| 226 | |||
| 227 | 324248 | double out {input.at(i)}; | |
| 228 |
3/4✓ Branch 1 taken 324248 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 324268 times.
✓ Branch 7 taken 324248 times.
|
648516 | for (auto &s : stages.at(i)) { |
| 229 | 324268 | out = s.process(out); | |
| 230 | } | ||
| 231 | 324248 | output.at(i) = out; | |
| 232 | } | ||
| 233 | else { | ||
| 234 | 1026 | output.at(i) = input.at(i); | |
| 235 | |||
| 236 | // Track the input with the filter memory, so that filtering | ||
| 237 | // continues smoothly from the current output once tau becomes | ||
| 238 | // positive again (or reset is released). | ||
| 239 |
3/4✓ Branch 1 taken 1026 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1028 times.
✓ Branch 7 taken 1026 times.
|
2054 | for (auto &s : stages.at(i)) { |
| 240 |
1/2✓ Branch 1 taken 1028 times.
✗ Branch 2 not taken.
|
1028 | s.setSteadyState(input.at(i)); |
| 241 | } | ||
| 242 | } | ||
| 243 | } | ||
| 244 | 323271 | } | |
| 245 | |||
| 246 | /****************************************************************************/ | ||
| 247 | |||
| 248 | 29 | LowPassFilter::LowPassFilter( | |
| 249 | shared_ptr<Task> task, | ||
| 250 | const std::string &prefix, | ||
| 251 | double initTau, | ||
| 252 | 29 | unsigned int width) : | |
| 253 |
2/2✓ Branch 2 taken 28 times.
✓ Branch 3 taken 1 times.
|
30 | LowPassFilter {task, prefix, initTau, width, 1} |
| 254 | 28 | {} | |
| 255 | |||
| 256 | /****************************************************************************/ | ||
| 257 | |||
| 258 | 50 | LowPassFilter::LowPassFilter( | |
| 259 | shared_ptr<Task> task, | ||
| 260 | const std::string &prefix, | ||
| 261 | double initTau, | ||
| 262 | unsigned int width, | ||
| 263 | 50 | unsigned int order) : | |
| 264 | 50 | impl {make_unique<Impl>(task, prefix, initTau, width, order)} | |
| 265 | 48 | {} | |
| 266 | |||
| 267 | /****************************************************************************/ | ||
| 268 | |||
| 269 | 48 | LowPassFilter::~LowPassFilter() | |
| 270 | 48 | {} | |
| 271 | |||
| 272 | /****************************************************************************/ | ||
| 273 | |||
| 274 | 15 | void LowPassFilter::setTau(double newTau, unsigned int i) | |
| 275 | { | ||
| 276 | 15 | impl->tau.at(i) = newTau; | |
| 277 | 15 | } | |
| 278 | |||
| 279 | /****************************************************************************/ | ||
| 280 | |||
| 281 | 322259 | void LowPassFilter::update(double input, bool reset) | |
| 282 | { | ||
| 283 |
3/6✓ Branch 3 taken 322259 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 322259 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 322259 times.
✗ Branch 11 not taken.
|
322259 | impl->update(vector<double> {input}, vector<uint8_t> {reset}); |
| 284 | 322259 | } | |
| 285 | |||
| 286 | /****************************************************************************/ | ||
| 287 | |||
| 288 | 12 | void LowPassFilter::update( | |
| 289 | const vector<double> &input, | ||
| 290 | const vector<uint8_t> &reset) | ||
| 291 | { | ||
| 292 | 12 | impl->update(input, reset); | |
| 293 | 12 | } | |
| 294 | |||
| 295 | /****************************************************************************/ | ||
| 296 | |||
| 297 | 1000 | void LowPassFilter::update( | |
| 298 | function<double(unsigned int)> input, | ||
| 299 | function<bool(unsigned int)> reset) | ||
| 300 | { | ||
| 301 |
1/2✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
2000 | vector<double> inputVector(impl->output.size()); |
| 302 |
1/2✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
2000 | vector<uint8_t> resetVector(impl->output.size()); |
| 303 | |||
| 304 |
2/2✓ Branch 2 taken 3000 times.
✓ Branch 3 taken 1000 times.
|
4000 | for (unsigned int i = 0; i < impl->output.size(); i++) { |
| 305 |
2/4✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3000 times.
✗ Branch 5 not taken.
|
3000 | inputVector.at(i) = input(i); |
| 306 |
2/4✓ Branch 1 taken 3000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3000 times.
✗ Branch 5 not taken.
|
3000 | resetVector.at(i) = reset(i); |
| 307 | } | ||
| 308 | |||
| 309 |
1/2✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
|
1000 | impl->update(inputVector, resetVector); |
| 310 | 1000 | } | |
| 311 | |||
| 312 | /****************************************************************************/ | ||
| 313 | |||
| 314 | 316179 | double LowPassFilter::getOutput(unsigned int i) const | |
| 315 | { | ||
| 316 | 316179 | return impl->output.at(i); | |
| 317 | } | ||
| 318 | |||
| 319 | /****************************************************************************/ | ||
| 320 | |||
| 321 | 9 | bool LowPassFilter::isTauLimited(unsigned int i) const | |
| 322 | { | ||
| 323 | 9 | return impl->tauLimited.at(i); | |
| 324 | } | ||
| 325 | |||
| 326 | /****************************************************************************/ | ||
| 327 |