diff --git a/src/deck/drivers/src/lighthouse.c b/src/deck/drivers/src/lighthouse.c
index 24f821ceb9..9ed44b71b8 100644
--- a/src/deck/drivers/src/lighthouse.c
+++ b/src/deck/drivers/src/lighthouse.c
@@ -47,7 +47,7 @@
 #include "estimator_kalman.h"
 
 
-static float angles[2][2];
+static pulseProcessorResult_t angles[PULSE_PROCESSOR_N_SENSORS];
 
 typedef union frame_u {
   struct {
@@ -88,7 +88,6 @@ static void lighthouseTask(void *param)
   static frame_t frame;
   static pulseProcessor_t ppState = {};
 
-  float angle;
   int basestation;
   int axis;
 
@@ -123,23 +122,27 @@ static void lighthouseTask(void *param)
         continue;
       }
 
-      if (frame.sensor == 0) {
-        // DEBUG_PRINT("Reading %08X:%04X\n", frame.timestamp, frame.width);
-        if (pulseProcessorProcessPulse(&ppState, frame.timestamp, frame.width, &angle, &basestation, &axis)) {
-          angles[basestation][axis] = angle;
-          
-          if (basestation == 1 && axis == 1) {
-            lighthouseGeometryGetPosition(baseStationsGeometry, (void*)angles, position, &delta);
-
-            ext_pos.x = position[0];
-            ext_pos.y = -position[2];
-            ext_pos.z = position[1];
-            ext_pos.stdDev = 0.01;
-            estimatorKalmanEnqueuePosition(&ext_pos);
+      if (pulseProcessorProcessPulse(&ppState, frame.sensor, frame.timestamp, frame.width, angles, &basestation, &axis)) {
+        if (basestation == 1 && axis == 1) {
+          for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+            // Only use sensor 0 for now
+            if (sensor == 0) {
+              if (angles[sensor].validCount == 4) {
+                lighthouseGeometryGetPosition(baseStationsGeometry, (void*)angles[sensor].angles, position, &delta);
+
+                ext_pos.x = position[0];
+                ext_pos.y = -position[2];
+                ext_pos.z = position[1];
+                ext_pos.stdDev = 0.01;
+                estimatorKalmanEnqueuePosition(&ext_pos);
+
+//                DEBUG_PRINT("%i %f %f %f\n", sensor, (double)ext_pos.x, (double)ext_pos.y, (double)ext_pos.z);
+              }
+            }
+
+            angles[sensor].validCount = 0;
           }
         }
-      } else {
-        DEBUG_PRINT("Receiving from wrong sensor?!?!?!\n");
       }
 
       synchronized = getFrame(&frame);
@@ -174,10 +177,10 @@ static const DeckDriver lighthouse_deck = {
 DECK_DRIVER(lighthouse_deck);
 
 LOG_GROUP_START(lighthouse)
-LOG_ADD(LOG_FLOAT, angle0x, &angles[0][0])
-LOG_ADD(LOG_FLOAT, angle0y, &angles[1][0])
-LOG_ADD(LOG_FLOAT, angle1x, &angles[0][1])
-LOG_ADD(LOG_FLOAT, angle1y, &angles[1][1])
+LOG_ADD(LOG_FLOAT, angle0x, &angles[0].angles[0][0])
+LOG_ADD(LOG_FLOAT, angle0y, &angles[0].angles[1][0])
+LOG_ADD(LOG_FLOAT, angle1x, &angles[0].angles[0][1])
+LOG_ADD(LOG_FLOAT, angle1y, &angles[0].angles[1][1])
 LOG_ADD(LOG_FLOAT, x, &position[0])
 LOG_ADD(LOG_FLOAT, y, &position[1])
 LOG_ADD(LOG_FLOAT, z, &position[2])
diff --git a/src/utils/interface/lighthouse/pulse_processor.h b/src/utils/interface/lighthouse/pulse_processor.h
index 4ae5266f81..6375945ae9 100644
--- a/src/utils/interface/lighthouse/pulse_processor.h
+++ b/src/utils/interface/lighthouse/pulse_processor.h
@@ -4,7 +4,7 @@
 #include <stdint.h>
 #include <stdlib.h>
 
-#define PULSE_PROCESSOR_N_SENSORS 8
+#define PULSE_PROCESSOR_N_SENSORS 4
 #define PULSE_PROCESSOR_HISTORY_LENGTH 8
 #define TIMESTAMP_BITWIDTH 29
 #define TIMESTAMP_MAX ((1<<TIMESTAMP_BITWIDTH)-1)
@@ -17,6 +17,17 @@ typedef struct {
   int width;
 } pulseProcessorPulse_t;
 
+typedef enum {
+  sweepDirection_j = 0,
+  sweepDirection_k = 1
+} SweepDirection;
+
+typedef enum {
+  sweepStorageStateWaiting = 0,
+  sweepStorageStateValid,
+  sweepStorageStateError,
+} SweepStorageState_t;
+
 typedef struct pulseProcessor_s {
   bool synchronized;    // At true if we are currently syncthonized (ie. we have seen one short sweep)
 
@@ -31,23 +42,28 @@ typedef struct pulseProcessor_s {
   int nSyncPulses;          // Number of sync pulses accumulated
 
   // Sync pulse timestamps
-  uint32_t currentSync;   // Sync currently used for sweep phase measurment
+  uint32_t currentSync;   // Sync currently used for sweep phase measurement
   uint32_t currentSync0;  // Sync0 of the current frame
   uint32_t currentSync1;  // Sync1 of the current frame
 
   // Base station and axis of the current frame
-  int currentBs;
-  int currentAxis;
+  int currentBaseStation;
+  SweepDirection currentAxis;
 
   // Sweep timestamps
   struct {
     uint32_t timestamp;
-    bool valid;
-    bool error;
+    SweepStorageState_t state;
   } sweeps[PULSE_PROCESSOR_N_SENSORS];
+  bool sweepDataStored;
 
 } pulseProcessor_t;
 
+typedef struct {
+  float angles[2][2];
+  int validCount;
+} pulseProcessorResult_t;
+
 // If returns true, the angle, base station and direction are written
-bool pulseProcessorProcessPulse(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, float *angle, int *baseStation, int *axis);
+bool pulseProcessorProcessPulse(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t angles[], int *baseStation, int *axis);
 
diff --git a/src/utils/src/lighthouse/pulse_processor.c b/src/utils/src/lighthouse/pulse_processor.c
index 03506dfdfb..5f9c0ac84c 100644
--- a/src/utils/src/lighthouse/pulse_processor.c
+++ b/src/utils/src/lighthouse/pulse_processor.c
@@ -17,27 +17,25 @@
 #define SYNC_DIVIDER 500
 #define SYNC_MAX_SEPARATION 25000   // More than 400us (400us is 19200)
 #define SYNC_SEPARATION 19200
-#define SENSOR_MAX_DISPERTION 10
+#define SENSOR_MAX_DISPERTION 20
 #define MAX_FRAME_LENGTH_NOISE 400
 
 // Utility functions and macros
-#define TS_DIFF(X, Y) ((X-Y)&((1<<TIMESTAMP_BITWIDTH)-1))
+// #define TS_DIFF(X, Y) ((X-Y)&((1<<TIMESTAMP_BITWIDTH)-1))
+static uint32_t TS_DIFF(uint32_t x, uint32_t y) {
+  const uint32_t bitmask = (1 << TIMESTAMP_BITWIDTH) - 1;
+  return (x - y) & bitmask;
+}
+
 
 
 TESTABLE_STATIC bool findSyncTime(const pulseProcessorPulse_t pulseHistory[], uint32_t *foundSyncTime);
 TESTABLE_STATIC bool getSystemSyncTime(const uint32_t syncTimes[], size_t nSyncTimes, uint32_t *syncTime);
 
-
-
 static void resetPulseHistory(pulseProcessor_t *state) {
   memset(state->pulseHistoryPtr, 0, sizeof(state->pulseHistoryPtr));
 }
 
-static void resetSynchronization(pulseProcessor_t *state)
-{
-  state->synchronized = false;
-}
-
 static void synchronize(pulseProcessor_t *state, int sensor, uint32_t timestamp, uint32_t width)
 {
   state->pulseHistory[sensor][state->pulseHistoryPtr[sensor]].timestamp = timestamp;
@@ -68,13 +66,13 @@ static void synchronize(pulseProcessor_t *state, int sensor, uint32_t timestamp,
 
 static bool isSweep(pulseProcessor_t *state, unsigned int timestamp, int width)
 {
-  int delta = TS_DIFF(timestamp, state->lastSync);
+  uint32_t delta = TS_DIFF(timestamp, state->lastSync);
   return ((delta > SYNC_MAX_SEPARATION) && (delta < (FRAME_LENGTH - (2*SYNC_MAX_SEPARATION)))) || (width < SWEEP_MAX_WIDTH);
 }
 
 TESTABLE_STATIC bool isSync(pulseProcessor_t *state, unsigned int timestamp, int width)
 {
-  int delta = TS_DIFF(timestamp, state->currentSync0);
+  uint32_t delta = TS_DIFF(timestamp, state->currentSync0);
   int deltaModulo = delta % FRAME_LENGTH;
 
   // We expect a modulo close to 0, detect and handle wrapping around FRAME_LENGTH
@@ -88,70 +86,147 @@ TESTABLE_STATIC bool isSync(pulseProcessor_t *state, unsigned int timestamp, int
   return false;
 }
 
-static bool getAxis(int width)
-{
-  return (((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x01) != 0;
+static int getBaseStationId(pulseProcessor_t *state, unsigned int timestamp) {
+  int baseStation = 1;
+
+  uint32_t delta = TS_DIFF(timestamp, state->currentSync0);
+  if (delta > SYNC_MAX_SEPARATION) {
+    baseStation = 0;
+  }
+
+  return baseStation;
+}
+
+static SweepDirection getAxis(int width) {
+  SweepDirection result = sweepDirection_j;
+
+  if ((((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x01) != 0) {
+    result = sweepDirection_k;
+  }
+
+  return result;
+}
+
+static bool isSweepActiveThisFrame(int width) {
+  return (((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x04) == 0;
+}
+
+static void storeSweepData(pulseProcessor_t *state, int sensor, unsigned int timestamp) {
+  if (state->sweeps[sensor].state == sweepStorageStateWaiting) {
+    state->sweeps[sensor].timestamp = timestamp;
+    state->sweeps[sensor].state = sweepStorageStateValid;
+  } else {
+    state->sweeps[sensor].state = sweepStorageStateError;
+  }
+
+  state->sweepDataStored = true;
+}
+
+static void resetSweepData(pulseProcessor_t *state) {
+  for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+    state->sweeps[sensor].state = sweepStorageStateWaiting;
+  }
+  state->sweepDataStored = false;
 }
 
-static bool getSkip(int width)
+static void resetSynchronization(pulseProcessor_t *state)
 {
-  return (((width-SYNC_BASE_WIDTH)/SYNC_DIVIDER)&0x04) != 0;
+  resetSweepData(state);
+  state->synchronized = false;
 }
 
+static bool processPreviousFrame(pulseProcessor_t *state, pulseProcessorResult_t result[], int *baseStation, int *axis) {
+  bool anglesMeasured = false;
 
-static bool processWhenSynchronized(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, float *angle, int *baseStation, int *axis) {
-  bool angleMeasured = false;
+  if (state->sweepDataStored) {
+    for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
+      if (state->sweeps[sensor].state == sweepStorageStateValid) {
+        int delta = TS_DIFF(state->sweeps[sensor].timestamp, state->currentSync);
+        if (delta < FRAME_LENGTH) {
+          float angle = (delta - SWEEP_CENTER)*(float)M_PI/FRAME_LENGTH;
 
-  if (isSweep(state, timestamp, width)) {
-    int delta = TS_DIFF(timestamp, state->currentSync);
+          *baseStation = state->currentBaseStation;
+          *axis = state->currentAxis;
 
-    if (delta < FRAME_LENGTH) {
-      *angle = (delta - SWEEP_CENTER)*(float)M_PI/FRAME_LENGTH;
-      *baseStation = state->currentBs;
-      *axis = state->currentAxis;
-      angleMeasured = true;
-    }
+          result[sensor].angles[state->currentBaseStation][state->currentAxis] = angle;
+          result[sensor].validCount++;
 
-    state->currentSync = 0;
-  } else if (isSync(state, timestamp, width)) {
-    if (TS_DIFF(timestamp, state->lastSync) > SYNC_MAX_SEPARATION) {
-      // This is sync0
-      if (!getSkip(width)) {
-        state->currentBs = 0;
-        state->currentAxis = getAxis(width);
-        state->currentSync = timestamp;
-        state->currentSync0 = timestamp;
+          anglesMeasured = true;
+        }
       }
+    }
+
+    resetSweepData(state);
+  }
+
+  return anglesMeasured;
+}
+
+static void storeSyncData(pulseProcessor_t *state, unsigned int timestamp, unsigned int width) {
+  int baseStation = getBaseStationId(state, timestamp);
+  if (0 == baseStation) {
+    state->currentSync0 = timestamp;
+  } else {
+    state->currentSync1 = timestamp;
+  }
+
+  state->lastSync = timestamp;
+
+  if (isSweepActiveThisFrame(width)) {
+    state->currentBaseStation = baseStation;
+    state->currentAxis = getAxis(width);
+    state->currentSync = timestamp;
+  }
+}
+
+TESTABLE_STATIC bool isNewSync(uint32_t timestamp, uint32_t lastSync) {
+  const uint32_t min = (1 << TIMESTAMP_BITWIDTH) - SENSOR_MAX_DISPERTION;
+  const uint32_t max = SENSOR_MAX_DISPERTION;
+
+  uint32_t diff = TS_DIFF(timestamp, lastSync);
+  return (diff > max) && (diff < min);
+}
+
+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, width)) {
+      anglesMeasured = processPreviousFrame(state, angles, baseStation, axis);
+      storeSyncData(state, timestamp, width);
     } else {
-      // this is sync1
-      if (!getSkip(width)) {
-        state->currentBs = 1;
-        state->currentAxis = getAxis(width);
-        state->currentSync = timestamp;
-      }
+      // Expected a sync but something is wrong, re-synchronize.
+      resetSynchronization(state);
     }
+  }
+
+  return anglesMeasured;
+}
+
+static bool processWhenSynchronized(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t angles[], int *baseStation, int *axis) {
+  bool anglesMeasured = false;
 
-    state->lastSync = timestamp;
+  if (isSweep(state, timestamp, width)) {
+    storeSweepData(state, sensor, timestamp);
   } else {
-    // If the pulse is not sync nor sweep: we are not syncronized anymore!
-    resetSynchronization(state);
+    anglesMeasured = processSync(state, timestamp, width, angles, baseStation, axis);
   }
 
-  return angleMeasured;
+  return anglesMeasured;
 }
 
 
-bool pulseProcessorProcessPulse(pulseProcessor_t *state, unsigned int timestamp, unsigned int width, float *angle, int *baseStation, int *axis)
+bool pulseProcessorProcessPulse(pulseProcessor_t *state, int sensor, unsigned int timestamp, unsigned int width, pulseProcessorResult_t angles[], int *baseStation, int *axis)
 {
-  bool angleMeasured = false;
+  bool anglesMeasured = false;
 
   if (!state->synchronized) {
-    synchronize(state, 0, timestamp, width);
+    synchronize(state, sensor, timestamp, width);
   } else {
-    angleMeasured = processWhenSynchronized(state, timestamp, width, angle, baseStation, axis);
+    anglesMeasured = processWhenSynchronized(state, sensor, timestamp, width, angles, baseStation, axis);
   }
 
-  return angleMeasured;
+  return anglesMeasured;
 }
 
 
diff --git a/test/utils/src/lighthouse/test_pulse_processor.c b/test/utils/src/lighthouse/test_pulse_processor.c
index 1825855e13..ec79e0b6c2 100644
--- a/test/utils/src/lighthouse/test_pulse_processor.c
+++ b/test/utils/src/lighthouse/test_pulse_processor.c
@@ -455,7 +455,6 @@ void testThatIsSyncFindsSync1WithWrapping()
   TEST_ASSERT_TRUE(result);
 }
 
-
 void testThatIsSyncReturnsFalseIfSync1WasSync0AndTheRealSync0IsReceived()
 {
   // Fixture
@@ -471,6 +470,55 @@ void testThatIsSyncReturnsFalseIfSync1WasSync0AndTheRealSync0IsReceived()
   TEST_ASSERT_FALSE(result);
 }
 
+bool isNewSync(uint32_t timestamp, uint32_t lastSync);
+
+void testThatIsNewSyncMatchesTimestampCloseAfter() {
+  // Fixture
+  uint32_t lastSync = 4711;
+  uint32_t timestamp = lastSync + 3;
+
+  // Test
+  bool actual = isNewSync(timestamp, lastSync);
+
+  // Assert
+  TEST_ASSERT_FALSE(actual);
+}
+
+void testThatIsNewSyncMatchesTimestampCloseBefore() {
+  // Fixture
+  uint32_t lastSync = 4711;
+  uint32_t timestamp = lastSync - 3;
+
+  // Test
+  bool actual = isNewSync(timestamp, lastSync);
+
+  // Assert
+  TEST_ASSERT_FALSE(actual);
+}
+
+void testThatIsNewSyncMatchesTimestampCloseBeforeWhenWrapping() {
+  // Fixture
+  uint32_t lastSync = 1;
+  uint32_t timestamp = (1 << TIMESTAMP_BITWIDTH) - 1;
+
+  // Test
+  bool actual = isNewSync(timestamp, lastSync);
+
+  // Assert
+  TEST_ASSERT_FALSE(actual);
+}
+
+void testThatIsNewSyncDoesNotMatchTimestampTooFarAway() {
+  // Fixture
+  uint32_t lastSync = 4711;
+  uint32_t timestamp = lastSync + 30;
+
+  // Test
+  bool actual = isNewSync(timestamp, lastSync);
+
+  // Assert
+  TEST_ASSERT_TRUE(actual);
+}
 
 
 // Test helpers