#679 Added bit field for valid calibration data

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);

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);
   }
 }
 

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)
 

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