mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-17 23:32:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

selfdrived: feed PoseCalibrator with updates (#35893)

Browse Source 
this is also slow
This commit is contained in:
Shane Smiskol
2025-08-02 00:09:54 -07:00
committed by GitHub
parent 07909906d4
commit 5a8e3470ff
1 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/selfdrived/selfdrived.py
+13 -9
View File
@@ -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.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)
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
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)