From bfa3f3cccbfae6cdd4fab00e6d12c3872f59def0 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Tue, 10 Jun 2025 00:22:13 -0700
Subject: [PATCH] Add calPerc progress tracking for torque calibration (#35512)

* Add calPerc field and torque progress

* Fix torqued test style and CarParams usage

* test: remove unused numpy import

* move here

* trying all combinations to see what's most linear

* clean up with best method

* no no

* epic

* clean up

* last min not needed

* doesn't hurt

* list comp
---
 cereal/log.capnp                         |  1 +
 selfdrive/locationd/helpers.py           |  6 ++++++
 selfdrive/locationd/test/test_torqued.py | 25 ++++++++++++++++++++++++
 selfdrive/locationd/torqued.py           |  1 +
 selfdrive/test/process_replay/ref_commit |  2 +-
 5 files changed, 34 insertions(+), 1 deletion(-)
 create mode 100644 selfdrive/locationd/test/test_torqued.py

diff --git a/cereal/log.capnp b/cereal/log.capnp
index 102118c3b6..c569eeeaf8 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -2281,6 +2281,7 @@ struct LiveTorqueParametersData {
   points @10 :List(List(Float32));
   version @11 :Int32;
   useParams @12 :Bool;
+  calPerc @13 :Int8;
 }
 
 struct LiveDelayData {
diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py
index a3e3ae4a8c..bf4588a40c 100644
--- a/selfdrive/locationd/helpers.py
+++ b/selfdrive/locationd/helpers.py
@@ -82,6 +82,12 @@ class PointBuckets:
     total_points_valid = self.__len__() >= self.min_points_total
     return individual_buckets_valid and total_points_valid
 
+  def get_valid_percent(self) -> int:
+    total_points_perc = min(self.__len__() / self.min_points_total * 100, 100)
+    individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in
+                                      zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100)
+    return int((total_points_perc + individual_buckets_perc) / 2)
+
   def is_calculable(self) -> bool:
     return all(len(v) > 0 for v in self.buckets.values())
 
diff --git a/selfdrive/locationd/test/test_torqued.py b/selfdrive/locationd/test/test_torqued.py
new file mode 100644
index 0000000000..53f3120c36
--- /dev/null
+++ b/selfdrive/locationd/test/test_torqued.py
@@ -0,0 +1,25 @@
+from cereal import car
+from openpilot.selfdrive.locationd.torqued import TorqueEstimator
+
+
+def test_cal_percent():
+  est = TorqueEstimator(car.CarParams())
+  msg = est.get_msg()
+  assert msg.liveTorqueParameters.calPerc == 0
+
+  for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
+                                  est.filtered_points.buckets_min_points.values(), strict=True):
+    for _ in range(int(min_pts)):
+      est.filtered_points.add_point((low + high) / 2.0, 0.0)
+
+  # enough bucket points, but not enough total points
+  msg = est.get_msg()
+  assert msg.liveTorqueParameters.calPerc == (len(est.filtered_points) / est.min_points_total * 100 + 100) / 2
+
+  # add enough points to bucket with most capacity
+  key = list(est.filtered_points.buckets)[0]
+  for _ in range(est.min_points_total - len(est.filtered_points)):
+    est.filtered_points.add_point((key[0] + key[1]) / 2.0, 0.0)
+
+  msg = est.get_msg()
+  assert msg.liveTorqueParameters.calPerc == 100
diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py
index 5ed86e457f..23bd99931b 100755
--- a/selfdrive/locationd/torqued.py
+++ b/selfdrive/locationd/torqued.py
@@ -233,6 +233,7 @@ class TorqueEstimator(ParameterEstimator):
     liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x)
     liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x)
     liveTorqueParameters.totalBucketPoints = len(self.filtered_points)
+    liveTorqueParameters.calPerc = self.filtered_points.get_valid_percent()
     liveTorqueParameters.decay = self.decay
     liveTorqueParameters.maxResets = self.resets
     return msg
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index e692f56eba..8e58f9a27b 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-9e2fe2942fbf77f24bccdbef15893831f9c0b390
\ No newline at end of file
+1f5f2708a82fb8a1b68f23dd2eb246c0b1d0b47d
\ No newline at end of file