torqued: log raw params if calculable (#31521)

* log params when calculable

* better

* Update ref_commit

* this is redundant

* this is only used in one place, confusing which to use so remove

* better

selfdrive/locationd/helpers.py

@@ -27,17 +27,17 @@ class PointBuckets:
     self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
     self.min_points_total = min_points_total
 
-  def bucket_lengths(self) -> List[int]:
-    return [len(v) for v in self.buckets.values()]
-
   def __len__(self) -> int:
-    return sum(self.bucket_lengths())
+    return sum([len(v) for v in self.buckets.values()])
 
   def is_valid(self) -> bool:
     individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True))
     total_points_valid = self.__len__() >= self.min_points_total
     return individual_buckets_valid and total_points_valid
 
+  def is_calculable(self) -> bool:
+    return all(len(v) > 0 for v in self.buckets.values())
+
   def add_point(self, x: float, y: float, bucket_val: float) -> None:
     raise NotImplementedError
 

selfdrive/locationd/torqued.py

@@ -184,23 +184,23 @@ class TorqueEstimator(ParameterEstimator):
     liveTorqueParameters.version = VERSION
     liveTorqueParameters.useParams = self.use_params
 
-    if self.filtered_points.is_valid():
+    # Calculate raw estimates when possible, only update filters when enough points are gathered
+    if self.filtered_points.is_calculable():
       latAccelFactor, latAccelOffset, frictionCoeff = self.estimate_params()
       liveTorqueParameters.latAccelFactorRaw = float(latAccelFactor)
       liveTorqueParameters.latAccelOffsetRaw = float(latAccelOffset)
       liveTorqueParameters.frictionCoefficientRaw = float(frictionCoeff)
 
-      if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]):
-        cloudlog.exception("Live torque parameters are invalid.")
-        liveTorqueParameters.liveValid = False
-        self.reset()
-      else:
-        liveTorqueParameters.liveValid = True
-        latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor)
-        frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction)
-        self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff})
-    else:
-      liveTorqueParameters.liveValid = False
+      if self.filtered_points.is_valid():
+        if any(val is None or np.isnan(val) for val in [latAccelFactor, latAccelOffset, frictionCoeff]):
+          cloudlog.exception("Live torque parameters are invalid.")
+          liveTorqueParameters.liveValid = False
+          self.reset()
+        else:
+          liveTorqueParameters.liveValid = True
+          latAccelFactor = np.clip(latAccelFactor, self.min_lataccel_factor, self.max_lataccel_factor)
+          frictionCoeff = np.clip(frictionCoeff, self.min_friction, self.max_friction)
+          self.update_params({'latAccelFactor': latAccelFactor, 'latAccelOffset': latAccelOffset, 'frictionCoefficient': frictionCoeff})
 
     if with_points:
       liveTorqueParameters.points = self.filtered_points.get_points()[:, [0, 2]].tolist()

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-1b16593e2d0c9ff2dae2916293ae5fbc7d6f26df
+d0cdea7eb15f3cac8a921f7ace3eaa6baebb4fd5