#679 Added log variable indicating overall status of the lighthouse s…

…ystem

src/modules/src/lighthouse/lighthouse_core.c

@@ -63,6 +63,13 @@ static lighthouseUartFrame_t frame;
 static lighthouseBsIdentificationData_t bsIdentificationData;
 
 // Stats
+
+typedef enum uwbEvent_e {
+  statusNotReceiving = 0,
+  statusMissingData = 1,
+  statusToEstimator = 2,
+} lhSystemStatus_t;
+
 static bool uartSynchronized = false;
 
 #define ONE_SECOND 1000
@@ -78,8 +85,10 @@ static statsCntRateLogger_t* bsRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&bs0Rat
 
 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 lhSystemStatus_t systemStatus;
+static lhSystemStatus_t systemStatusWs;
+static const uint32_t SYSTEM_STATUS_UPDATE_INTERVAL = FIFTH_SECOND;
+static uint32_t nextUpdateTimeOfSystemStatus = 0;
 
 static uint16_t pulseWidth[PULSE_PROCESSOR_N_SENSORS];
 pulseProcessor_t lighthouseCoreState = {
@@ -195,7 +204,7 @@ TESTABLE_STATIC void waitForUartSynchFrame() {
 void lighthouseCoreSetLeds(lighthouseCoreLedState_t red, lighthouseCoreLedState_t orange, lighthouseCoreLedState_t green)
 {
   uint8_t commandBuffer[2];
-  
+
   commandBuffer[0] = 0x01;
   commandBuffer[1] = (green<<4) | (orange<<2) | red;
 
@@ -254,24 +263,33 @@ static void convertV2AnglesToV1Angles(pulseProcessorResult_t* angles) {
 
 static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation, int sweepId) {
   if (sweepId == sweepIdSecond) {
-    pulseProcessorApplyCalibration(appState, angles, basestation);
-    if (lighthouseBsTypeV2 == angles->measurementType) {
-      // Emulate V1 base stations for now, convert to V1 angles
-      convertV2AnglesToV1Angles(angles);
-    }
+    const bool hasCalibrationData = pulseProcessorApplyCalibration(appState, angles, basestation);
+    const bool hasGeoData = appState->bsGeometry[basestation].valid;
+    if (hasCalibrationData && hasGeoData) {
+      if (lighthouseBsTypeV2 == angles->measurementType) {
+        // Emulate V1 base stations for now, convert to V1 angles
+        convertV2AnglesToV1Angles(angles);
+      }
+
+      // Send measurement to the ground
+      locSrvSendLighthouseAngle(basestation, angles);
 
-    // Send measurement to the ground
-    locSrvSendLighthouseAngle(basestation, angles);
-
-    switch(estimationMethod) {
-      case 0:
-        usePulseResultCrossingBeams(appState, angles, basestation);
-        break;
-      case 1:
-        usePulseResultSweeps(appState, angles, basestation);
-        break;
-      default:
-        break;
+      systemStatusWs = statusToEstimator;
+
+      switch(estimationMethod) {
+        case 0:
+          usePulseResultCrossingBeams(appState, angles, basestation);
+          break;
+        case 1:
+          usePulseResultSweeps(appState, angles, basestation);
+          break;
+        default:
+          break;
+      }
+    } else {
+      if (systemStatusWs != statusToEstimator) {
+        systemStatusWs = statusMissingData;
+      }
     }
   }
 }
@@ -366,12 +384,15 @@ static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFram
   }
 }
 
-static void updateBaseStationVisibilityMap(const uint32_t now_ms) {
-  if (now_ms > nextUpdateTimeOfBaseStationVisibilityMap) {
+static void updateSystemStatus(const uint32_t now_ms) {
+  if (now_ms > nextUpdateTimeOfSystemStatus) {
     baseStationVisibilityMap = baseStationVisibilityMapWs;
     baseStationVisibilityMapWs = 0;
 
-    nextUpdateTimeOfBaseStationVisibilityMap = now_ms + BASE_STATION_VISIBILITY_MAP_UPDATE_INTERVAL;
+    systemStatus = systemStatusWs;
+    systemStatusWs = statusNotReceiving;
+
+    nextUpdateTimeOfSystemStatus = now_ms + SYSTEM_STATUS_UPDATE_INTERVAL;
   }
 }
 
@@ -422,7 +443,7 @@ void lighthouseCoreTask(void *param) {
 
       previousWasSyncFrame = frame.isSyncFrame;
 
-      updateBaseStationVisibilityMap(now_ms);
+      updateSystemStatus(now_ms);
     }
 
     uartSynchronized = false;
@@ -561,6 +582,7 @@ LOG_ADD(LOG_UINT16, width3, &pulseWidth[3])
 LOG_ADD(LOG_UINT8, comSync, &uartSynchronized)
 
 LOG_ADD(LOG_UINT16, bsVis, &baseStationVisibilityMap)
+LOG_ADD(LOG_UINT8, status, &systemStatus)
 LOG_GROUP_STOP(lighthouse)
 
 PARAM_GROUP_START(lighthouse)

src/utils/interface/lighthouse/pulse_processor.h

@@ -264,8 +264,11 @@ typedef bool (*pulseProcessorProcessPulse_t)(pulseProcessor_t *state, const puls
  * @param state
  * @param angles
  * @param baseStation
+ *
+ * @return true, calibration data has been applied
+ * @return false, calibration data is missing
  */
-void pulseProcessorApplyCalibration(pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation);
+bool pulseProcessorApplyCalibration(pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation);
 
 void pulseProcessorClearOutdated(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation);
 

src/utils/src/lighthouse/pulse_processor.c

@@ -40,7 +40,7 @@
  * @param angles The raw and calibrated angles
  * @param baseStation The base station in question
  */
-void pulseProcessorApplyCalibration(pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation){
+bool pulseProcessorApplyCalibration(pulseProcessor_t *state, pulseProcessorResult_t* angles, int baseStation){
   const lighthouseCalibration_t* calibrationData = &state->bsCalibration[baseStation];
   const bool doApplyCalibration = calibrationData->valid;
 
@@ -61,6 +61,8 @@ void pulseProcessorApplyCalibration(pulseProcessor_t *state, pulseProcessorResul
       lighthouseCalibrationApplyNothing(bsMeasurement->angles, bsMeasurement->correctedAngles);
     }
   }
+
+  return doApplyCalibration;
 }
 
 /**