From 5a8e3470ff345905645b74e4b2edd6976d9de614 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 2 Aug 2025 00:09:54 -0700 Subject: [PATCH] selfdrived: feed PoseCalibrator with updates (#35893) this is also slow --- selfdrive/selfdrived/selfdrived.py | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 94ba1b84b2..96077414de 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -43,10 +43,8 @@ SafetyModel = car.CarParams.SafetyModel IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) -def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibrator: PoseCalibrator, counter: int) -> tuple[int, bool]: +def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibrated_pose: Pose, counter: int) -> tuple[int, bool]: # CS.aEgo can be noisy to bumps in the road, transitioning from standstill, losing traction, etc. - device_pose = Pose.from_live_pose(sm['livePose']) - calibrated_pose = calibrator.build_calibrated_pose(device_pose) accel_calibrated = calibrated_pose.acceleration.x # livePose acceleration can be noisy due to bad mounting or aliased livePose measurements @@ -73,7 +71,9 @@ class SelfdriveD: self.CP = CP self.car_events = CarSpecificEvents(self.CP) - self.calibrator = PoseCalibrator() + + self.pose_calibrator = PoseCalibrator() + self.calibrated_pose: Pose | None = None # Setup sockets self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents']) @@ -251,12 +251,16 @@ class SelfdriveD: # Check for excessive (longitudinal) actuation if self.sm.updated['liveCalibration']: - self.calibrator.feed_live_calib(self.sm['liveCalibration']) - - self.excessive_actuation_counter, excessive_actuation = check_excessive_actuation(self.sm, CS, self.calibrator, self.excessive_actuation_counter) - if not self.excessive_actuation and excessive_actuation: - set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text="longitudinal") - self.excessive_actuation = True + self.pose_calibrator.feed_live_calib(self.sm['liveCalibration']) + if self.sm.updated['livePose']: + device_pose = Pose.from_live_pose(self.sm['livePose']) + self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) + + if self.calibrated_pose is not None: + self.excessive_actuation_counter, excessive_actuation = check_excessive_actuation(self.sm, CS, self.calibrated_pose, self.excessive_actuation_counter) + if not self.excessive_actuation and excessive_actuation: + set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text="longitudinal") + self.excessive_actuation = True if self.excessive_actuation: self.events.add(EventName.excessiveActuation)