From 93eb0e8544824b6e9d30a98a8534346e5883a27e Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 4 Feb 2021 12:58:06 +0100 Subject: [PATCH 1/3] #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; } /** From 42211e6870443c1e18af2dd771c0cc844feee211 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 4 Feb 2021 13:09:42 +0100 Subject: [PATCH 2/3] #679 Added missing mock --- test/modules/src/lighthouse/test_lighthouse_core.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/modules/src/lighthouse/test_lighthouse_core.c b/test/modules/src/lighthouse/test_lighthouse_core.c index 9ff5fc523d..a2f0b468eb 100644 --- a/test/modules/src/lighthouse/test_lighthouse_core.c +++ b/test/modules/src/lighthouse/test_lighthouse_core.c @@ -32,8 +32,9 @@ lighthouseBaseStationType_t identifyBaseStationType(const lighthouseUartFrame_t* void initializeGeoDataFromStorage(); initializeCalibDataFromStorage(); -// Dummy mock +// Dummy mocks uint32_t xTaskGetTickCount() {return 0;} +void vTaskDelay(const uint32_t ignore) {} static int nrOfCallsToStorageFetchForCalib = 0; static size_t mockStorageFetchForCalib(char* key, void* buffer, size_t length, int cmock_num_calls); From 1538f812d123ca44b976a41d78ee0dbe8e6dbd10 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Thu, 4 Feb 2021 14:55:33 +0100 Subject: [PATCH 3/3] #679 Changed meaning of lh visibility bit map, it is now a bit map indicating which base stations that are active and are used in the position estimation --- src/modules/src/lighthouse/lighthouse_core.c | 27 ++++++++++++-------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index 62c5cda575..78b28473c7 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/src/modules/src/lighthouse/lighthouse_core.c @@ -83,10 +83,15 @@ 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}; -static uint16_t baseStationVisibilityMapWs; -static uint16_t baseStationVisibilityMap; +// Contains a bit map that indicates which base staions that are actively used, that is recevied +// and has valid geo and calib dats +static uint16_t baseStationActiveMapWs; +static uint16_t baseStationActiveMap; + +// An overall system status indicating if data is sent to the estimator static lhSystemStatus_t systemStatus; static lhSystemStatus_t systemStatusWs; + static const uint32_t SYSTEM_STATUS_UPDATE_INTERVAL = FIFTH_SECOND; static uint32_t nextUpdateTimeOfSystemStatus = 0; @@ -274,7 +279,7 @@ static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* a // Send measurement to the ground locSrvSendLighthouseAngle(basestation, angles); - systemStatusWs = statusToEstimator; + baseStationActiveMapWs = baseStationActiveMapWs | (1 << basestation); switch(estimationMethod) { case 0: @@ -286,10 +291,12 @@ static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* a default: break; } + } + + if (baseStationActiveMapWs != 0) { + systemStatusWs = statusToEstimator; } else { - if (systemStatusWs != statusToEstimator) { - systemStatusWs = statusMissingData; - } + systemStatusWs = statusMissingData; } } } @@ -351,8 +358,6 @@ static void processFrame(pulseProcessor_t *appState, pulseProcessorResult_t* ang if (pulseProcessorProcessPulse(appState, &frame->data, angles, &basestation, &sweepId)) { STATS_CNT_RATE_EVENT(bsRates[basestation]); - baseStationVisibilityMapWs = baseStationVisibilityMapWs | (1 << basestation); - usePulseResult(appState, angles, basestation, sweepId); } } @@ -386,8 +391,8 @@ static void deckHealthCheck(pulseProcessor_t *appState, const lighthouseUartFram static void updateSystemStatus(const uint32_t now_ms) { if (now_ms > nextUpdateTimeOfSystemStatus) { - baseStationVisibilityMap = baseStationVisibilityMapWs; - baseStationVisibilityMapWs = 0; + baseStationActiveMap = baseStationActiveMapWs; + baseStationActiveMapWs = 0; systemStatus = systemStatusWs; systemStatusWs = statusNotReceiving; @@ -581,7 +586,7 @@ LOG_ADD(LOG_UINT16, width3, &pulseWidth[3]) LOG_ADD(LOG_UINT8, comSync, &uartSynchronized) -LOG_ADD(LOG_UINT16, bsVis, &baseStationVisibilityMap) +LOG_ADD(LOG_UINT16, bsActive, &baseStationActiveMap) LOG_ADD(LOG_UINT8, status, &systemStatus) LOG_GROUP_STOP(lighthouse)