#382: Fix and unit test isSync LH1 sync pulse detection function

bitcraze · Dec 4, 2018 · 7723d83 · 7723d83
1 parent 761658e
commit 7723d83
Show file tree

Hide file tree

Showing 2 changed files with 134 additions and 3 deletions.
diff --git a/src/utils/src/lighthouse/pulse_processor.c b/src/utils/src/lighthouse/pulse_processor.c
@@ -72,12 +72,17 @@ static bool isSweep(pulseProcessor_t *state, unsigned int timestamp, int width)
   return ((delta > SYNC_MAX_SEPARATION) && (delta < (FRAME_LENGTH - (2*SYNC_MAX_SEPARATION)))) || (width < SWEEP_MAX_WIDTH);
 }
 
-static bool isSync(pulseProcessor_t *state, unsigned int timestamp, int width)
+TESTABLE_STATIC bool isSync(pulseProcessor_t *state, unsigned int timestamp, int width)
 {
   int delta = TS_DIFF(timestamp, state->currentSync0);
   int deltaModulo = delta % FRAME_LENGTH;
 
-  if (deltaModulo < MAX_FRAME_LENGTH_NOISE || abs(deltaModulo - (FRAME_LENGTH - SYNC_SEPARATION)) < MAX_FRAME_LENGTH_NOISE) {
+  // We expect a modulo close to 0, detect and handle wrapping around FRAME_LENGTH
+  if (deltaModulo > (FRAME_LENGTH/2)) {
+    deltaModulo -= FRAME_LENGTH;
+  }
+
+  if (abs(deltaModulo) < SYNC_MAX_SEPARATION) {
     return true;
   }
   return false;
@@ -115,6 +120,7 @@ static bool processWhenSynchronized(pulseProcessor_t *state, unsigned int timest
         state->currentBs = 0;
         state->currentAxis = getAxis(width);
         state->currentSync = timestamp;
+        state->currentSync0 = timestamp;
       }
     } else {
       // this is sync1
@@ -128,7 +134,7 @@ static bool processWhenSynchronized(pulseProcessor_t *state, unsigned int timest
     state->lastSync = timestamp;
   } else {
     // If the pulse is not sync nor sweep: we are not syncronized anymore!
-    resetSynchonization(state);
+    resetSynchronization(state);
   }
 
   return angleMeasured;

diff --git a/test/utils/src/lighthouse/test_pulse_processor.c b/test/utils/src/lighthouse/test_pulse_processor.c
@@ -332,6 +332,131 @@ void testThatGetSystemSyncTimeHandlesTimestampsWithWrapping()
   TEST_ASSERT_EQUAL(expectedSyncTime, actualSyncTime);
 }
 
+bool isSync(pulseProcessor_t *state, unsigned int timestamp, int width);
+
+void testThatIsSyncFindsNextSync0()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = 0;
+  uint32_t timestamp = state.currentSync0 + FRAME_LENGTH;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_TRUE(result);
+}
+
+void testThatIsSyncFindsNextSync1()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = 0;
+  uint32_t timestamp = state.currentSync0 + FRAME_LENGTH + SYNC_SEPARATION;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_TRUE(result);
+}
+
+void testThatIsSyncFindsDistantSync1()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = 0;
+  uint32_t timestamp = state.currentSync0 + (10*FRAME_LENGTH) + SYNC_SEPARATION;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_TRUE(result);
+}
+
+void testThatIsSyncReturnFalseOnSweep()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = 0;
+  uint32_t timestamp = state.currentSync0 + FRAME_LENGTH + SWEEP_CENTER;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_FALSE(result);
+}
+
+void testThatIsSyncFindsSync0WithSomeNoise()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = 0;
+  uint32_t timestamp = state.currentSync0 + FRAME_LENGTH - 10;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_TRUE(result);
+}
+
+void testThatIsSyncFindsSync1WithSomeNoise()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = 0;
+  uint32_t timestamp = state.currentSync0 + FRAME_LENGTH + SYNC_SEPARATION + 500;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_TRUE(result);
+}
+
+
+void testThatIsSyncFindsSync0WithWrapping()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = TIMESTAMP_MAX - (FRAME_LENGTH/2);
+  uint32_t timestamp = (state.currentSync0 + FRAME_LENGTH) & TIMESTAMP_MAX;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_TRUE(result);
+}
+
+void testThatIsSyncFindsSync1WithWrapping()
+{
+  // Fixture
+  pulseProcessor_t state = {0};
+  state.currentSync0 = TIMESTAMP_MAX - (FRAME_LENGTH/2);;
+  uint32_t timestamp = (state.currentSync0 + FRAME_LENGTH + SYNC_SEPARATION) & TIMESTAMP_MAX;;
+  uint32_t width = SYNC_X;
+
+  // Test
+  bool result = isSync(&state, timestamp, width);
+
+  // Assert
+  TEST_ASSERT_TRUE(result);
+}
+
+
+
 // Test helpers
 
 static void assertSyncTimeIsMultipleOfFrameLength(uint32_t expectedSyncTime, uint32_t actualSyncTime)