summaryrefslogtreecommitdiff
path: root/controller/fw/tools
diff options
context:
space:
mode:
Diffstat (limited to 'controller/fw/tools')
-rw-r--r--controller/fw/tools/butter_filter_gen.py20
1 files changed, 17 insertions, 3 deletions
diff --git a/controller/fw/tools/butter_filter_gen.py b/controller/fw/tools/butter_filter_gen.py
index 059dea0..c77a6ec 100644
--- a/controller/fw/tools/butter_filter_gen.py
+++ b/controller/fw/tools/butter_filter_gen.py
@@ -1,9 +1,11 @@
#!/usr/bin/env python3
+import math
import sys
import contextlib
import scipy.signal as sig
+import numpy as np
@contextlib.contextmanager
@@ -38,13 +40,25 @@ if __name__ == '__main__':
print(f'#define {args.macro_name.upper()}_ORDER {args.n}')
print(f'#define {args.macro_name.upper()}_CLEN {(args.n+1)//2}')
print(f'#define {args.macro_name.upper()}_COEFF ', end='')
- for sec in sos:
+
+ # scipy.signal.butter by default returns extremely small bs for the first biquad and large ones for subsequent
+ # sections. Balance magnitudes to reduce possible rounding errors.
+ first_biquad_bs = sos[0][:3]
+ approx_mag = round(math.log10(np.mean(first_biquad_bs)))
+ mags = [approx_mag // len(sos)] * len(sos)
+ mags[0] += approx_mag - sum(mags)
+ sos[0][:3] /= 10**approx_mag
+
+ for mag, sec in zip(mags, sos):
+ bs, ases = sec[:3], sec[4:6]
+ bs *= 10**mag
+
with wrap():
print('.b=', end='')
with wrap():
- print(', '.join(f'{v}' for v in sec[:3]), end='')
+ print(', '.join(f'{v}' for v in bs), end='')
print(', .a=', end='')
with wrap():
- print(', '.join(f'{v}' for v in sec[4:6]), end='')
+ print(', '.join(f'{v}' for v in ases), end='')
print(', ', end='')
print()