#641 Refactoring, renamed types to work better with both LH1 and LH2

examples/app_api/src/app_lighthouse.c

@@ -37,14 +37,14 @@ baseStationGeometry_t sampleGeoData[PULSE_PROCESSOR_N_BASE_STATIONS]  = {
 lighthouseCalibration_t sampleCalibrationData[PULSE_PROCESSOR_N_BASE_STATIONS] = {
   { // Base station 0
     .valid = true,
-    .axis = {
+    .sweep = {
       {.tilt = -0.047058, .phase = 0.0, .curve = 0.052215, .gibphase = 2.087890, .gibmag = -0.003913, .ogeephase = 0.433105, .ogeemag = -0.049285},
       {.tilt = 0.048065, .phase = -0.005336, .curve = 0.122375, .gibphase = 2.097656, .gibmag = -0.003883, .ogeephase = 0.631835, .ogeemag = -0.034851},
     },
   },
   { // Base station 1
     .valid = true,
-    .axis = {
+    .sweep = {
       {.tilt = -0.051208, .phase = 0.0, .curve = 0.011756, .gibphase = 2.136718, .gibmag = -0.006057, .ogeephase = 2.705078,},
       {.tilt = 0.045623, .phase = -0.004142, .curve = 0.104736, .gibphase = 2.349609, .gibmag = -0.003332, .ogeephase = 0.380859, .ogeemag = -0.240112,},
     },

examples/demos/swarm_demo/app.c

@@ -181,14 +181,14 @@ const baseStationGeometry_t lighthouseGeoData[PULSE_PROCESSOR_N_BASE_STATIONS]
 lighthouseCalibration_t lighthouseCalibrationData[PULSE_PROCESSOR_N_BASE_STATIONS] = {
   { // Base station 0
     .valid = true,
-    .axis = {
+    .sweep = {
       {.tilt = -0.047058, .phase = 0.0, .curve = 0.052215, .gibphase = 2.087890, .gibmag = -0.003913, .ogeephase = 0.433105, .ogeemag = -0.049285},
       {.tilt = 0.048065, .phase = -0.005336, .curve = 0.122375, .gibphase = 2.097656, .gibmag = -0.003883, .ogeephase = 0.631835, .ogeemag = -0.034851},
     },
   },
   { // Base station 1
     .valid = true,
-    .axis = {
+    .sweep = {
       {.tilt = -0.051208, .phase = 0.0, .curve = 0.011756, .gibphase = 2.136718, .gibmag = -0.006057, .ogeephase = 2.705078,},
       {.tilt = 0.045623, .phase = -0.004142, .curve = 0.104736, .gibphase = 2.349609, .gibmag = -0.003332, .ogeephase = 0.380859, .ogeemag = -0.240112,},
     },

src/modules/interface/stabilizer_types.h

@@ -256,7 +256,7 @@ typedef struct {
   float measuredSweepAngle;
   float stdDev;
   int baseStationType;       // Cast to lighthouseBaseStationType_t enum.
-  const lighthouseCalibrationAxis_t* calib;
+  const lighthouseCalibrationSweep_t* calib;
 } sweepAngleMeasurement_t;
 
 // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number.

src/modules/src/lighthouse/lighthouse_core.c

@@ -190,8 +190,8 @@ static void convertV2AnglesToV1Angles(pulseProcessorResult_t* angles) {
   }
 }
 
-static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation, int axis) {
-  if (axis == sweepDirection_y) {
+static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation, int sweepId) {
+  if (sweepId == sweepIdSecond) {
     pulseProcessorApplyCalibration(appState, angles, basestation);
     if (lighthouseBsTypeV2 == angles->measurementType) {
       // Emulate V1 base stations for now, convert to V1 angles
@@ -262,13 +262,13 @@ static pulseProcessorProcessPulse_t identifySystem(const lighthouseUartFrame_t*
 
 static void processFrame(pulseProcessor_t *appState, pulseProcessorResult_t* angles, const lighthouseUartFrame_t* frame) {
     int basestation;
-    int axis;
+    int sweepId;
 
     pulseWidth[frame->data.sensor] = frame->data.width;
 
-    if (pulseProcessorProcessPulse(&ppState, &frame->data, angles, &basestation, &axis)) {
+    if (pulseProcessorProcessPulse(&ppState, &frame->data, angles, &basestation, &sweepId)) {
         STATS_CNT_RATE_EVENT(bsRates[basestation]);
-        usePulseResult(appState, angles, basestation, axis);
+        usePulseResult(appState, angles, basestation, sweepId);
     }
 }
 

src/modules/src/lighthouse/lighthouse_position_est.c

@@ -217,7 +217,7 @@ static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseSt
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.rotorRot = &lighthouseBaseStationsGeometry[baseStation].mat;
         sweepInfo.rotorRotInv = &baseStationInvertedRotationMatrixes[baseStation];
-        sweepInfo.calib = &bsCalib->axis[0];
+        sweepInfo.calib = &bsCalib->sweep[0];
 
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
@@ -227,7 +227,7 @@ static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseSt
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.rotorRot = &lh1Rotor2RotationMatrixes[baseStation];
         sweepInfo.rotorRotInv = &lh1Rotor2InvertedRotationMatrixes[baseStation];
-        sweepInfo.calib = &bsCalib->axis[1];
+        sweepInfo.calib = &bsCalib->sweep[1];
 
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
@@ -252,15 +252,15 @@ static void estimatePositionSweepsLh2(pulseProcessorResult_t* angles, int baseSt
       sweepInfo.measuredSweepAngle = bsMeasurement->angles[0];
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.t = -t30;
-        sweepInfo.calib = &bsCalib->axis[0];
+        sweepInfo.calib = &bsCalib->sweep[0];
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
       }
 
       sweepInfo.measuredSweepAngle = bsMeasurement->angles[1];
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.t = t30;
-        sweepInfo.calib = &bsCalib->axis[1];
+        sweepInfo.calib = &bsCalib->sweep[1];
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
       }

src/utils/interface/lighthouse/lighthouse_calibration.h

@@ -11,11 +11,11 @@ typedef struct {
   // Lh2 extra params
   float ogeemag;
   float ogeephase;
-} lighthouseCalibrationAxis_t;
+} lighthouseCalibrationSweep_t;
 
 typedef struct {
   bool valid;
-  lighthouseCalibrationAxis_t axis[2];
+  lighthouseCalibrationSweep_t sweep[2];
 } lighthouseCalibration_t;
 
 /**
@@ -60,7 +60,7 @@ void lighthouseCalibrationApplyNothing(const float rawAngles[2], float corrected
  * @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 lighthouseCalibrationAxis_t* calib);
+float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, 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.
@@ -71,4 +71,4 @@ float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, con
  * @param calib Calibration data for the rotor
  * @return float The predicted uncompensated sweep angle of the rotor
  */
-float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationAxis_t* calib);
+float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationSweep_t* calib);

src/utils/interface/lighthouse/pulse_processor.h

@@ -107,9 +107,9 @@ typedef struct {
 } pulseProcessorPulse_t;
 
 typedef enum {
-  sweepDirection_x = 0,
-  sweepDirection_y = 1
-} SweepDirection;
+  sweepIdFirst = 0,  // The X sweep in LH 1
+  sweepIdSecond = 1  // The Y sweep in LH 1
+} SweepId_t;
 
 typedef enum {
   sweepStorageStateWaiting = 0,
@@ -150,7 +150,7 @@ typedef struct {
 
   // Base station and axis of the current frame
   int currentBaseStation;
-  SweepDirection currentAxis;
+  SweepId_t currentAxis;
 
   // Sweep timestamps
   struct {

src/utils/src/lighthouse/lighthouse_calibration.c

@@ -38,21 +38,21 @@
 
 void lighthouseCalibrationInitFromFrame(lighthouseCalibration_t *calib, struct ootxDataFrame_s *frame)
 {
-  calib->axis[0].phase = frame->phase0;
-  calib->axis[0].tilt = frame->tilt0;
-  calib->axis[0].curve = frame->curve0;
-  calib->axis[0].gibmag = frame->gibmag0;
-  calib->axis[0].gibphase = frame->gibphase0;
-  calib->axis[0].ogeemag = frame->ogeemag0;
-  calib->axis[0].ogeephase = frame->ogeephase0;
-
-  calib->axis[1].phase = frame->phase1;
-  calib->axis[1].tilt = frame->tilt1;
-  calib->axis[1].curve = frame->curve1;
-  calib->axis[1].gibmag = frame->gibmag1;
-  calib->axis[1].gibphase = frame->gibphase1;
-  calib->axis[1].ogeemag = frame->ogeemag1;
-  calib->axis[1].ogeephase = frame->ogeephase1;
+  calib->sweep[0].phase = frame->phase0;
+  calib->sweep[0].tilt = frame->tilt0;
+  calib->sweep[0].curve = frame->curve0;
+  calib->sweep[0].gibmag = frame->gibmag0;
+  calib->sweep[0].gibphase = frame->gibphase0;
+  calib->sweep[0].ogeemag = frame->ogeemag0;
+  calib->sweep[0].ogeephase = frame->ogeephase0;
+
+  calib->sweep[1].phase = frame->phase1;
+  calib->sweep[1].tilt = frame->tilt1;
+  calib->sweep[1].curve = frame->curve1;
+  calib->sweep[1].gibmag = frame->gibmag1;
+  calib->sweep[1].gibphase = frame->gibphase1;
+  calib->sweep[1].ogeemag = frame->ogeemag1;
+  calib->sweep[1].ogeephase = frame->ogeephase1;
 
   calib->valid = true;
 }
@@ -65,8 +65,8 @@ static void idealToDistortedV1(const lighthouseCalibration_t* calib, const float
   const float y = tanf(ax);
   const float z = tanf(ay);
 
-  distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, &calib->axis[0]);
-  distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, &calib->axis[1]);
+  distorted[0] = lighthouseCalibrationMeasurementModelLh1(x, y, z, &calib->sweep[0]);
+  distorted[1] = lighthouseCalibrationMeasurementModelLh1(x, z, -y, &calib->sweep[1]);
 }
 
 static void idealToDistortedV2(const lighthouseCalibration_t* calib, const float* ideal, float* distorted) {
@@ -80,8 +80,8 @@ static void idealToDistortedV2(const lighthouseCalibration_t* calib, const float
   const float y = tanf((a2 + a1) / 2.0f);
   const float z = sinf(a2 - a1) / (tan30 * (cosf(a2) + cosf(a1)));
 
-  distorted[0] = lighthouseCalibrationMeasurementModelLh2(x, y, z, -t30, &calib->axis[0]);
-  distorted[1] = lighthouseCalibrationMeasurementModelLh2(x, y, z, t30, &calib->axis[1]);
+  distorted[0] = lighthouseCalibrationMeasurementModelLh2(x, y, z, -t30, &calib->sweep[0]);
+  distorted[1] = lighthouseCalibrationMeasurementModelLh2(x, y, z, t30, &calib->sweep[1]);
 }
 
 typedef void (* idealToDistortedFcn_t)(const lighthouseCalibration_t* calib, const float* ideal, float* distorted);
@@ -123,7 +123,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 lighthouseCalibrationAxis_t* calib) {
+float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, const float z, 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);
@@ -135,7 +135,7 @@ float lighthouseCalibrationMeasurementModelLh1(const float x, const float y, con
   return ax - (compTilt + calib->phase + compGib + compCurve);
 }
 
-float lighthouseCalibrationMeasurementModelLh2(const float x, const float y, const float z, const float t, const lighthouseCalibrationAxis_t* calib) {
+float lighthouseCalibrationMeasurementModelLh2(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);

src/utils/src/lighthouse/pulse_processor_v1.c

@@ -145,11 +145,11 @@ TESTABLE_STATIC int getBaseStationId(pulseProcessorV1_t *stateV1, unsigned int t
   return baseStation;
 }
 
-static SweepDirection getAxis(int width) {
-  SweepDirection result = sweepDirection_x;
+static SweepId_t getAxis(int width) {
+  SweepId_t result = sweepIdFirst;
 
   if ((((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x01) != 0) {
-    result = sweepDirection_y;
+    result = sweepIdSecond;
   }
 
   return result;
@@ -226,7 +226,7 @@ static void storeSyncData(pulseProcessorV1_t *stateV1, int baseStation, unsigned
   if (0 == baseStation) {
     stateV1->currentSync0 = timestamp;
     stateV1->currentSync0Width = width;
-    if (getAxis(width) == sweepDirection_x) {
+    if (getAxis(width) == sweepIdFirst) {
       uint32_t prevSync0X = stateV1->currentSync0X;
       stateV1->currentSync0X = timestamp;
       stateV1->frameWidth[0][0] = TS_DIFF(stateV1->currentSync0X, prevSync0X);
@@ -237,7 +237,7 @@ static void storeSyncData(pulseProcessorV1_t *stateV1, int baseStation, unsigned
     }
   } else {
     stateV1->currentSync1Width = width;
-    if (getAxis(width) == sweepDirection_x) {
+    if (getAxis(width) == sweepIdFirst) {
       uint32_t prevSync1X = stateV1->currentSync1X;
       stateV1->currentSync1X = timestamp;
       stateV1->frameWidth[1][0] = TS_DIFF(stateV1->currentSync1X, prevSync1X);

src/utils/src/lighthouse/pulse_processor_v2.c

@@ -306,7 +306,7 @@ bool handleAngles(pulseProcessor_t *state, const pulseProcessorFrame_t* frameDat
                 calculateAngles(block, previousBlock, angles);
 
                 *baseStation = channel;
-                *axis = sweepDirection_y;
+                *axis = sweepIdSecond;
                 angles->measurementType = lighthouseBsTypeV2;
 
                 anglesMeasured = true;