#461 Refactoring

bitcraze · Nov 28, 2019 · 75b4132 · 75b4132
1 parent 5ca8853
commit 75b4132
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 13 deletions.
diff --git a/src/deck/drivers/src/lighthouse.c b/src/deck/drivers/src/lighthouse.c
@@ -550,14 +550,13 @@ LOG_ADD(LOG_UINT16, width3, &pulseWidth[3])
 LOG_ADD(LOG_UINT8, comSync, &comSynchronized)
 LOG_GROUP_STOP(lighthouse)
 
-#endif // DISABLE_LIGHTHOUSE_DRIVER
-
 #ifdef FF_EXPERIMENTAL
 PARAM_GROUP_START(lh)
 PARAM_ADD(PARAM_FLOAT, sweepStd, &sweepStd)
 PARAM_GROUP_STOP(lh)
 #endif
 
+#endif // DISABLE_LIGHTHOUSE_DRIVER
 
 PARAM_GROUP_START(deck)
 PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bdLighthouse4, &isInit)

diff --git a/src/utils/src/lighthouse/pulse_processor.c b/src/utils/src/lighthouse/pulse_processor.c
@@ -254,25 +254,28 @@ static void printBSInfo(struct ootxDataFrame_s *frame)
   DEBUG_PRINT("  phase1: %f\n", (double)frame->phase1);
 }
 
+static void decodeAndApplyBaseStationCalibrationData(pulseProcessor_t *state) {
+  if (!state->bsCalibration[0].valid &&
+      ootxDecoderProcessBit(&state->ootxDecoder0, getOotxDataBit(state->currentSync0Width))) {
+    printBSInfo(&state->ootxDecoder0.frame);
+    lighthouseCalibrationInitFromFrame(&state->bsCalibration[0], &state->ootxDecoder0.frame);
+  }
+  if (!state->bsCalibration[1].valid &&
+      ootxDecoderProcessBit(&state->ootxDecoder1, getOotxDataBit(state->currentSync1Width))) {
+    printBSInfo(&state->ootxDecoder1.frame);
+    lighthouseCalibrationInitFromFrame(&state->bsCalibration[1], &state->ootxDecoder1.frame);
+  }
+}
+
 static bool processSync(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, pulseProcessorResult_t* angles, int *baseStation, int *axis) {
   bool anglesMeasured = false;
 
   if (isNewSync(timestamp, state->lastSync)) {
     if (isSync(state, timestamp)) {
       anglesMeasured = processPreviousFrame(state, angles, baseStation, axis);
 
-      // Receive OOTX data frame and initialize BS calibration when we get it
       if (anglesMeasured) {
-        if (!state->bsCalibration[0].valid &&
-            ootxDecoderProcessBit(&state->ootxDecoder0, getOotxDataBit(state->currentSync0Width))) {
-          printBSInfo(&state->ootxDecoder0.frame);
-          lighthouseCalibrationInitFromFrame(&state->bsCalibration[0], &state->ootxDecoder0.frame);
-        }
-        if (!state->bsCalibration[1].valid &&
-            ootxDecoderProcessBit(&state->ootxDecoder1, getOotxDataBit(state->currentSync1Width))) {
-          printBSInfo(&state->ootxDecoder1.frame);
-          lighthouseCalibrationInitFromFrame(&state->bsCalibration[1], &state->ootxDecoder1.frame);
-        }
+        decodeAndApplyBaseStationCalibrationData(state);
       }
 
       storeSyncData(state, timestamp, width);