#641 Refactoring: removed dependency between kalman core and pulse pr…

…ocessor

src/modules/interface/stabilizer_types.h

@@ -255,8 +255,8 @@ typedef struct {
   float t;                   // t is the tilt angle of the light plane on the rotor
   float measuredSweepAngle;
   float stdDev;
-  int baseStationType;       // Cast to lighthouseBaseStationType_t enum.
   const lighthouseCalibrationSweep_t* calib;
+  lighthouseCalibrationMeasurementModel_t calibrationMeasurementModel;
 } sweepAngleMeasurement_t;
 
 // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number.

src/modules/src/kalman_core.c

@@ -68,7 +68,6 @@
 #include "static_mem.h"
 
 #include "lighthouse_calibration.h"
-#include "pulse_processor.h"
 
 // #define DEBUG_STATE_CHECK
 
@@ -598,12 +597,7 @@ void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasureme
   const float r2 = x * x + y * y;
   const float r = arm_sqrt(r2);
 
-  float predictedSweepAngle = 0.0f;
-  if (sweepInfo->baseStationType == lighthouseBsTypeV1) {
-    predictedSweepAngle = lighthouseCalibrationMeasurementModelLh1(x, y, z, sweepInfo->calib);
-  } else {
-    predictedSweepAngle = lighthouseCalibrationMeasurementModelLh2(x, y, z, t, sweepInfo->calib);
-  }
+  const float predictedSweepAngle = sweepInfo->calibrationMeasurementModel(x, y, z, t, sweepInfo->calib);
   const float measuredSweepAngle = sweepInfo->measuredSweepAngle;
   const float error = measuredSweepAngle - predictedSweepAngle;
 

src/modules/src/lighthouse/lighthouse_position_est.c

@@ -206,7 +206,7 @@ static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseSt
   sweepInfo.stdDev = sweepStd;
   sweepInfo.rotorPos = &lighthouseBaseStationsGeometry[baseStation].origin;
   sweepInfo.t = 0;
-  sweepInfo.baseStationType = lighthouseBsTypeV1;
+  sweepInfo.calibrationMeasurementModel = lighthouseCalibrationMeasurementModelLh1;
 
   for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
     pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh1[sensor].baseStatonMeasurements[baseStation];
@@ -242,7 +242,7 @@ static void estimatePositionSweepsLh2(pulseProcessorResult_t* angles, int baseSt
   sweepInfo.rotorPos = &lighthouseBaseStationsGeometry[baseStation].origin;
   sweepInfo.rotorRot = &lighthouseBaseStationsGeometry[baseStation].mat;
   sweepInfo.rotorRotInv = &baseStationInvertedRotationMatrixes[baseStation];
-  sweepInfo.baseStationType = lighthouseBsTypeV2;
+  sweepInfo.calibrationMeasurementModel = lighthouseCalibrationMeasurementModelLh2;
 
   for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
     pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh2[sensor].baseStatonMeasurements[baseStation];

src/utils/interface/lighthouse/lighthouse_calibration.h

@@ -52,15 +52,29 @@ void lighthouseCalibrationApplyV2(const lighthouseCalibration_t* calib, const fl
  */
 void lighthouseCalibrationApplyNothing(const float rawAngles[2], float correctedAngles[2]);
 
+/**
+ * @brief Generic function pointer type for a calibration measurement model.
+ *        Predict the measured sweep angle based on a position for a lighthouse rotor. The position is relative to the rotor reference frame.
+ * @param x meters
+ * @param y meters
+ * @param z meters
+ * @param t Tilt of the light plane in radians
+ * @param calib Calibration data for the rotor
+ * @return float The predicted uncompensated sweep angle of the rotor
+ *
+ */
+typedef float (*lighthouseCalibrationMeasurementModel_t)(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib);
+
 /**
  * @brief Predict the measured sweep angle based on a position for a lighthouse 1 rotor. The position is relative to the rotor reference frame.
  * @param x meters
  * @param y meters
  * @param z meters
+ * @param t Tilt of the light plane in radians - not used in LH1, will always use 0
  * @param calib Calibration data for the rotor
  * @return float The predicted uncompensated sweep angle of the rotor
  */
-float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const lighthouseCalibrationSweep_t* calib);
+float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib);
 
 /**
  * @brief Predict the measured sweep angle based on a position for a lighthouse 2 rotor. The position is relative to the rotor reference frame.

src/utils/src/lighthouse/lighthouse_calibration.c

@@ -64,9 +64,10 @@ static void idealToDistortedV1(const lighthouseCalibration_t* calib, const float
   const float x = 1.0f;
   const float y = tanf(ax);
   const float z = tanf(ay);
+  const float tIgnore = 0.0f;
 
-  distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, &calib->sweep[0]);
-  distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, &calib->sweep[1]);
+  distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, tIgnore, &calib->sweep[0]);
+  distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, tIgnore, &calib->sweep[1]);
 }
 
 static void idealToDistortedV2(const lighthouseCalibration_t* calib, const float* ideal, float* distorted) {
@@ -123,7 +124,7 @@ void lighthouseCalibrationApplyNothing(const float rawAngles[2], float corrected
   correctedAngles[1] = rawAngles[1];
 }
 
-float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const lighthouseCalibrationSweep_t* calib) {
+float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib) {
   const float ax = atan2f(y, x);
   const float ay = atan2f(z, x);
   const float r = arm_sqrt(x * x + y * y);