diff --git a/src/modules/src/lighthouse/lighthouse_core.c b/src/modules/src/lighthouse/lighthouse_core.c index 8553f6a0f3..c44efc018d 100644 --- a/src/modules/src/lighthouse/lighthouse_core.c +++ b/src/modules/src/lighthouse/lighthouse_core.c @@ -83,6 +83,8 @@ static bool uartSynchronized = false; #define ONE_SECOND 1000 #define HALF_SECOND 500 #define FIFTH_SECOND 200 + +#ifdef CONFIG_DEBUG_LOG_ENABLE static STATS_CNT_RATE_DEFINE(serialFrameRate, ONE_SECOND); static STATS_CNT_RATE_DEFINE(frameRate, ONE_SECOND); static STATS_CNT_RATE_DEFINE(cycleRate, ONE_SECOND); @@ -91,7 +93,6 @@ static STATS_CNT_RATE_DEFINE(bs0Rate, HALF_SECOND); static STATS_CNT_RATE_DEFINE(bs1Rate, HALF_SECOND); static statsCntRateLogger_t* bsRates[CONFIG_DECK_LIGHTHOUSE_MAX_N_BS] = {&bs0Rate, &bs1Rate}; -#ifdef CONFIG_DEBUG_LOG_ENABLE static STATS_CNT_RATE_DEFINE(preThrottleRate, ONE_SECOND); static STATS_CNT_RATE_DEFINE(postThrottleRate, ONE_SECOND); #endif @@ -260,7 +261,7 @@ TESTABLE_STATIC bool getUartFrameRaw(lighthouseUartFrame_t *frame) { bool isPaddingZero = (((data[5] | data[8]) & 0xfe) == 0); bool isFrameValid = (isPaddingZero || frame->isSyncFrame); - STATS_CNT_RATE_EVENT(&serialFrameRate); + STATS_CNT_RATE_EVENT_DEBUG(&serialFrameRate); return isFrameValid; } @@ -343,7 +344,7 @@ static void usePulseResultCrossingBeams(pulseProcessor_t *appState, pulseProcess } if (foundPair) { - STATS_CNT_RATE_EVENT(&cycleRate); + STATS_CNT_RATE_EVENT_DEBUG(&cycleRate); lighthousePositionEstimatePoseCrossingBeams(appState, angles, baseStation, otherBaseStation); @@ -353,7 +354,7 @@ static void usePulseResultCrossingBeams(pulseProcessor_t *appState, pulseProcess } static void usePulseResultSweeps(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int baseStation) { - STATS_CNT_RATE_EVENT(&cycleRate); + STATS_CNT_RATE_EVENT_DEBUG(&cycleRate); pulseProcessorClearOutdated(appState, angles, baseStation); @@ -458,7 +459,7 @@ static void processFrame(pulseProcessor_t *appState, pulseProcessorResult_t* ang pulseWidth[frame->data.sensor] = frame->data.width; if (pulseProcessorProcessPulse(appState, &frame->data, angles, &baseStation, &sweepId, &calibDataIsDecoded)) { - STATS_CNT_RATE_EVENT(bsRates[baseStation]); + STATS_CNT_RATE_EVENT_DEBUG(bsRates[baseStation]); usePulseResult(appState, angles, baseStation, sweepId, now_ms); } @@ -555,7 +556,7 @@ void lighthouseCoreTask(void *param) { } // Now we are receiving items else if(!frame.isSyncFrame) { - STATS_CNT_RATE_EVENT(&frameRate); + STATS_CNT_RATE_EVENT_DEBUG(&frameRate); lighthouseTransmitProcessFrame(&frame); deckHealthCheck(&lighthouseCoreState, &frame, now_ms); @@ -690,7 +691,7 @@ LOG_ADD(LOG_FLOAT, angle1y, &angles.baseStationMeasurementsLh1[1].sensorMeasurem * * If a base station of type V2 is used, this will contain the V2 angles converted to V1 style for the base station with channel 1. */ -LOG_ADD(LOG_FLOAT, angle0x_1, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[1].correctedAngles[0]) +LOG_ADD_DEBUG(LOG_FLOAT, angle0x_1, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[1].correctedAngles[0]) /** * @brief The V1 angle received by sensor 1, corrected using calibration data [rad] @@ -702,7 +703,7 @@ LOG_ADD(LOG_FLOAT, angle0x_1, &angles.baseStationMeasurementsLh1[0].sensorMeasur * * If a base station of type V2 is used, this will contain the V2 angles converted to V1 style for the base station with channel 1. */ -LOG_ADD(LOG_FLOAT, angle0y_1, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[1].correctedAngles[1]) +LOG_ADD_DEBUG(LOG_FLOAT, angle0y_1, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[1].correctedAngles[1]) /** * @brief The V1 angle received by sensor 1, corrected using calibration data [rad] @@ -714,7 +715,7 @@ LOG_ADD(LOG_FLOAT, angle0y_1, &angles.baseStationMeasurementsLh1[0].sensorMeasur * * If a base station of type V2 is used, this will contain the V2 angles converted to V1 style for the base station with channel 2. */ -LOG_ADD(LOG_FLOAT, angle1x_1, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[1].correctedAngles[0]) +LOG_ADD_DEBUG(LOG_FLOAT, angle1x_1, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[1].correctedAngles[0]) /** * @brief The V1 angle received by sensor 1, corrected using calibration data [rad] @@ -726,17 +727,17 @@ LOG_ADD(LOG_FLOAT, angle1x_1, &angles.baseStationMeasurementsLh1[1].sensorMeasur * * If a base station of type V2 is used, this will contain the V2 angles converted to V1 style for the base station with channel 2. */ -LOG_ADD(LOG_FLOAT, angle1y_1, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[1].correctedAngles[1]) +LOG_ADD_DEBUG(LOG_FLOAT, angle1y_1, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[1].correctedAngles[1]) -LOG_ADD(LOG_FLOAT, angle0x_2, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[2].correctedAngles[0]) -LOG_ADD(LOG_FLOAT, angle0y_2, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[2].correctedAngles[1]) -LOG_ADD(LOG_FLOAT, angle1x_2, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[2].correctedAngles[0]) -LOG_ADD(LOG_FLOAT, angle1y_2, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[2].correctedAngles[1]) +LOG_ADD_DEBUG(LOG_FLOAT, angle0x_2, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[2].correctedAngles[0]) +LOG_ADD_DEBUG(LOG_FLOAT, angle0y_2, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[2].correctedAngles[1]) +LOG_ADD_DEBUG(LOG_FLOAT, angle1x_2, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[2].correctedAngles[0]) +LOG_ADD_DEBUG(LOG_FLOAT, angle1y_2, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[2].correctedAngles[1]) -LOG_ADD(LOG_FLOAT, angle0x_3, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[3].correctedAngles[0]) -LOG_ADD(LOG_FLOAT, angle0y_3, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[3].correctedAngles[1]) -LOG_ADD(LOG_FLOAT, angle1x_3, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[3].correctedAngles[0]) -LOG_ADD(LOG_FLOAT, angle1y_3, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[3].correctedAngles[1]) +LOG_ADD_DEBUG(LOG_FLOAT, angle0x_3, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[3].correctedAngles[0]) +LOG_ADD_DEBUG(LOG_FLOAT, angle0y_3, &angles.baseStationMeasurementsLh1[0].sensorMeasurements[3].correctedAngles[1]) +LOG_ADD_DEBUG(LOG_FLOAT, angle1x_3, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[3].correctedAngles[0]) +LOG_ADD_DEBUG(LOG_FLOAT, angle1y_3, &angles.baseStationMeasurementsLh1[1].sensorMeasurements[3].correctedAngles[1]) /** * @brief The raw V2 angle received by sensor 0 [rad] @@ -821,25 +822,25 @@ LOG_ADD(LOG_FLOAT, angle1y_0lh2, &angles.baseStationMeasurementsLh2[1].sensorMea /** * @brief Rate of frames from the Lighthouse deck on the serial buss [1/s] */ -STATS_CNT_RATE_LOG_ADD(serRt, &serialFrameRate) +STATS_CNT_RATE_LOG_ADD_DEBUG(serRt, &serialFrameRate) /** * @brief Rate of frames from the Lighthouse deck that contains sweep data [1/s] */ -STATS_CNT_RATE_LOG_ADD(frmRt, &frameRate) +STATS_CNT_RATE_LOG_ADD_DEBUG(frmRt, &frameRate) -STATS_CNT_RATE_LOG_ADD(cycleRt, &cycleRate) +STATS_CNT_RATE_LOG_ADD_DEBUG(cycleRt, &cycleRate) -STATS_CNT_RATE_LOG_ADD(bs0Rt, &bs0Rate) -STATS_CNT_RATE_LOG_ADD(bs1Rt, &bs1Rate) +STATS_CNT_RATE_LOG_ADD_DEBUG(bs0Rt, &bs0Rate) +STATS_CNT_RATE_LOG_ADD_DEBUG(bs1Rt, &bs1Rate) STATS_CNT_RATE_LOG_ADD_DEBUG(preThRt, &preThrottleRate) STATS_CNT_RATE_LOG_ADD_DEBUG(postThRt, &postThrottleRate) -LOG_ADD(LOG_UINT16, width0, &pulseWidth[0]) -LOG_ADD(LOG_UINT16, width1, &pulseWidth[1]) -LOG_ADD(LOG_UINT16, width2, &pulseWidth[2]) -LOG_ADD(LOG_UINT16, width3, &pulseWidth[3]) +LOG_ADD_DEBUG(LOG_UINT16, width0, &pulseWidth[0]) +LOG_ADD_DEBUG(LOG_UINT16, width1, &pulseWidth[1]) +LOG_ADD_DEBUG(LOG_UINT16, width2, &pulseWidth[2]) +LOG_ADD_DEBUG(LOG_UINT16, width3, &pulseWidth[3]) LOG_ADD(LOG_UINT8, comSync, &uartSynchronized)