torqued: clean up (#32958)

* formatting

* function signatures didn't match

* function signatures didn't match

* filtered and raw mean something totally different when it comes to params

filtered and raw mean something totally different when it comes to params

* cmt

* probably better for organization

* add todo

* STASH

* revert some stuff

* clean up

* oof
old-commit-hash: fbc53a24a3

selfdrive/locationd/helpers.py

@@ -38,7 +38,7 @@ class PointBuckets:
   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:
+  def add_point(self, x: float, y: float) -> None:
     raise NotImplementedError
 
   def get_points(self, num_points: int = None) -> Any:

selfdrive/locationd/torqued.py

@@ -120,7 +120,8 @@ class TorqueEstimator(ParameterEstimator):
     for param in initial_params:
       self.filtered_params[param] = FirstOrderFilter(initial_params[param], self.decay, DT_MDL)
 
-  def get_restore_key(self, CP, version):
+  @staticmethod
+  def get_restore_key(CP, version):
     a, b = None, None
     if CP.lateralTuning.which() == 'torque':
       a = CP.lateralTuning.torque.friction
@@ -167,8 +168,11 @@ class TorqueEstimator(ParameterEstimator):
       self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer)
     elif which == "carState":
       self.raw_points["carState_t"].append(t + self.lag)
+      # TODO: check if high aEgo affects resulting lateral accel
       self.raw_points["vego"].append(msg.vEgo)
       self.raw_points["steer_override"].append(msg.steeringPressed)
+
+    # calculate lateral accel from past steering torque
     elif which == "liveLocationKalman":
       if len(self.raw_points['steer_torque']) == self.hist_len:
         yaw_rate = msg.angularVelocityCalibrated.value[2]