From 75b4132da15bcc42b01a7fd748f6d1439bead13b Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 28 Nov 2019 06:56:36 +0100
Subject: [PATCH] #461 Refactoring

---
 src/deck/drivers/src/lighthouse.c          |  3 +--
 src/utils/src/lighthouse/pulse_processor.c | 25 ++++++++++++----------
 2 files changed, 15 insertions(+), 13 deletions(-)

diff --git a/src/deck/drivers/src/lighthouse.c b/src/deck/drivers/src/lighthouse.c
index a5b833ecc0..3340c074ce 100644
--- 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
index f00dd43e1b..47b30b6985 100644
--- a/src/utils/src/lighthouse/pulse_processor.c
+++ b/src/utils/src/lighthouse/pulse_processor.c
@@ -254,6 +254,19 @@ 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;
 
@@ -261,18 +274,8 @@ static bool processSync(pulseProcessor_t *state, unsigned int timestamp, unsigne
     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);