From fbc53a24a3b1977814b50c4021ba9c445c8f7f74 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 9 Jul 2024 22:26:58 -0700 Subject: [PATCH] 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 --- selfdrive/locationd/helpers.py | 2 +- selfdrive/locationd/torqued.py | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index 786bdbbfec..8f7211aebf 100644 --- a/selfdrive/locationd/helpers.py +++ b/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: diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index cb10fa93ab..ba649b318d 100755 --- a/selfdrive/locationd/torqued.py +++ b/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]