#679 Refactoring - moved calibration data update from pulse processor…

…s to lighthouse_core

src/modules/src/lighthouse/lighthouse_core.c

@@ -374,6 +374,17 @@ TESTABLE_STATIC lighthouseBaseStationType_t identifyBaseStationType(const lighth
     return lighthouseBsTypeUnknown;
 }
 
+static void useCalibrationData(pulseProcessor_t *appState) {
+  for (int baseStation = 0; baseStation < PULSE_PROCESSOR_N_BASE_STATIONS; baseStation++) {
+    if (appState->ootxDecoder[baseStation].isFullyDecoded) {
+      if (! appState->bsCalibration[baseStation].valid) {
+        DEBUG_PRINT("Got calibration from %08X on channel %d\n", (unsigned int)appState->ootxDecoder[baseStation].frame.id, baseStation);
+        lighthouseCalibrationInitFromFrame(&appState->bsCalibration[baseStation], &appState->ootxDecoder[baseStation].frame);
+      }
+    }
+  }
+}
+
 static pulseProcessorProcessPulse_t identifySystem(const lighthouseUartFrame_t* frame, lighthouseBsIdentificationData_t* bsIdentificationData) {
   pulseProcessorProcessPulse_t result = (void*)0;
 
@@ -397,13 +408,18 @@ static pulseProcessorProcessPulse_t identifySystem(const lighthouseUartFrame_t*
 static void processFrame(pulseProcessor_t *appState, pulseProcessorResult_t* angles, const lighthouseUartFrame_t* frame) {
     int basestation;
     int sweepId;
+    bool calibDataIsDecoded = false;
 
     pulseWidth[frame->data.sensor] = frame->data.width;
 
-    if (pulseProcessorProcessPulse(appState, &frame->data, angles, &basestation, &sweepId)) {
+    if (pulseProcessorProcessPulse(appState, &frame->data, angles, &basestation, &sweepId, &calibDataIsDecoded)) {
         STATS_CNT_RATE_EVENT(bsRates[basestation]);
         usePulseResult(appState, angles, basestation, sweepId);
     }
+
+    if (calibDataIsDecoded) {
+      useCalibrationData(appState);
+    }
 }
 
 static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFrame_t* frame, const uint32_t now_ms) {

src/utils/interface/lighthouse/ootx_decoder.h

@@ -77,6 +77,7 @@ typedef struct ootxDecoderState_s {
   bool synchronized;
   int nZeros;
   enum {rxLength, rxData, rxCrc0, rxCrc1, rxDone} rxState;
+  bool isFullyDecoded;
 
   union {
     uint16_t data[(OOTX_MAX_FRAME_LENGTH+1) / 2];

src/utils/interface/lighthouse/pulse_processor.h

@@ -249,14 +249,16 @@ typedef struct {
 /**
  * @brief Interface for processing of pulse data from the lighthouse
  *
- * @param state
- * @param frameData
- * @param baseStation
- * @param axis
+ * @param state               The current pulse processing state
+ * @param frameData           The frame of pulse data to process
+ * @param angles              The resulting angle information that was extracted from the frameData. Valid if this function returns true.
+ * @param baseStation         The channel (base station) that the frame originates from. Valid if this function returns true.
+ * @param axis                The axis (first of second sweep) represented by the frame. Valid if this function returns true.
+ * @param calibDataIsDecoded  True if there is one or more blocks of calibration data that have been decoded.
  * @return true, angle, base station and axis are written
  * @return false, no valid result
  */
-typedef bool (*pulseProcessorProcessPulse_t)(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis);
+typedef bool (*pulseProcessorProcessPulse_t)(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded);
 
 /**
  * @brief Apply calibration correction to all angles of all sensors for a particular baseStation

src/utils/interface/lighthouse/pulse_processor_v1.h

@@ -45,7 +45,7 @@
  * @return true, angle, base station and axis are written
  * @return false, no valid result
  */
-bool pulseProcessorV1ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis);
+bool pulseProcessorV1ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded);
 
 void pulseProcessorV1ProcessValidAngles(pulseProcessorResult_t* angles, int basestation);
 

src/utils/interface/lighthouse/pulse_processor_v2.h

@@ -45,7 +45,7 @@
  * @return true, angle, base station and axis are written
  * @return false, no valid result
  */
-bool pulseProcessorV2ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis);
+bool pulseProcessorV2ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded);
 
 /**
  * @brief Convert Lighthouse v2 angles to Lighthouse V1 angles

src/utils/src/lighthouse/ootx_decoder.c

@@ -43,7 +43,7 @@ uint16_t betole(uint16_t value)
 
 // Frame format described there: https://github.com/nairol/LighthouseRedox/blob/master/docs/Light%20Emissions.md#ootx-frame
 bool ootxDecoderProcessBit(ootxDecoderState_t * state, int data)
-{ 
+{
   data &= 1;
 
   // Synchronization finder
@@ -53,6 +53,7 @@ bool ootxDecoderProcessBit(ootxDecoderState_t * state, int data)
     state->wordReceived = 0;
     state->rxState = rxLength;
     // DEBUG_PRINT("Synchronized!\n");
+    state->isFullyDecoded = false;
     return false;
   }
   if (data == 0) {
@@ -68,6 +69,7 @@ bool ootxDecoderProcessBit(ootxDecoderState_t * state, int data)
       if (data == 0) {
         // DEBUG_PRINT("Unsynchronized!\n");
         state->synchronized = false;
+        state->isFullyDecoded = false;
         return false;
       }
       state->bitInWord = 0;
@@ -76,8 +78,10 @@ bool ootxDecoderProcessBit(ootxDecoderState_t * state, int data)
       // TODO: Check CRC!
       if (state->rxState == rxDone) {
         state->synchronized = false;
+        state->isFullyDecoded = true;
         return true;
       } else {
+        state->isFullyDecoded = false;
         return false;
       }
     }
@@ -93,6 +97,7 @@ bool ootxDecoderProcessBit(ootxDecoderState_t * state, int data)
           // DEBUG_PRINT("Length %0d\n", state->frameLength);
           if (state->frameLength > OOTX_MAX_FRAME_LENGTH) {
             state->synchronized = false;
+            state->isFullyDecoded = false;
             return false;
           }
           state->rxState = rxData;
@@ -119,5 +124,6 @@ bool ootxDecoderProcessBit(ootxDecoderState_t * state, int data)
       }
     }
   }
+  state->isFullyDecoded = false;
   return false;
 }

src/utils/src/lighthouse/pulse_processor_v1.c

@@ -274,27 +274,20 @@ TESTABLE_STATIC bool isNewSync(uint32_t timestamp, uint32_t lastSync) {
 #define DEBUG_PRINT printf
 #endif
 
-static void printBSInfo(struct ootxDataFrame_s *frame)
-{
-  DEBUG_PRINT("Got calibration from %08X\n", (unsigned int)frame->id);
-  DEBUG_PRINT("  phase0: %f\n", (double)frame->phase0);
-  DEBUG_PRINT("  phase1: %f\n", (double)frame->phase1);
-}
+static bool decodeAndApplyBaseStationCalibrationData(pulseProcessor_t *state) {
+  bool isDecoded = false;
 
-static void decodeAndApplyBaseStationCalibrationData(pulseProcessor_t *state) {
-  if (!state->bsCalibration[0].valid &&
-      ootxDecoderProcessBit(&state->ootxDecoder[0], getOotxDataBit(state->v1.currentSync0Width))) {
-    printBSInfo(&state->ootxDecoder[0].frame);
-    lighthouseCalibrationInitFromFrame(&state->bsCalibration[0], &state->ootxDecoder[0].frame);
+  if (ootxDecoderProcessBit(&state->ootxDecoder[0], getOotxDataBit(state->v1.currentSync0Width))) {
+    isDecoded = true;
   }
-  if (!state->bsCalibration[1].valid &&
-      ootxDecoderProcessBit(&state->ootxDecoder[1], getOotxDataBit(state->v1.currentSync1Width))) {
-    printBSInfo(&state->ootxDecoder[1].frame);
-    lighthouseCalibrationInitFromFrame(&state->bsCalibration[1], &state->ootxDecoder[1].frame);
+  if (ootxDecoderProcessBit(&state->ootxDecoder[1], getOotxDataBit(state->v1.currentSync1Width))) {
+    isDecoded = true;
   }
+
+  return isDecoded;
 }
 
-static bool processSync(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, pulseProcessorResult_t* angles, int *baseStation, int *axis) {
+static bool processSync(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded) {
   bool anglesMeasured = false;
   pulseProcessorV1_t* stateV1 = &state->v1;
 
@@ -303,7 +296,7 @@ static bool processSync(pulseProcessor_t *state, unsigned int timestamp, unsigne
       anglesMeasured = processPreviousFrame(stateV1, angles, baseStation, axis);
 
       if (anglesMeasured) {
-        decodeAndApplyBaseStationCalibrationData(state);
+        *calibDataIsDecoded = decodeAndApplyBaseStationCalibrationData(state);
       }
 
       int baseStation = getBaseStationId(stateV1, timestamp);
@@ -322,20 +315,19 @@ static bool processSync(pulseProcessor_t *state, unsigned int timestamp, unsigne
   return anglesMeasured;
 }
 
-static bool processWhenSynchronized(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t* angles, int *baseStation, int *axis) {
+static bool processWhenSynchronized(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded) {
   bool anglesMeasured = false;
 
   if (isSweep(&state->v1, timestamp, width)) {
     storeSweepData(&state->v1, sensor, timestamp);
   } else {
-    anglesMeasured = processSync(state, timestamp, width, angles, baseStation, axis);
+    anglesMeasured = processSync(state, timestamp, width, angles, baseStation, axis, calibDataIsDecoded);
   }
 
   return anglesMeasured;
 }
 
-
-bool pulseProcessorV1ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis)
+bool pulseProcessorV1ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded)
 {
   bool anglesMeasured = false;
 
@@ -346,7 +338,7 @@ bool pulseProcessorV1ProcessPulse(pulseProcessor_t *state, const pulseProcessorF
       pulseProcessorAllClear(angles);
     }
   } else {
-    anglesMeasured = processWhenSynchronized(state, frameData->sensor, frameData->timestamp, frameData->width, angles, baseStation, axis);
+    anglesMeasured = processWhenSynchronized(state, frameData->sensor, frameData->timestamp, frameData->width, angles, baseStation, axis, calibDataIsDecoded);
     angles->measurementType = lighthouseBsTypeV1;
   }
 

src/utils/src/lighthouse/pulse_processor_v2.c

@@ -253,45 +253,24 @@ TESTABLE_STATIC bool isBlockPairGood(const pulseProcessorV2SweepBlock_t* latest,
     return true;
 }
 
-static void printBSInfo(struct ootxDataFrame_s *frame) {
-  DEBUG_PRINT("Got calibration from %08X\n", (unsigned int)frame->id);
-//   DEBUG_PRINT("  tilt0: %f\n", (double)frame->tilt0);
-//   DEBUG_PRINT("  phase0: %f\n", (double)frame->phase0);
-//   DEBUG_PRINT("  curve0: %f\n", (double)frame->curve0);
-//   DEBUG_PRINT("  gibphase0: %f\n", (double)frame->gibphase0);
-//   DEBUG_PRINT("  gibmag0: %f\n", (double)frame->gibmag0);
-//   DEBUG_PRINT("  ogeephase0: %f\n", (double)frame->ogeephase0);
-//   DEBUG_PRINT("  ogeemag0: %f\n", (double)frame->ogeemag0);
-
-//   DEBUG_PRINT("  tilt1: %f\n", (double)frame->tilt1);
-//   DEBUG_PRINT("  phase1: %f\n", (double)frame->phase1);
-//   DEBUG_PRINT("  curve1: %f\n", (double)frame->curve1);
-//   DEBUG_PRINT("  gibphase1: %f\n", (double)frame->gibphase1);
-//   DEBUG_PRINT("  gibmag1: %f\n", (double)frame->gibmag1);
-//   DEBUG_PRINT("  ogeephase1: %f\n", (double)frame->ogeephase1);
-//   DEBUG_PRINT("  ogeemag1: %f\n", (double)frame->ogeemag1);
-}
+TESTABLE_STATIC bool handleCalibrationData(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData) {
+    bool isFullMessage = false;
 
-TESTABLE_STATIC void handleCalibrationData(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData) {
     if (frameData->channelFound && frameData->channel < PULSE_PROCESSOR_N_BASE_STATIONS) {
         const uint8_t channel = frameData->channel;
         if (frameData->offset != NO_OFFSET) {
-            if (! state->bsCalibration[channel].valid) {
-                const uint32_t prevTimestamp0 = state->v2.ootxTimestamps[channel];
-                const uint32_t timestamp0 = TS_DIFF(frameData->timestamp, frameData->offset);
-
-                if (TS_ABS_DIFF_LARGER_THAN(timestamp0, prevTimestamp0, MIN_TICKS_BETWEEN_SLOW_BITS)) {
-                    bool fullMessage = ootxDecoderProcessBit(&state->ootxDecoder[channel], frameData->slowbit);
-                    if (fullMessage) {
-                        printBSInfo(&state->ootxDecoder[channel].frame);
-                        lighthouseCalibrationInitFromFrame(&state->bsCalibration[channel], &state->ootxDecoder[channel].frame);
-                    }
-                }
-
-                state->v2.ootxTimestamps[channel] = timestamp0;
+            const uint32_t prevTimestamp0 = state->v2.ootxTimestamps[channel];
+            const uint32_t timestamp0 = TS_DIFF(frameData->timestamp, frameData->offset);
+
+            if (TS_ABS_DIFF_LARGER_THAN(timestamp0, prevTimestamp0, MIN_TICKS_BETWEEN_SLOW_BITS)) {
+                isFullMessage = ootxDecoderProcessBit(&state->ootxDecoder[channel], frameData->slowbit);
             }
+
+            state->v2.ootxTimestamps[channel] = timestamp0;
         }
     }
+
+    return isFullMessage;
 }
 
 bool handleAngles(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis) {
@@ -319,8 +298,8 @@ bool handleAngles(pulseProcessor_t *state, const pulseProcessorFrame_t* frameDat
     return anglesMeasured;
 }
 
-bool pulseProcessorV2ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis) {
-    handleCalibrationData(state, frameData);
+bool pulseProcessorV2ProcessPulse(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData, pulseProcessorResult_t* angles, int *baseStation, int *axis, bool* calibDataIsDecoded) {
+    *calibDataIsDecoded = handleCalibrationData(state, frameData);
     return handleAngles(state, frameData, angles, baseStation, axis);
 }
 

test/modules/src/lighthouse/test_lighthouse_core.c

@@ -8,6 +8,7 @@
 #include "mock_pulse_processor_v2.h"
 #include "mock_lighthouse_deck_flasher.h"
 #include "mock_lighthouse_position_est.h"
+#include "mock_lighthouse_calibration.h"
 #include "mock_uart1.h"
 #include "mock_statsCnt.h"
 #include "mock_storage.h"

test/utils/src/lighthouse/test_pulse_processor_v2.c

@@ -15,7 +15,7 @@ bool storePulse(const pulseProcessorFrame_t* frameData, pulseProcessorV2PulseWor
 void augmentFramesInWorkspace(pulseProcessorV2PulseWorkspace_t* pulseWorkspace);
 bool processWorkspaceBlock(const pulseProcessorFrame_t slots[], pulseProcessorV2SweepBlock_t* block);
 bool isBlockPairGood(const pulseProcessorV2SweepBlock_t* latest, const pulseProcessorV2SweepBlock_t* storage);
-void handleCalibrationData(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData);
+bool handleCalibrationData(pulseProcessor_t *state, const pulseProcessorFrame_t* frameData);
 
 // Helpers
 static void addDefaultFrames();
@@ -397,19 +397,6 @@ void testThatSlowBitIsNotProcessedIfOffsetIsMissing() {
     TEST_ASSERT_EQUAL(0, nrOfCallsToOotxDecoderProcessBit);
 }
 
-void testThatSlowBitIsNotProcessedIfThereAlreadyIsCalibrationData() {
-    // Fixture
-    state.bsCalibration[SB_CHANNEL].valid = true;
-
-    setUpOotxDecoderProcessBitCallCounter();
-
-    // Test
-    handleCalibrationData(&state, &slowbitFrame);
-
-    // Assert
-    TEST_ASSERT_EQUAL(0, nrOfCallsToOotxDecoderProcessBit);
-}
-
 void testThatMoreThanOneSlowBitIsProcessed() {
     // Fixture
     setUpOotxDecoderProcessBitCallCounter();
@@ -473,16 +460,26 @@ void testThatOnlyOneSlowBitsPerRevolutionIsProcessed2() {
     TEST_ASSERT_EQUAL(1, nrOfCallsToOotxDecoderProcessBit);
 }
 
-void testThatFullSlowbitMessageIsPassedOnAsCalibrationData() {
+void testThatFullSlowbitMessageIsReported() {
     // Fixture
     ootxDecoderProcessBit_IgnoreAndReturn(true);
-    lighthouseCalibrationInitFromFrame_Expect(&state.bsCalibration[SB_CHANNEL], &state.ootxDecoder[SB_CHANNEL].frame);
 
     // Test
-    handleCalibrationData(&state, &slowbitFrame);
+    bool actual = handleCalibrationData(&state, &slowbitFrame);
+
+    // Assert
+    TEST_ASSERT_TRUE(actual);
+}
+
+void testThatNonFullSlowbitMessageIsNotReported() {
+    // Fixture
+    ootxDecoderProcessBit_IgnoreAndReturn(false);
+
+    // Test
+    bool actual = handleCalibrationData(&state, &slowbitFrame);
 
     // Assert
-    // Verified in mocks
+    TEST_ASSERT_FALSE(actual);
 }