Lighthouse pythonbindings

Makefile

@@ -210,7 +210,7 @@ MOD_INC = src/modules/interface
 MOD_SRC = src/modules/src
 
 bindings_python build/cffirmware.py: bindings/setup.py $(MOD_SRC)/*.c
-	swig -python -I$(MOD_INC) -Isrc/hal/interface -Isrc/utils/interface -I$(MOD_INC)/controller -Isrc/platform/interface -I$(MOD_INC)/outlierfilter -I$(MOD_INC)/kalman_core -o build/cffirmware_wrap.c bindings/cffirmware.i
+	swig -python -I$(MOD_INC) -Isrc/hal/interface -Isrc/utils/interface -Isrc/utils/interface/lighthouse -I$(MOD_INC)/controller -Isrc/platform/interface -I$(MOD_INC)/outlierfilter -I$(MOD_INC)/kalman_core -o build/cffirmware_wrap.c bindings/cffirmware.i
 	$(PYTHON) bindings/setup.py build_ext --inplace
 	cp cffirmware_setup.py build/setup.py
 

bindings/cffirmware.i

@@ -24,6 +24,14 @@
 #include "outlierFilterTdoa.h"
 #include "kalman_core.h"
 #include "mm_tdoa.h"
+#include "mm_sweep_angles.h"
+#include "outlierFilterLighthouse.h"
+#include "mm_yaw_error.h"
+#include "lighthouse_types.h"
+#include "lighthouse_geometry.h"
+#include "ootx_decoder.h"
+#include "crc32.h"
+#include "lighthouse_calibration.h"
 %}
 
 %include "math3d.h"
@@ -40,9 +48,71 @@
 %include "outlierFilterTdoa.h"
 %include "kalman_core.h"
 %include "mm_tdoa.h"
+%include "mm_sweep_angles.h"
+%include "outlierFilterLighthouse.h"
+%include "mm_yaw_error.h"
+%include "lighthouse_types.h"
+%include "lighthouse_geometry.h"
+%include "ootx_decoder.h"
+%include "crc32.h"
+%include "lighthouse_calibration.h"
+
 
 
 %inline %{
+
+float get_state(kalmanCoreData_t *data, int i)
+{
+    return data->S[i];
+}
+
+float get_mat_index(kalmanCoreData_t *data, int i, int j)
+{
+    return data->R[i][j];
+}
+
+float set_calibration_model(sweepAngleMeasurement_t *sweep, lighthouseCalibrationSweep_t *calib_in)
+{
+    sweep->calibrationMeasurementModel = lighthouseCalibrationMeasurementModelLh2;
+    sweep->calib = calib_in;
+
+}
+
+
+void print_sweep_angle(sweepAngleMeasurement_t *sweep)
+{
+    //print all variables of struct
+    printf("uint32_t timestamp: %d\n", sweep->timestamp);
+    printf("sensorPos: %f, %f, %f\n", sweep->sensorPos.x, sweep->sensorPos.y, sweep->sensorPos.z);
+    printf('rotorPos: %f, %f, %f\n', sweep->rotorPos.x, sweep->rotorPos.y, sweep->rotorPos.z);
+    printf('rotorRot: %f, %f, %f\n', sweep->rotorRot.i11, sweep->rotorRot.i12, sweep->rotorRot.i13);
+    printf('rotorRot: %f, %f, %f\n', sweep->rotorRot.i21, sweep->rotorRot.i22, sweep->rotorRot.i23);
+    printf('rotorRot: %f, %f, %f\n', sweep->rotorRot.i31, sweep->rotorRot.i32, sweep->rotorRot.i33);
+    printf('rotorRotInv: %f, %f, %f\n', sweep->rotorRotInv.i11, sweep->rotorRotInv.i12, sweep->rotorRotInv.i13);
+    printf('rotorRotInv: %f, %f, %f\n', sweep->rotorRotInv.i21, sweep->rotorRotInv.i22, sweep->rotorRotInv.i23);
+    printf('rotorRotInv: %f, %f, %f\n', sweep->rotorRotInv.i31, sweep->rotorRotInv.i32, sweep->rotorRotInv.i33);
+    printf("sensorID: %d\n", sweep->sensorId);
+    printf("baseStationID: %d\n", sweep->baseStationId);
+    printf("sweepID: %d\n", sweep->sweepId);
+    printf("t: %f\n", sweep->t);
+    printf("measuredSweepAngle: %f\n", sweep->measuredSweepAngle);
+    printf("stdDev: %f\n", sweep->stdDev);
+    printf("calib: %f, %f, %f, %f, %f\n", sweep->calib->phase, sweep->calib->tilt, sweep->calib->curve, sweep->calib->gibmag, sweep->calib->gibphase);
+    printf("calibrationMeasurementModel: %f\n", sweep->calibrationMeasurementModel);
+}
+
+
+
+
+void set_sweep(lighthouseCalibration_t *calib, lighthouseCalibrationSweep_t sweep, int i)
+{
+    calib->sweep[i] = sweep;
+}
+void print_sweeps(lighthouseCalibration_t calib)
+{
+    printf("Sweep 0: phase: %f, tilt: %f, curve: %f, gibmag: %f, gibphase: %f\n", calib.sweep[0].phase, calib.sweep[0].tilt, calib.sweep[0].curve, calib.sweep[0].gibmag, calib.sweep[0].gibphase);
+    printf("Sweep 1: phase: %f, tilt: %f, curve: %f, gibmag: %f, gibphase: %f\n", calib.sweep[1].phase, calib.sweep[1].tilt, calib.sweep[1].curve, calib.sweep[1].gibmag, calib.sweep[1].gibphase);
+}
 struct poly4d* piecewise_get(struct piecewise_traj *pp, int i)
 {
     return &pp->pieces[i];

bindings/setup.py

@@ -50,6 +50,13 @@
     "src/modules/src/kalman_core/kalman_core.c",
     "src/modules/src/kalman_core/mm_tdoa.c",
     "src/modules/src/outlierfilter/outlierFilterTdoa.c",
+    "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(

bindings/util/estimator_kalman_emulator.py

@@ -1,10 +1,7 @@
+from __future__ import annotations
 import math
 import cffirmware
 
-
-
-
-
 class EstimatorKalmanEmulator:
     """
     This class emulates the behavior of estimator_kalman.c and is used as a helper to enable testing of the kalman
@@ -18,14 +15,18 @@ class EstimatorKalmanEmulator:
     how they are connected.
 
     """
-    def __init__(self, anchor_positions) -> None:
+    def __init__(self, anchor_positions=None, basestation_poses=None, basestation_calibration=None) -> None:
         self.anchor_positions = anchor_positions
+        self.basestation_poses = basestation_poses
+        self.basestation_calibration = basestation_calibration
         self.accSubSampler = cffirmware.Axis3fSubSampler_t()
         self.gyroSubSampler = cffirmware.Axis3fSubSampler_t()
         self.coreData = cffirmware.kalmanCoreData_t()
-        self.outlierFilterState = cffirmware.OutlierFilterTdoaState_t()
+        self.outlierFilterStateTdoa = cffirmware.OutlierFilterTdoaState_t()
+        self.outlierFilterStateLH = cffirmware.OutlierFilterLhState_t()
 
         self.TDOA_ENGINE_MEASUREMENT_NOISE_STD = 0.30
+        self.LH_ENGINE_MEASUREMENT_NOISE_STD = 0.001
         self.PREDICT_RATE = 100
         self.PREDICT_STEP_MS = 1000 / self.PREDICT_RATE
 
@@ -86,7 +87,8 @@ def _lazy_init(self, sample_time_ms):
 
         self.coreParams = cffirmware.kalmanCoreParams_t()
         cffirmware.kalmanCoreDefaultParams(self.coreParams)
-        cffirmware.outlierFilterTdoaReset(self.outlierFilterState)
+        cffirmware.outlierFilterTdoaReset(self.outlierFilterStateTdoa)
+        cffirmware.outlierFilterLighthouseReset(self.outlierFilterStateLH, self.now_ms)
         cffirmware.kalmanCoreInit(self.coreData, self.coreParams, self.now_ms)
 
         self._is_initialized = True
@@ -103,6 +105,17 @@ def _update_queued_measurements(self, now_ms: int, sensor_samples):
                 return
 
     def _update_with_sample(self, sample, now_ms):
+        position = [0.0, 0.0, 0.0]
+        position[0] = cffirmware.get_state(self.coreData, 0)
+        position[1] = cffirmware.get_state(self.coreData, 1)
+        position[2] = cffirmware.get_state(self.coreData, 2)
+
+        rotation_matrix = [[0.0, 0.0, 0.0],[0.0, 0.0, 0.0],[0.0, 0.0, 0.0]]
+
+        for i in range(0, 3):
+            for j in range(0, 3):
+                rotation_matrix[i][j] = cffirmware.get_mat_index(self.coreData, i,j)
+
         if sample[0] == 'estTDOA':
             tdoa_data = sample[1]
             tdoa = cffirmware.tdoaMeasurement_t()
@@ -114,7 +127,81 @@ def _update_with_sample(self, sample, now_ms):
             tdoa.distanceDiff = float(tdoa_data['distanceDiff'])
             tdoa.stdDev = self.TDOA_ENGINE_MEASUREMENT_NOISE_STD
 
-            cffirmware.kalmanCoreUpdateWithTdoa(self.coreData, tdoa, now_ms, self.outlierFilterState)
+            cffirmware.kalmanCoreUpdateWithTdoa(self.coreData, tdoa, now_ms, self.outlierFilterStateTdoa)
+
+        if sample[0] == 'estYawError':
+            yaw_error_data  = sample[1]
+            yaw_error = cffirmware.yawErrorMeasurement_t()
+            yaw_error.yawError = float(yaw_error_data['yawError'])
+            yaw_error.stdDev = 0.01
+
+            cffirmware.kalmanCoreUpdateWithYawError(self.coreData, yaw_error)
+
+
+        if sample[0] == 'estSweepAngle':
+            sweep_data = sample[1]
+            sweep = cffirmware.sweepAngleMeasurement_t()
+
+            sweep.sensorId = int(sweep_data['sensorId'])
+            sweep.baseStationId = int(sweep_data['baseStationId'])
+            sweep.sweepId = int(sweep_data['sweepId'])
+            sweep.t = float(sweep_data['t'])
+            sweep.measuredSweepAngle = float(sweep_data['sweepAngle'])
+            sweep.stdDev = self.LH_ENGINE_MEASUREMENT_NOISE_STD
+            cffirmware.set_calibration_model(sweep, self.basestation_calibration[sweep.baseStationId][sweep.sweepId])
+
+            sensor_pos_w = 0.015/2.0
+            sensor_pos_l = 0.030/2.0
+            sensor_position = {}
+            sensor_position[0] = [-sensor_pos_w, sensor_pos_l, 0.0]
+            sensor_position[1] = [-sensor_pos_w, -sensor_pos_l, 0.0]
+            sensor_position[2] = [sensor_pos_w, sensor_pos_l, 0.0]
+            sensor_position[3] = [sensor_pos_w, -sensor_pos_l, 0.0]
+
+            sensorPos = cffirmware.vec3_s()
+            sensorPos.x = sensor_position[int(sweep.sensorId)][0]
+            sensorPos.y = sensor_position[int(sweep.sensorId)][1]
+            sensorPos.z = sensor_position[int(sweep.sensorId)][2]
+
+            rotorPos = cffirmware.vec3_s()
+            rotorPos.x = self.basestation_poses[sweep.baseStationId]['origin'].x
+            rotorPos.y = self.basestation_poses[sweep.baseStationId]['origin'].y
+            rotorPos.z = self.basestation_poses[sweep.baseStationId]['origin'].z
+
+            rotorRot = cffirmware.mat3_s()
+            rotorRot.i11 = self.basestation_poses[sweep.baseStationId]['mat'].i11
+            rotorRot.i12 = self.basestation_poses[sweep.baseStationId]['mat'].i12
+            rotorRot.i13 = self.basestation_poses[sweep.baseStationId]['mat'].i13
+            rotorRot.i21 = self.basestation_poses[sweep.baseStationId]['mat'].i21
+            rotorRot.i22 = self.basestation_poses[sweep.baseStationId]['mat'].i22
+            rotorRot.i23 = self.basestation_poses[sweep.baseStationId]['mat'].i23
+            rotorRot.i31 = self.basestation_poses[sweep.baseStationId]['mat'].i31
+            rotorRot.i32 = self.basestation_poses[sweep.baseStationId]['mat'].i32
+            rotorRot.i33 = self.basestation_poses[sweep.baseStationId]['mat'].i33
+
+            # transpose of the rotation matrix
+            rotorRotInv = cffirmware.mat3_s()
+            rotorRotInv.i11 = self.basestation_poses[sweep.baseStationId]['mat'].i11
+            rotorRotInv.i12 = self.basestation_poses[sweep.baseStationId]['mat'].i21
+            rotorRotInv.i13 = self.basestation_poses[sweep.baseStationId]['mat'].i31
+            rotorRotInv.i21 = self.basestation_poses[sweep.baseStationId]['mat'].i12
+            rotorRotInv.i22 = self.basestation_poses[sweep.baseStationId]['mat'].i22
+            rotorRotInv.i23 = self.basestation_poses[sweep.baseStationId]['mat'].i32
+            rotorRotInv.i31 = self.basestation_poses[sweep.baseStationId]['mat'].i13
+            rotorRotInv.i32 = self.basestation_poses[sweep.baseStationId]['mat'].i23
+            rotorRotInv.i33 = self.basestation_poses[sweep.baseStationId]['mat'].i33
+
+
+
+            sweep.sensorPos = sensorPos
+            sweep.rotorPos = rotorPos
+            sweep.rotorRot = rotorRot
+            sweep.rotorRotInv = rotorRotInv
+
+            cffirmware.kalmanCoreUpdateWithSweepAngles(self.coreData, sweep, now_ms, self.outlierFilterStateLH)
+
+
+
 
         if sample[0] == 'estAcceleration':
             acc_data = sample[1]

bindings/util/lighthouse_utils.py

@@ -0,0 +1,68 @@
+from __future__ import annotations
+import cffirmware
+import yaml
+import ctypes
+
+
+def read_lh_basestation_pose_calibration(file_name: str) -> dict[int, cffirmware.vec3_s]:
+    """
+    Read basestation calibration and position data from a file exported from the client.
+
+    Args:
+        file_name (str): The name of the file
+
+    Returns:
+        dict[int, cffirmware.vec3_s]: A dictionary from anchor id to a 3D-vector
+    """
+    result = {}
+
+    with open(file_name, 'r') as file:
+        data = yaml.safe_load(file)
+        data_calib = data['calibs']
+        results_calib = {}
+        for id, vals in data_calib.items():
+
+
+            lhCalibration = {}
+
+            for i in range(0, 2):
+                data_calib_sweep = vals['sweeps'][i]
+                lhSweep = cffirmware.lighthouseCalibrationSweep_t()
+                lhSweep.phase = data_calib_sweep['phase']
+                lhSweep.tilt = data_calib_sweep['tilt']
+                lhSweep.curve = data_calib_sweep['curve']
+                lhSweep.gibmag = data_calib_sweep['gibmag']
+                lhSweep.gibphase = data_calib_sweep['gibphase']
+                lhSweep.ogeemag = data_calib_sweep['ogeemag']
+                lhSweep.ogeephase = data_calib_sweep['ogeephase']
+                lhCalibration[i] = lhSweep
+
+            results_calib[id] = lhCalibration
+
+
+        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]
+            mat = cffirmware.mat3_s()
+            mat.i11 = vals['rotation'][0][0]
+            mat.i12 = vals['rotation'][0][1]
+            mat.z13 = vals['rotation'][0][2]
+            mat.i21 = vals['rotation'][1][0]
+            mat.i22 = vals['rotation'][1][1]
+            mat.i23 = vals['rotation'][1][2]
+            mat.i31 = vals['rotation'][2][0]
+            mat.i32 = vals['rotation'][2][1]
+            mat.i33 = vals['rotation'][2][2]
+
+            #cffirmware.set_origin_mat(basestation_geo, origin, mat1, mat2, mat3)
+
+
+            results_geo[id] = {'origin': origin, 'mat': mat}
+
+
+    return results_calib, results_geo

bindings/util/loco_utils.py

@@ -1,3 +1,4 @@
+from __future__ import annotations
 import cffirmware
 import yaml
 

src/modules/interface/stabilizer_types.h

@@ -61,6 +61,14 @@ struct vec3_s {
   float z;
 };
 
+struct mat3_s {
+  uint32_t timestamp; // Timestamp when the data was computed
+
+  float i11, i12, i13;
+  float i21, i22, i23;
+  float i31, i32, i33;
+};
+
 typedef struct vec3_s vector_t;
 typedef struct vec3_s point_t;
 typedef struct vec3_s velocity_t;
@@ -323,10 +331,10 @@ typedef struct {
 /** Sweep angle measurement */
 typedef struct {
   uint32_t timestamp;
-  const vec3d* sensorPos;    // Sensor position in the CF reference frame
-  const vec3d* rotorPos;     // Pos of rotor origin in global reference frame
-  const mat3d* rotorRot;     // Rotor rotation matrix
-  const mat3d* rotorRotInv;  // Inverted rotor rotation matrix
+  struct vec3_s sensorPos;    // Sensor position in the CF reference frame
+  struct vec3_s rotorPos;     // Pos of rotor origin in global reference frame
+  struct mat3_s rotorRot;     // Rotor rotation matrix
+  struct mat3_s rotorRotInv;  // Inverted rotor rotation matrix
   uint8_t sensorId;
   uint8_t baseStationId;
   uint8_t sweepId;