diff --git a/src/utils/src/lighthouse/pulse_processor.c b/src/utils/src/lighthouse/pulse_processor.c index 42d8bcc8e1..65cc49b62c 100644 --- 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 index 6083d1cdd6..c5b5b3c908 100644 --- 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)