|
1 | | -"""Madgwick AHRS recurrence block — numpy behavior + C++ roundtrip. |
| 1 | +"""Madgwick AHRS recurrence block — numpy behavior + codegen smoke. |
2 | 2 |
|
3 | | -Madgwick adds zero backend code (it reuses the generic `lower_recurrence` |
4 | | -path PID established). These tests pin the convention (gyro integration, |
5 | | -accelerometer convergence, quaternion stays unit) and prove the compiled |
6 | | -C++ `step()` reproduces the numpy quaternion trajectory exactly. |
| 3 | +Madgwick adds zero backend code: it reuses the generic `lower_recurrence` |
| 4 | +path PID establishes and Mahony already roundtrips through compiled C++ |
| 5 | +(quaternion output included). So there's no separate Madgwick C++ roundtrip |
| 6 | +— these tests pin the convention (gyro integration, accelerometer |
| 7 | +convergence, quaternion stays unit) and that the C++ target emits its files. |
7 | 8 | """ |
8 | 9 |
|
9 | | -import shutil |
10 | | -import subprocess |
11 | 10 | from pathlib import Path |
12 | 11 |
|
13 | 12 | import numpy as np |
@@ -61,83 +60,11 @@ def test_madgwick_quaternion_stays_unit(): |
61 | 60 | assert np.linalg.norm(q) == pytest.approx(1.0, abs=1e-9) |
62 | 61 |
|
63 | 62 |
|
64 | | -# --- C++ roundtrip ---------------------------------------------------------- |
65 | | - |
66 | | -_GYRO = [[0.1, -0.2, 0.05], [0.3, 0.1, -0.1], [-0.2, 0.2, 0.15], |
67 | | - [0.0, -0.3, 0.2], [0.25, 0.1, -0.05]] |
68 | | -_ACCEL = [[0.0, 0.0, 9.81], [0.5, 0.0, 9.7], [0.3, -0.4, 9.6], |
69 | | - [-0.2, 0.3, 9.75], [0.1, 0.1, 9.8]] |
70 | | -_DT = 0.02 |
71 | | - |
72 | | -_HARNESS = r""" |
73 | | -#include "madgwick.hpp" |
74 | | -#include <cstdio> |
75 | | -
|
76 | | -int main() { |
77 | | - manta_gen::Madgwick f; |
78 | | - const double g[5][3] = {{0.1,-0.2,0.05},{0.3,0.1,-0.1},{-0.2,0.2,0.15}, |
79 | | - {0.0,-0.3,0.2},{0.25,0.1,-0.05}}; |
80 | | - const double a[5][3] = {{0.0,0.0,9.81},{0.5,0.0,9.7},{0.3,-0.4,9.6}, |
81 | | - {-0.2,0.3,9.75},{0.1,0.1,9.8}}; |
82 | | - for (int i = 0; i < 5; ++i) { |
83 | | - manta_gen::Madgwick::Inputs u; |
84 | | - u.gyro << g[i][0], g[i][1], g[i][2]; |
85 | | - u.accel << a[i][0], a[i][1], a[i][2]; |
86 | | - auto o = f.step(u, 0.02, 0.0); |
87 | | - std::printf("q %.17g %.17g %.17g %.17g\n", |
88 | | - o.orientation[0], o.orientation[1], |
89 | | - o.orientation[2], o.orientation[3]); |
90 | | - } |
91 | | - return 0; |
92 | | -} |
93 | | -""" |
| 63 | +# --- codegen smoke ---------------------------------------------------------- |
94 | 64 |
|
95 | 65 |
|
96 | 66 | def test_madgwick_emits_cpp_files(tmp_path: Path): |
97 | 67 | result = TargetCpp(Madgwick(beta=0.1), tmp_path, class_name="Madgwick") |
98 | 68 | for p in (result.kernels_c, result.kernels_h, result.wrapper_hpp, |
99 | 69 | result.wrapper_cpp, result.cmakelists): |
100 | 70 | assert p.exists(), p |
101 | | - |
102 | | - |
103 | | -def test_madgwick_python_cpp_roundtrip(tmp_path: Path): |
104 | | - cxx = next((c for c in ("c++", "g++", "clang++") if shutil.which(c)), None) |
105 | | - cc = next((c for c in ("cc", "gcc", "clang") if shutil.which(c)), None) |
106 | | - if cxx is None or cc is None: |
107 | | - pytest.skip("no C/C++ compiler on PATH") |
108 | | - eigen_inc = next((p for p in ("/usr/include/eigen3", |
109 | | - "/usr/local/include/eigen3") |
110 | | - if Path(p, "Eigen", "Dense").exists()), None) |
111 | | - if eigen_inc is None: |
112 | | - pytest.skip("Eigen headers not found") |
113 | | - |
114 | | - f = Madgwick(beta=0.1) |
115 | | - result = TargetCpp(f, tmp_path, class_name="Madgwick") |
116 | | - |
117 | | - k_obj, w_obj = tmp_path / "k.o", tmp_path / "w.o" |
118 | | - for cmd in ( |
119 | | - [cc, "-c", "-O2", "-fPIC", str(result.kernels_c), "-o", str(k_obj)], |
120 | | - [cxx, "-c", "-std=c++17", "-O2", "-fPIC", f"-I{eigen_inc}", |
121 | | - f"-I{tmp_path}", str(result.wrapper_cpp), "-o", str(w_obj)], |
122 | | - ): |
123 | | - p = subprocess.run(cmd, capture_output=True, text=True) |
124 | | - assert p.returncode == 0, p.stderr |
125 | | - |
126 | | - h_src = tmp_path / "harness_main.cpp" |
127 | | - h_src.write_text(_HARNESS) |
128 | | - binary = tmp_path / "harness" |
129 | | - p = subprocess.run( |
130 | | - [cxx, "-std=c++17", "-O2", f"-I{eigen_inc}", f"-I{tmp_path}", |
131 | | - str(h_src), str(w_obj), str(k_obj), "-o", str(binary)], |
132 | | - capture_output=True, text=True) |
133 | | - assert p.returncode == 0, p.stderr |
134 | | - p = subprocess.run([str(binary)], capture_output=True, text=True) |
135 | | - assert p.returncode == 0, p.stderr |
136 | | - cpp_q = [[float(x) for x in line.split()[1:]] |
137 | | - for line in p.stdout.strip().splitlines()] |
138 | | - |
139 | | - r = TargetNumpy(f) |
140 | | - np_q = [list(r.step(_DT, gyro=g, accel=a)["orientation"]) |
141 | | - for g, a in zip(_GYRO, _ACCEL)] |
142 | | - |
143 | | - np.testing.assert_allclose(cpp_q, np_q, atol=1e-12) |
0 commit comments