From 93eb0e8544824b6e9d30a98a8534346e5883a27e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 4 Feb 2021 12:58:06 +0100 Subject: [PATCH] #679 Added log variable indicating overall status of the lighthouse system --- src/modules/src/lighthouse/lighthouse_core.c | 70 ++++++++++++------- .../interface/lighthouse/pulse_processor.h | 5 +- src/utils/src/lighthouse/pulse_processor.c | 4 +- 3 files changed, 53 insertions(+), 26 deletions(-) diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index 81107142d8..62c5cda575 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/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) diff --git a/src/utils/interface/lighthouse/pulse_processor.h b/src/utils/interface/lighthouse/pulse_processor.h index d882e941a0..578e252ff5 100644 --- a/src/utils/interface/lighthouse/pulse_processor.h +++ b/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); diff --git a/src/utils/src/lighthouse/pulse_processor.c b/src/utils/src/lighthouse/pulse_processor.c index 7686858dfa..37fea84a7c 100644 --- a/src/utils/src/lighthouse/pulse_processor.c +++ b/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; } /**