diff --git a/src/modules/interface/lighthouse/lighthouse_position_est.h b/src/modules/interface/lighthouse/lighthouse_position_est.h index 50d4c4bd9b..471bdfd1f7 100644 --- a/src/modules/interface/lighthouse/lighthouse_position_est.h +++ b/src/modules/interface/lighthouse/lighthouse_position_est.h @@ -41,5 +41,12 @@ void lighthousePositionEstInit(); */ void lighthousePositionSetGeometryData(const uint8_t baseStation, const baseStationGeometry_t* geometry); +/** + * @brief Updates bit field that describes which base stations that have valid calibration data + * + * @param baseStation The base station that was updated + */ +void lighthousePositionCalibrationDataWritten(const uint8_t baseStation); + void lighthousePositionEstimatePoseCrossingBeams(const pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation); void lighthousePositionEstimatePoseSweeps(const pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation); diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index a57f31a76f..87fe09b6b9 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/src/modules/src/lighthouse/lighthouse_core.c @@ -380,6 +380,7 @@ static void useCalibrationData(pulseProcessor_t *appState) { if (! appState->bsCalibration[baseStation].valid) { DEBUG_PRINT("Got calibration from %08X on channel %d\n", (unsigned int)appState->ootxDecoder[baseStation].frame.id, baseStation); lighthouseCalibrationInitFromFrame(&appState->bsCalibration[baseStation], &appState->ootxDecoder[baseStation].frame); + lighthousePositionCalibrationDataWritten(baseStation); } } } @@ -520,6 +521,7 @@ void lighthouseCoreTask(void *param) { void lighthouseCoreSetCalibrationData(const uint8_t baseStation, const lighthouseCalibration_t* calibration) { if (baseStation < PULSE_PROCESSOR_N_BASE_STATIONS) { lighthouseCoreState.bsCalibration[baseStation] = *calibration; + lighthousePositionCalibrationDataWritten(baseStation); } } diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c index 8a8cce08b0..86cde3b076 100644 --- a/src/modules/src/lighthouse/lighthouse_position_est.c +++ b/src/modules/src/lighthouse/lighthouse_position_est.c @@ -40,8 +40,6 @@ #include "lighthouse_geometry.h" #include "lighthouse_state.h" -// lighthouseBaseStationsGeometry has been moved to lighthouse_core.c - #define ONE_SECOND 1000 #define HALF_SECOND 500 static STATS_CNT_RATE_DEFINE(positionRate, ONE_SECOND); @@ -71,6 +69,18 @@ static const MemoryHandlerDef_t memDef = { // A bitmap indicating which base stations that has valid geo data static uint16_t baseStationGeoValidMap; +// A bitmap indicating which base stations that have valid calibration data +static uint16_t baseStationCalibValidMap; + +static void modifyBit(uint16_t *bitmap, const int index, const bool value) { + const uint16_t mask = (1 << index); + + if (value) { + *bitmap |= mask; + } else { + *bitmap &= ~mask; + } +} void lighthousePositionEstInit() { for (int i = 0; i < PULSE_PROCESSOR_N_BASE_STATIONS; i++) { @@ -137,9 +147,16 @@ static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint32_t inPageAddr = calibOffsetAddr % pageSize; if (index < PULSE_PROCESSOR_N_BASE_STATIONS) { if (inPageAddr + writeLen <= sizeof(lighthouseCalibration_t)) { + // Mark the calibration data as invalid since this write probably only will update part of it + // If this is the last write in this block, the valid flag will be part of the data and set appropriately + // This is based on the assumption that the writes are done in oder with increasing addresses + lighthouseCoreState.bsCalibration[index].valid = false; + uint8_t* start = (uint8_t*)&lighthouseCoreState.bsCalibration[index]; memcpy(start + inPageAddr, buffer, writeLen); + lighthousePositionCalibrationDataWritten(index); + result = true; } } @@ -148,18 +165,19 @@ static bool handleMemWrite(const uint32_t memAddr, const uint8_t writeLen, const return result; } -static void lighthousePositionGeometryDataUpdated(const int baseStation) { - const uint16_t basestationBitMap = (1 << baseStation); +void lighthousePositionCalibrationDataWritten(const uint8_t baseStation) { + if (baseStation < PULSE_PROCESSOR_N_BASE_STATIONS) { + modifyBit(&baseStationCalibValidMap, baseStation, lighthouseCoreState.bsCalibration[baseStation].valid); + } +} +static void lighthousePositionGeometryDataUpdated(const int 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; } + modifyBit(&baseStationGeoValidMap, baseStation, lighthouseCoreState.bsGeometry[baseStation].valid); } void lighthousePositionSetGeometryData(const uint8_t baseStation, const baseStationGeometry_t* geometry) { @@ -427,6 +445,7 @@ LOG_ADD(LOG_FLOAT, z, &position[2]) LOG_ADD(LOG_FLOAT, delta, &deltaLog) LOG_ADD(LOG_UINT16, bsGeo, &baseStationGeoValidMap) +LOG_ADD(LOG_UINT16, bsCalibVal, &baseStationCalibValidMap) LOG_GROUP_STOP(lighthouse) diff --git a/test/modules/src/lighthouse/test_lighthouse_core.c b/test/modules/src/lighthouse/test_lighthouse_core.c index e52b6a7bce..4c2f09a63e 100644 --- a/test/modules/src/lighthouse/test_lighthouse_core.c +++ b/test/modules/src/lighthouse/test_lighthouse_core.c @@ -401,6 +401,7 @@ void testInitializationOfCalibIsDoneFromStorage() { for (int i = 0; i < PULSE_PROCESSOR_N_BASE_STATIONS; i++) { storageFetch_StubWithCallback(mockStorageFetchForCalib); + lighthousePositionCalibrationDataWritten_Expect(i); } // Test