Skip to content

Commit

Permalink
Changed some LH logs to debug logs
Browse files Browse the repository at this point in the history
  • Loading branch information
@krichardsson
krichardsson committed Sep 21, 2022
1 parent 66302d2 commit 5f9d544
Showing 1 changed file with 28 additions and 27 deletions.
55 changes: 28 additions & 27 deletions src/modules/src/lighthouse/lighthouse_core.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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);

Expand All @@ -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);

Expand Down Expand Up @@ -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);
}

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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]
Expand All @@ -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]
Expand All @@ -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]
Expand All @@ -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]
Expand Down Expand Up @@ -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)

Expand Down

0 comments on commit 5f9d544

Please sign in to comment.