Merge pull request #477 from whoenig/feature_locsrv_led

Add LED-ring effect to show status of localization service.

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 **********/

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)