diff --git a/src/deck/drivers/src/ledring12.c b/src/deck/drivers/src/ledring12.c index f3fb706b46..08b2cd2950 100644 --- a/src/deck/drivers/src/ledring12.c +++ b/src/deck/drivers/src/ledring12.c @@ -707,6 +707,53 @@ static void rssiEffect(uint8_t buffer[][3], bool reset) } } +/** + * An effect that shows the status of the location service. + * + * Red means bad, green means good. + * Blinking means battery was low during flight. + */ +static void locSrvStatus(uint8_t buffer[][3], bool reset) +{ + static int locSrvTickId = -1; + static int pmStateId = -1; + + static int tic = 0; + static bool batteryEverLow = false; + + // lazy initialization of the logging variables + if (locSrvTickId == -1) { + locSrvTickId = logGetVarId("locSrvZ", "tick"); + pmStateId = logGetVarId("pm", "state"); + } + + // compute time since the last update in milliseconds + uint16_t time_since_last_update = xTaskGetTickCount() - logGetUint(locSrvTickId); + if (time_since_last_update > 30) { + time_since_last_update = 30; + } + + int8_t pmstate = logGetInt(pmStateId); + if (pmstate == lowPower) { + batteryEverLow = true; + } + + for (int i = 0; i < NBR_LEDS; i++) { + if (batteryEverLow && tic < 10) { + buffer[i][0] = 0; + buffer[i][1] = 0; + } else { + buffer[i][0] = LIMIT(LINSCALE(0, 30, 0, 100, time_since_last_update)); // Red (large time_since_last_update) + buffer[i][1] = LIMIT(LINSCALE(0, 30, 100, 0, time_since_last_update)); // Green (small time_since_last_update) + } + buffer[i][2] = 0; + } + + if (++tic >= 20) { + tic = 0; + } +} + /**************** Effect list ***************/ @@ -728,6 +775,7 @@ Ledring12Effect effectsFct[] = virtualMemEffect, fadeColorEffect, rssiEffect, + locSrvStatus, }; /********** Ring init and switching **********/ diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c index b1a9006fc2..df695db22e 100644 --- a/src/modules/src/crtp_localization_service.c +++ b/src/modules/src/crtp_localization_service.c @@ -92,6 +92,7 @@ static float extPosStdDev = 0.01; static float extQuatStdDev = 4.5e-3; static bool isInit = false; static uint8_t my_id; +static uint16_t tickOfLastPacket; // tick when last packet was received static void locSrvCrtpCB(CRTPPacket* pk); static void extPositionHandler(CRTPPacket* pk); @@ -138,6 +139,7 @@ static void extPositionHandler(CRTPPacket* pk) ext_pos.z = data->z; ext_pos.stdDev = extPosStdDev; estimatorEnqueuePosition(&ext_pos); + tickOfLastPacket = xTaskGetTickCount(); } static void genericLocHandle(CRTPPacket* pk) @@ -169,6 +171,7 @@ static void genericLocHandle(CRTPPacket* pk) ext_pose.stdDevPos = extPosStdDev; ext_pose.stdDevQuat = extQuatStdDev; estimatorEnqueuePose(&ext_pose); + tickOfLastPacket = xTaskGetTickCount(); } else if (type == EXT_POSE_PACKED) { uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem); for (uint8_t i = 0; i < numItems; ++i) { @@ -181,6 +184,7 @@ static void genericLocHandle(CRTPPacket* pk) ext_pose.stdDevPos = extPosStdDev; ext_pose.stdDevQuat = extQuatStdDev; estimatorEnqueuePose(&ext_pose); + tickOfLastPacket = xTaskGetTickCount(); break; } } @@ -198,7 +202,7 @@ static void extPositionPackedHandler(CRTPPacket* pk) ext_pos.z = item->z / 1000.0f; ext_pos.stdDev = extPosStdDev; estimatorEnqueuePosition(&ext_pos); - + tickOfLastPacket = xTaskGetTickCount(); break; } } @@ -246,6 +250,10 @@ LOG_GROUP_START(ext_pos) LOG_ADD(LOG_FLOAT, Z, &ext_pos.z) LOG_GROUP_STOP(ext_pos) +LOG_GROUP_START(locSrvZ) + LOG_ADD(LOG_UINT16, tick, &tickOfLastPacket) // time when data was received last (ms/ticks) +LOG_GROUP_STOP(locSrvZ) + PARAM_GROUP_START(locSrv) PARAM_ADD(PARAM_UINT8, enRangeStreamFP32, &enableRangeStreamFloat) PARAM_ADD(PARAM_FLOAT, extPosStdDev, &extPosStdDev)