diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index f6e9917d16..74eeb3f384 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/src/modules/src/lighthouse/lighthouse_core.c @@ -83,8 +83,13 @@ 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}; -// Contains a bit map that indicates which base staions that are actively used, that is recevied -// and has valid geo and calib dats + +// A bit map that indicates which base staions that are received +static uint16_t baseStationReceivedMapWs; +static uint16_t baseStationReceivedMap; + +// A bit map that indicates which base staions that are actively used in the estimation process, that is recevied +// and has valid geo and calib data static uint16_t baseStationActiveMapWs; static uint16_t baseStationActiveMap; @@ -301,6 +306,9 @@ static void convertV2AnglesToV1Angles(pulseProcessorResult_t* angles) { } static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation, int sweepId) { + const uint16_t basestationBitMap = (1 << basestation); + baseStationReceivedMapWs |= basestationBitMap; + if (sweepId == sweepIdSecond) { const bool hasCalibrationData = pulseProcessorApplyCalibration(appState, angles, basestation); if (hasCalibrationData) { @@ -314,7 +322,7 @@ static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* a const bool hasGeoData = appState->bsGeometry[basestation].valid; if (hasGeoData) { - baseStationActiveMapWs = baseStationActiveMapWs | (1 << basestation); + baseStationActiveMapWs |= basestationBitMap; switch(estimationMethod) { case 0: @@ -427,6 +435,9 @@ static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFram static void updateSystemStatus(const uint32_t now_ms) { if (now_ms > nextUpdateTimeOfSystemStatus) { + baseStationReceivedMap = baseStationReceivedMapWs; + baseStationReceivedMapWs = 0; + baseStationActiveMap = baseStationActiveMapWs; baseStationActiveMapWs = 0; @@ -626,7 +637,9 @@ LOG_ADD(LOG_UINT16, width3, &pulseWidth[3]) LOG_ADD(LOG_UINT8, comSync, &uartSynchronized) +LOG_ADD(LOG_UINT16, bsReceive, &baseStationReceivedMap) LOG_ADD(LOG_UINT16, bsActive, &baseStationActiveMap) + LOG_ADD(LOG_UINT8, status, &systemStatus) LOG_GROUP_STOP(lighthouse) diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c index e006ed32df..8a8cce08b0 100644 --- a/src/modules/src/lighthouse/lighthouse_position_est.c +++ b/src/modules/src/lighthouse/lighthouse_position_est.c @@ -68,6 +68,10 @@ static const MemoryHandlerDef_t memDef = { .write = handleMemWrite, }; + +// A bitmap indicating which base stations that has valid geo data +static uint16_t baseStationGeoValidMap; + void lighthousePositionEstInit() { for (int i = 0; i < PULSE_PROCESSOR_N_BASE_STATIONS; i++) { lighthousePositionGeometryDataUpdated(i); @@ -145,10 +149,17 @@ static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const } static void lighthousePositionGeometryDataUpdated(const int baseStation) { + const uint16_t basestationBitMap = (1 << baseStation); + if (lighthouseCoreState.bsGeometry[baseStation].valid) { baseStationGeometryCache_t* cache = &lighthouseCoreState.bsGeoCache[baseStation]; preProcessGeometryData(lighthouseCoreState.bsGeometry[baseStation].mat, cache->baseStationInvertedRotationMatrixes, cache->lh1Rotor2RotationMatrixes, cache->lh1Rotor2InvertedRotationMatrixes); + + baseStationGeoValidMap |= basestationBitMap; + } else { + baseStationGeoValidMap &= ~basestationBitMap; } + } void lighthousePositionSetGeometryData(const uint8_t baseStation, const baseStationGeometry_t* geometry) { @@ -414,6 +425,9 @@ LOG_ADD(LOG_FLOAT, y, &position[1]) LOG_ADD(LOG_FLOAT, z, &position[2]) LOG_ADD(LOG_FLOAT, delta, &deltaLog) + +LOG_ADD(LOG_UINT16, bsGeo, &baseStationGeoValidMap) + LOG_GROUP_STOP(lighthouse) PARAM_GROUP_START(lighthouse)