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