#679 Added bit fields for received base stations and valid geo data

bitcraze · Feb 16, 2021 · 72cb335 · 72cb335
1 parent 6e87f6e
commit 72cb335
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 3 deletions.
diff --git 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
@@ -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)