Correlator seems to be working

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()