read out geometry lighthouse file

bitcraze · Apr 2, 2024 · cdd8445 · cdd8445
1 parent 86bac18
commit cdd8445
Show file tree

Hide file tree

Showing 4 changed files with 62 additions and 1 deletion.
diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i
@@ -28,6 +28,9 @@
 #include "outlierFilterLighthouse.h"
 #include "mm_yaw_error.h"
 #include "lighthouse_types.h"
+#include "lighthouse_geometry.h"
+#include "ootx_decoder.h"
+#include "crc32.h"
 %}
 
 %include "math3d.h"
@@ -48,9 +51,36 @@
 %include "outlierFilterLighthouse.h"
 %include "mm_yaw_error.h"
 %include "lighthouse_types.h"
+%include "lighthouse_geometry.h"
+%include "ootx_decoder.h"
+%include "crc32.h"
+
+
 
 %inline %{
 
+void set_origin_mat(baseStationGeometry_t *geo, struct vec3_s *origin, struct vec3_s *mat1, struct vec3_s *mat2, struct vec3_s *mat3)
+{
+    geo->origin[0] = origin->x;
+    geo->origin[1] = origin->y;
+    geo->origin[2] = origin->z;
+    geo->mat[0][0] = mat1->x;
+    geo->mat[0][1] = mat1->y;
+    geo->mat[0][2] = mat1->z;
+    geo->mat[1][0] = mat2->x;
+    geo->mat[1][1] = mat2->y;
+    geo->mat[1][2] = mat2->z;
+    geo->mat[2][0] = mat3->x;
+    geo->mat[2][1] = mat3->y;
+    geo->mat[2][2] = mat3->z;
+
+    //print origin and mat
+    printf("origin: %f, %f, %f\n", geo->origin[0], geo->origin[1], geo->origin[2]);
+    printf("mat: %f, %f, %f\n", geo->mat[0][0], geo->mat[0][1], geo->mat[0][2]);
+    printf("mat: %f, %f, %f\n", geo->mat[1][0], geo->mat[1][1], geo->mat[1][2]);
+    printf("mat: %f, %f, %f\n", geo->mat[2][0], geo->mat[2][1], geo->mat[2][2]);
+}
+
 void set_sweep(lighthouseCalibration_t *calib, lighthouseCalibrationSweep_t sweep, int i)
 {
     calib->sweep[i] = sweep;

diff --git a/bindings/setup.py b/bindings/setup.py
@@ -53,6 +53,10 @@
     "src/modules/src/kalman_core/mm_sweep_angles.c",
     "src/modules/src/outlierfilter/outlierFilterLighthouse.c",
     "src/modules/src/kalman_core/mm_yaw_error.c",
+    "src/utils/src/lighthouse/lighthouse_calibration.c",
+    "src/utils/src/lighthouse/ootx_decoder.c",
+    "src/utils/src/lighthouse/lighthouse_geometry.c",
+    "src/utils/src/crc32.c",
 ]
 
 cffirmware = Extension(

diff --git a/bindings/util/lighthouse_utils.py b/bindings/util/lighthouse_utils.py
@@ -38,7 +38,34 @@ def read_lh_basestation_pose_calibration(file_name: str) -> dict[int, cffirmware
 
             lhCalibration.uid = vals['uid']
 
+            results_calib[id] = lhCalibration
+
             cffirmware.print_sweeps(lhCalibration)
 
+        print(results_calib)
+
+        data_geo = data['geos']
+        results_geo = {}
+        for id, vals in data_geo.items():
+            basestation_geo = cffirmware.baseStationGeometry_t()
+            origin = cffirmware.vec3_s()
+            origin.x = vals['origin'][0]
+            origin.y = vals['origin'][1]
+            origin.z = vals['origin'][2]
+            mat1 = cffirmware.vec3_s()
+            mat1.x = vals['rotation'][0][0]
+            mat1.y = vals['rotation'][0][1]
+            mat1.z = vals['rotation'][0][2]
+            mat2 = cffirmware.vec3_s()
+            mat2.x = vals['rotation'][1][0]
+            mat2.y = vals['rotation'][1][1]
+            mat2.z = vals['rotation'][1][2]
+            mat3 = cffirmware.vec3_s()
+            mat3.x = vals['rotation'][2][0]
+            mat3.y = vals['rotation'][2][1]
+            mat3.z = vals['rotation'][2][2]
+
+            cffirmware.set_origin_mat(basestation_geo, origin, mat1, mat2, mat3)
+
 
     return result
diff --git a/test_python/test_kalman_core.py b/test_python/test_kalman_core.py
@@ -4,7 +4,7 @@
 from bindings.util.estimator_kalman_emulator import EstimatorKalmanEmulator
 from bindings.util.sd_card_file_runner import SdCardFileRunner
 from bindings.util.loco_utils import read_loco_anchor_positions
-from bindings.util.lighthouse_utils import read_lh_basestation_positions_calibration
+from bindings.util.lighthouse_utils import read_lh_basestation_pose_calibration
 
 def test_kalman_core_with_tdoa3():
     # Fixture