diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index cdaa3e0304..d91ddea555 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/src/modules/src/lighthouse/lighthouse_core.c @@ -67,6 +67,7 @@ static bool uartSynchronized = false; #define ONE_SECOND 1000 #define HALF_SECOND 500 +#define FIFTH_SECOND 200 static STATS_CNT_RATE_DEFINE(serialFrameRate, ONE_SECOND); static STATS_CNT_RATE_DEFINE(frameRate, ONE_SECOND); static STATS_CNT_RATE_DEFINE(cycleRate, ONE_SECOND); @@ -75,6 +76,11 @@ static STATS_CNT_RATE_DEFINE(bs0Rate, HALF_SECOND); static STATS_CNT_RATE_DEFINE(bs1Rate, HALF_SECOND); static statsCntRateLogger_t* bsRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&bs0Rate, &bs1Rate}; +static uint16_t baseStationVisibilityMapWs; +static uint16_t baseStationVisibilityMap; +static const uint32_t BASE_STATION_VISIBILITY_MAP_UPDATE_INTERVAL = FIFTH_SECOND; + static uint32_t nextUpdateTimeOfBaseStationVisibilityMap = 0; + static uint16_t pulseWidth[PULSE_PROCESSOR_N_SENSORS]; pulseProcessor_t lighthouseCoreState = { // .bsGeometry = { @@ -317,15 +323,16 @@ static void processFrame(pulseProcessor_t *appState, pulseProcessorResult_t* ang if (pulseProcessorProcessPulse(appState, &frame->data, angles, &basestation, &sweepId)) { STATS_CNT_RATE_EVENT(bsRates[basestation]); + baseStationVisibilityMapWs = baseStationVisibilityMapWs | (1 << basestation); + usePulseResult(appState, angles, basestation, sweepId); } } -static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFrame_t* frame) { +static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFrame_t* frame, const uint32_t now_ms) { if (!appState->healthDetermined) { - const uint32_t now = xTaskGetTickCount(); if (0 == appState->healthFirstSensorTs) { - appState->healthFirstSensorTs = now; + appState->healthFirstSensorTs = now_ms; } if (0x0f == appState->healthSensorBitField) { @@ -334,7 +341,7 @@ static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFram } else { appState->healthSensorBitField |= (0x01 << frame->data.sensor); - if ((now - appState->healthFirstSensorTs) > MAX_WAIT_TIME_FOR_HEALTH_MS) { + if ((now_ms - appState->healthFirstSensorTs) > MAX_WAIT_TIME_FOR_HEALTH_MS) { appState->healthDetermined = true; DEBUG_PRINT("Warning: not getting data from all sensors\n"); for (int i = 0; i < PULSE_PROCESSOR_N_SENSORS; i++) { @@ -349,6 +356,15 @@ static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFram } } +static void updateBaseStationVisibilityMap(const uint32_t now_ms) { + if (now_ms > nextUpdateTimeOfBaseStationVisibilityMap) { + baseStationVisibilityMap = baseStationVisibilityMapWs; + baseStationVisibilityMapWs = 0; + + nextUpdateTimeOfBaseStationVisibilityMap = now_ms + BASE_STATION_VISIBILITY_MAP_UPDATE_INTERVAL; + } +} + void lighthouseCoreTask(void *param) { bool isUartFrameValid = false; @@ -371,6 +387,8 @@ void lighthouseCoreTask(void *param) { bool previousWasSyncFrame = false; while((isUartFrameValid = getUartFrameRaw(&frame))) { + const uint32_t now_ms = T2M(xTaskGetTickCount()); + // If a sync frame is getting through, we are only receiving sync frames. So nothing else. Reset state if(frame.isSyncFrame && previousWasSyncFrame) { pulseProcessorAllClear(&angles); @@ -379,7 +397,7 @@ void lighthouseCoreTask(void *param) { else if(!frame.isSyncFrame) { STATS_CNT_RATE_EVENT(&frameRate); - deckHealthCheck(&lighthouseCoreState, &frame); + deckHealthCheck(&lighthouseCoreState, &frame, now_ms); if (pulseProcessorProcessPulse) { processFrame(&lighthouseCoreState, &angles, &frame); } else { @@ -388,6 +406,8 @@ void lighthouseCoreTask(void *param) { } previousWasSyncFrame = frame.isSyncFrame; + + updateBaseStationVisibilityMap(now_ms); } uartSynchronized = false; @@ -524,6 +544,8 @@ LOG_ADD(LOG_UINT16, width2, &pulseWidth[2]) LOG_ADD(LOG_UINT16, width3, &pulseWidth[3]) LOG_ADD(LOG_UINT8, comSync, &uartSynchronized) + +LOG_ADD(LOG_UINT16, bsVis, &baseStationVisibilityMap) LOG_GROUP_STOP(lighthouse) PARAM_GROUP_START(lighthouse)