From af8e38f95760f16ebb977d44e3f616d8c26c71d0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 12 Jul 2025 05:08:37 -0700 Subject: [PATCH] this is a terrible experience just to get lat accel --- selfdrive/selfdrived/selfdrived.py | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index c07db7332c..91f76fd8e4 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -2,6 +2,7 @@ import os import time import threading +import numpy as np import cereal.messaging as messaging @@ -17,6 +18,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.gps import get_gps_location_service from openpilot.selfdrive.car.car_specific import CarSpecificEvents +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.selfdrive.selfdrived.events import Events, ET from openpilot.selfdrive.selfdrived.state import StateMachine from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert @@ -41,8 +43,15 @@ SafetyModel = car.CarParams.SafetyModel IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) -def check_lateral_iso_violation(car_control, controls_state, car_state, live_parameters): - roll_compensated_lateral_accel = controls_state.curvature * car_state.vEgo ** 2 - live_parameters.roll * ACCELERATION_DUE_TO_GRAVITY +def check_lateral_iso_violation(car_control, controls_state, car_state, live_parameters, live_calibration, live_pose, calibrator): + calibrator.feed_live_calib(live_calibration) + device_pose = Pose.from_live_pose(live_pose) + calibrated_pose = calibrator.build_calibrated_pose(device_pose) + + yaw_rate = calibrated_pose.angular_velocity.yaw + roll = device_pose.orientation.roll + + roll_compensated_lateral_accel = float((car_state.vEgo * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)) print('roll_compensated_lateral_accel:', roll_compensated_lateral_accel) @@ -129,6 +138,7 @@ class SelfdriveD: self.recalibrating_seen = False self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) + self.calibrator = PoseCalibrator() # some comma three with NVMe experience NVMe dropouts mid-drive that # cause loggerd to crash on write, so ignore it only on that platform @@ -245,7 +255,8 @@ class SelfdriveD: # Check for lateral ISO violations if not self.lateral_iso_violation and check_lateral_iso_violation(self.sm['carControl'], self.sm['controlsState'], - CS, self.sm['liveParameters']): + CS, self.sm['liveParameters'], self.sm['liveCalibration'], + self.sm['livePose'], self.calibrator): set_offroad_alert("Offroad_LateralIsoViolation", True) self.lateral_iso_violation = True