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]