From d26d8b3ba9787dc02b9c116d7e750a3b36dd0846 Mon Sep 17 00:00:00 2001
From: Robbe Derks <robbe.derks@gmail.com>
Date: Thu, 16 Mar 2023 10:59:27 +0100
Subject: [PATCH] Improve OX frame timing jitter (#27582)

* frame timing script

* enable vsync output and improve fsin tracking

* increase measurement time

* fix python linting

---------

Co-authored-by: Comma Device <device@comma.ai>
---
 .../debug/internal/check_frame_frequencies.py | 43 +++++++++++++++++++
 system/camerad/cameras/sensor2_i2c.h          |  9 ++--
 2 files changed, 49 insertions(+), 3 deletions(-)
 create mode 100755 selfdrive/debug/internal/check_frame_frequencies.py

diff --git a/selfdrive/debug/internal/check_frame_frequencies.py b/selfdrive/debug/internal/check_frame_frequencies.py
new file mode 100755
index 0000000000..9ac8bfc80e
--- /dev/null
+++ b/selfdrive/debug/internal/check_frame_frequencies.py
@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+
+import time
+import statistics
+import cereal.messaging as messaging
+
+from typing import Dict
+
+camera_states = [
+  'roadCameraState',
+  'wideRoadCameraState',
+  'driverCameraState'
+]
+
+def fmt(val):
+  ref = 0.05
+  return f"{val:.6f} ({100 * val / ref:.2f}%)"
+
+if __name__ == "__main__":
+  sm = messaging.SubMaster(camera_states)
+
+  prev_sof = {state: None for state in camera_states}
+  diffs: Dict[str, list] = {state: [] for state in camera_states}
+
+  st = time.monotonic()
+  while True:
+    sm.update()
+
+    for state in camera_states:
+      if sm.updated[state]:
+        if prev_sof[state] is not None:
+          diffs[state].append((sm[state].timestampSof - prev_sof[state]) / 1e9)
+        prev_sof[state] = sm[state].timestampSof
+
+    if time.monotonic() - st > 10:
+      for state in camera_states:
+        values = diffs[state]
+        ref = 0.05
+        print(f"{state}  \tMean: {fmt(statistics.mean(values))} \t Min: {fmt(min(values))} \t Max: {fmt(max(values))} \t Std: {statistics.stdev(values):.6f} \t Num frames: {len(values)}")
+        diffs[state] = []
+
+      print()
+      st = time.monotonic()
diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h
index 83fcb8f7a9..4db4bfd4d8 100644
--- a/system/camerad/cameras/sensor2_i2c.h
+++ b/system/camerad/cameras/sensor2_i2c.h
@@ -58,7 +58,7 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
 
   // SC ctrl
   {0x3001, 0x03}, // io_pad_oen
-  {0x3002, 0xf8}, // io_pad_oen
+  {0x3002, 0xfc}, // io_pad_oen
   {0x3005, 0x80}, // io_pad_out
   {0x3007, 0x01}, // io_pad_sel
   {0x3008, 0x80}, // io_pad_sel
@@ -85,6 +85,9 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
   {0x3882, 0x8}, {0x3883, 0x0D},
   {0x3836, 0x1F}, {0x3837, 0x40},
 
+  {0x3892, 0x44},
+  {0x3823, 0x48},
+
   {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI
   {0x3020, 0x05}, // SC_CTRL_20
 
@@ -179,8 +182,8 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
   {0x3820, 0x04},
 
   {0x3821, 0x19},
-  {0x3832, 0x00},
-  {0x3834, 0x00},
+  {0x3832, 0xF0},
+  {0x3834, 0xF0},
   {0x384c, 0x02},
   {0x384d, 0x0d},
   {0x3850, 0x00},