Detect large difference in frame and gyroscope arrival times

Author: Bercon

python/cli/diagnose/diagnose.py

@@ -68,8 +68,11 @@ def addMeasurement(type, t, v):
 
     startTime = None
     timeOffset = 0
+    latestGyroTime = None
     gnssConverter = GnssConverter()
 
+    framesMissingNextGyroTime = []
+
     with open(jsonlFile) as f:
         nSkipped = 0
         for line in f.readlines():
@@ -113,8 +116,15 @@ def addMeasurement(type, t, v):
                 if measurementType in ["accelerometer", "gyroscope", "magnetometer"]:
                     v = [sensor["values"][i] for i in range(3)]
                     addMeasurement(measurementType, t, v)
+                    if measurementType == "gyroscope":
+                        latestGyroTime = t
+                        for f in framesMissingNextGyroTime:
+                            gyroTimeDeltas["next"] = abs(latestGyroTime - gyroTimeDeltas["t"])
+                        framesMissingNextGyroTime.clear()
+
             elif barometer is not None:
                 addMeasurement("barometer", t, barometer["pressureHectopascals"])
+
             elif gnss is not None:
                 gnssData = data["gnss"]
                 if len(gnssData["t"]) > 0:
@@ -124,18 +134,27 @@ def addMeasurement(type, t, v):
                 enu = gnssConverter.enu(gnss["latitude"], gnss["longitude"], gnss["altitude"])
                 gnssData["position"].append([enu[c] for c in "xyz"])
                 gnssData["altitude"].append(gnss["altitude"])
+
             elif frames is not None:
                 for f in frames:
                     if f.get("missingBitmap", False): continue
                     cameras = data['cameras']
                     ind = f["cameraInd"]
                     if cameras.get(ind) is None:
-                        cameras[ind] = {"td": [], "t": [], "features": []}
+                        cameras[ind] = {"td": [], "t": [], "features": [], "gyroTimeDeltas": []}
                     else:
                         diff = t - cameras[ind]["t"][-1]
                         cameras[ind]["td"].append(diff)
                     if "features" in f: cameras[ind]["features"].append(0 if not f["features"] else len(f["features"]))
                     cameras[ind]["t"].append(t)
+                    gyroTimeDeltas = {
+                        "prev": abs(latestGyroTime - t) if latestGyroTime != None else None,
+                        "next": None,
+                        "t": t
+                    }
+                    cameras[ind]["gyroTimeDeltas"].append(gyroTimeDeltas)
+                    framesMissingNextGyroTime.append(gyroTimeDeltas)
+
             elif metrics is not None and 'cpu' in metrics:
                 addMeasurement("cpu", t, metrics['cpu'].get('systemTotalUsagePercent', 0))
                 usedProcessNames = {}  # Track duplicate process names
@@ -148,6 +167,7 @@ def addMeasurement(type, t, v):
                     processData = data['cpu']["processes"].setdefault(uniqueName, {"v": [], "t": []})
                     processData['v'].append(process['usagePercent'])
                     processData['t'].append(t)
+
             elif vioOutput is not None:
                 vio = data["vio"]
                 if len(vio["t"]) > 0:

python/cli/diagnose/sensors.py

@@ -210,6 +210,67 @@ def toPercent(value):
                     f"Found {invalidTimestamps} ({toPercent(invalidTimestamps)}) "
                     "timestamps that don't overlap with IMU")
 
+    def analyzeArrivalTimes(
+            self,
+            timestamps,
+            gyroTimeDeltas,
+            plotArgs,
+            dataName="frames"):
+
+        deltaTimes = []
+        for d in gyroTimeDeltas:
+            if d["prev"] == None or d["next"] == None:
+                deltaTimes.append(None)
+            else:
+                deltaTimes.append(min(d["prev"], d["next"]))
+
+        deltaTimes = np.array(deltaTimes).astype(float)
+
+        total = len(deltaTimes)
+        if total == 0: return
+
+        medianDeltaTime = np.nanmedian(deltaTimes)
+        warningThreshold = min(.5, medianDeltaTime * 4)
+        errorThreshold = 1
+
+        COLOR_OK = (0, 1, 0) # Green
+        COLOR_WARNING = (1, 0.65, 0) # Orange
+        COLOR_ERROR = (1, 0, 0) # Red
+        deltaTimePlotColors = []
+        badDeltaTimes = 0
+        maxError = None
+        for td in deltaTimes:
+            error = max(0, (td - medianDeltaTime))
+            if maxError == None or maxError < error: maxError = error
+            if error > warningThreshold:
+                badDeltaTimes += 1
+                if error > errorThreshold:
+                    deltaTimePlotColors.append(COLOR_ERROR)
+                else:
+                    deltaTimePlotColors.append(COLOR_WARNING)
+            else:
+                deltaTimePlotColors.append(COLOR_OK)
+
+        if badDeltaTimes > 0:
+            self.images.append(plotFrame(
+                    timestamps,
+                    deltaTimes * SECONDS_TO_MILLISECONDS,
+                    color=deltaTimePlotColors,
+                    plottype="scatter",
+                    xLabel="Time (s)",
+                    yLabel="Time diff (ms)",
+                    yScale="symlog",
+                    s=10,
+                    **plotArgs))
+
+            self.__addIssue(DiagnosisLevel.ERROR if maxError > errorThreshold else DiagnosisLevel.WARNING,
+                f"Found {badDeltaTimes} {dataName} where the neibhgouring gyroscope samples "
+                f"have time difference (expected {medianDeltaTime*SECONDS_TO_MILLISECONDS:.1f}ms) "
+                f"larger than {warningThreshold*SECONDS_TO_MILLISECONDS:.1f}ms, up to {maxError*SECONDS_TO_MILLISECONDS:.1f}ms. "
+                f"This indicates that gyroscope and {dataName} for same moment of time are fed "
+                f"large time appart which may lead to data being dropped and higher latency."
+            )
+
     def analyzeSignalDuplicateValues(
             self,
             signal,
@@ -608,6 +669,12 @@ def diagnoseCamera(data, output):
             plotArgs={
                 "title": f"Camera #{ind} frame time diff"
             })
+        status.analyzeArrivalTimes(
+            timestamps,
+            camera["gyroTimeDeltas"],
+            plotArgs={
+                "title": f"Camera #{ind} nearest gyro (in file, not time) time diff"
+            })
         cameraOutput = {
             "diagnosis": status.diagnosis.toString(),
             "issues": status.serializeIssues(),