From 9cc2677a7aeeb5e0f8116dab18a46ff9a3c3c3de Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 21 Feb 2024 00:30:51 -0600 Subject: [PATCH] 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 old-commit-hash: c3e3103830ba62d70f45555bdd6fda554594f7a8 --- selfdrive/locationd/helpers.py | 8 ++++---- selfdrive/locationd/torqued.py | 24 ++++++++++++------------ selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index 93e2929139..c292e9886c 100644 --- a/selfdrive/locationd/helpers.py +++ b/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 diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 51418f9c1e..69bab8d1fa 100755 --- a/selfdrive/locationd/torqued.py +++ b/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() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 07ab676033..fc38f87310 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1b16593e2d0c9ff2dae2916293ae5fbc7d6f26df +d0cdea7eb15f3cac8a921f7ace3eaa6baebb4fd5