From dcc8fe078d64fa7ea6abcff3aac786aec5905e2d Mon Sep 17 00:00:00 2001 From: 1okko <15377594951@189.cn> Date: Wed, 21 Oct 2026 19:55:20 +0800 Subject: [PATCH] test: sync core controls and location updates --- .../selfdrive/controls/lib/drive_helpers.py | 1 - .../controls/lib/latcontrol_curvature.py | 11 ++++++++-- openpilot/selfdrive/controls/plannerd.py | 2 +- .../selfdrive/locationd/models/car_kf.py | 9 ++++---- .../selfdrive/locationd/models/pose_kf.py | 2 +- openpilot/selfdrive/locationd/paramsd.py | 7 +++---- openpilot/selfdrive/monitoring/policy.py | 5 +++++ .../selfdrive/selfdrived/alerts_offroad.json | 21 +++++++++++++------ openpilot/selfdrive/selfdrived/events.py | 6 +++++- 9 files changed, 44 insertions(+), 20 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/drive_helpers.py b/openpilot/selfdrive/controls/lib/drive_helpers.py index 35ca45e56..c350cff63 100644 --- a/openpilot/selfdrive/controls/lib/drive_helpers.py +++ b/openpilot/selfdrive/controls/lib/drive_helpers.py @@ -4,7 +4,6 @@ from openpilot.common.realtime import DT_CTRL, DT_MDL MIN_SPEED = 1.0 CONTROL_N = 17 -CAR_ROTATION_RADIUS = 0.0 # This is a turn radius smaller than most cars can achieve MAX_CURVATURE = 0.2 MIN_STABLE_DELAY = 0.3 diff --git a/openpilot/selfdrive/controls/lib/latcontrol_curvature.py b/openpilot/selfdrive/controls/lib/latcontrol_curvature.py index 3dfe691e1..efa073460 100644 --- a/openpilot/selfdrive/controls/lib/latcontrol_curvature.py +++ b/openpilot/selfdrive/controls/lib/latcontrol_curvature.py @@ -1,11 +1,13 @@ import math +import numpy as np +from collections import deque from openpilot.cereal import log from openpilot.common.pid import PIDController from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE -CURVATURE_SATURATION_THRESHOLD = 1e-3 # 1/m +LAT_ACCEL_SATURATION_THRESHOLD = 0.5 # m/s^2 class LatControlCurvature(LatControl): @@ -20,6 +22,8 @@ class LatControlCurvature(LatControl): else: self.pid = None self.kf = 1. + self.desired_curvature_buffer_len = int(1. / self.dt) + self.desired_curvature_buffer = deque([0.] * self.desired_curvature_buffer_len, maxlen=self.desired_curvature_buffer_len) def reset(self): super().reset() @@ -30,6 +34,9 @@ class LatControlCurvature(LatControl): curvature_log = log.ControlsState.LateralCurvatureState.new_message() actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) error = desired_curvature - actual_curvature + self.desired_curvature_buffer.append(desired_curvature) + delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.desired_curvature_buffer_len)) + expected_curvature = self.desired_curvature_buffer[-delay_frames] if not active: output_curvature = 0.0 @@ -53,6 +60,6 @@ class LatControlCurvature(LatControl): curvature_log.actualCurvature = float(actual_curvature) curvature_log.desiredCurvature = float(desired_curvature) curvature_log.output = float(output_curvature) - curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS, + curvature_log.saturated = bool(self._check_saturation(abs(expected_curvature - actual_curvature) * CS.vEgo ** 2 > LAT_ACCEL_SATURATION_THRESHOLD, CS, False, curvature_limited)) return 0.0, float(output_curvature), curvature_log diff --git a/openpilot/selfdrive/controls/plannerd.py b/openpilot/selfdrive/controls/plannerd.py index e7f36d324..645a6e8c8 100755 --- a/openpilot/selfdrive/controls/plannerd.py +++ b/openpilot/selfdrive/controls/plannerd.py @@ -28,7 +28,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP, CP_SP) pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) - sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState', + sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'selfdriveState', 'liveMapDataSP', 'carStateSP', 'selfdriveStateSP', gps_location_service], poll='modelV2', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services) diff --git a/openpilot/selfdrive/locationd/models/car_kf.py b/openpilot/selfdrive/locationd/models/car_kf.py index 27cc4ef9c..9bcec919d 100755 --- a/openpilot/selfdrive/locationd/models/car_kf.py +++ b/openpilot/selfdrive/locationd/models/car_kf.py @@ -80,7 +80,6 @@ class CarKalman(KalmanFilter): } global_vars = [ - 'mass', 'rotational_inertia', 'center_to_front', 'center_to_rear', @@ -99,7 +98,10 @@ class CarKalman(KalmanFilter): # globals global_vars = [sp.Symbol(name) for name in CarKalman.global_vars] - m, j, aF, aR, cF_orig, cR_orig = global_vars + j, aF, aR, cF_orig, cR_orig = global_vars + # Tire stiffness and rotational inertia are already divided by vehicle mass, which cancels out of the model. + # Use unit mass to preserve the standard bicycle-model equations below. + m = 1 # make functions and jacobians with sympy # state variables @@ -166,8 +168,7 @@ class CarKalman(KalmanFilter): self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial, dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog) - def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear): - self.filter.set_global("mass", mass) + def set_globals(self, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear): self.filter.set_global("rotational_inertia", rotational_inertia) self.filter.set_global("center_to_front", center_to_front) self.filter.set_global("center_to_rear", center_to_rear) diff --git a/openpilot/selfdrive/locationd/models/pose_kf.py b/openpilot/selfdrive/locationd/models/pose_kf.py index a8ff80c71..f71503c20 100755 --- a/openpilot/selfdrive/locationd/models/pose_kf.py +++ b/openpilot/selfdrive/locationd/models/pose_kf.py @@ -40,7 +40,7 @@ class PoseKalman(KalmanFilter): initial_P = np.diag([0.01**2, 0.01**2, 0.01**2, 10**2, 10**2, 10**2, 1**2, 1**2, 1**2, - 1**2, 1**2, 1**2, + 0.1**2, 0.1**2, 0.1**2, 100**2, 100**2, 100**2, 0.01**2, 0.01**2, 0.01**2]) diff --git a/openpilot/selfdrive/locationd/paramsd.py b/openpilot/selfdrive/locationd/paramsd.py index 760176bc5..18215c67b 100755 --- a/openpilot/selfdrive/locationd/paramsd.py +++ b/openpilot/selfdrive/locationd/paramsd.py @@ -36,12 +36,11 @@ class VehicleParamsLearner: self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial self.kf.set_globals( - mass=CP.mass, - rotational_inertia=CP.rotationalInertia, + rotational_inertia=CP.rotationalInertiaPerMass, center_to_front=CP.centerToFront, center_to_rear=CP.wheelbase - CP.centerToFront, - stiffness_front=CP.tireStiffnessFront, - stiffness_rear=CP.tireStiffnessRear + stiffness_front=CP.tireStiffnessFrontPerMass, + stiffness_rear=CP.tireStiffnessRearPerMass ) self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio diff --git a/openpilot/selfdrive/monitoring/policy.py b/openpilot/selfdrive/monitoring/policy.py index bd698214a..57c83683e 100644 --- a/openpilot/selfdrive/monitoring/policy.py +++ b/openpilot/selfdrive/monitoring/policy.py @@ -51,6 +51,7 @@ class DRIVER_MONITOR_SETTINGS: self._SG_THRESHOLD = 0.9 self._BLINK_THRESHOLD = 0.865 self._PHONE_THRESH = 0.5 + self._SLEEP_THRESH = 0.75 self._POSE_PITCH_THRESHOLD = 0.3133 self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD @@ -138,6 +139,7 @@ class DriverMonitoring: self.pose = DriverPose(settings=self.settings) self.blink = DriverBlink() self.phone_prob = 0. + self.sleep_prob = 0. self.alert_level = AlertLevel.none self.always_on = always_on @@ -240,6 +242,7 @@ class DriverMonitoring: self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold)) self.distracted_types['eye'] = bool((self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD) self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH) + self.distracted_types['sleep'] = bool(self.sleep_prob > self.settings._SLEEP_THRESH) def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, demo_mode=False, steering_angle_deg=0.): rhd_pred = driver_state.wheelOnRightProb @@ -277,6 +280,7 @@ class DriverMonitoring: self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) self.phone_prob = driver_data.phoneProb + self.sleep_prob = driver_data.sleepProb self._get_distracted_types() self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std @@ -406,6 +410,7 @@ class DriverMonitoring: dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose'] dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye'] dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone'] + dm.visionPolicyState.distractedTypes.sleep = self.distracted_types['sleep'] dm.visionPolicyState.faceDetected = self.face_detected dm.visionPolicyState.pose.pitch = self.pose.pitch dm.visionPolicyState.pose.yaw = self.pose.yaw diff --git a/openpilot/selfdrive/selfdrived/alerts_offroad.json b/openpilot/selfdrive/selfdrived/alerts_offroad.json index b3b507782..d3f568360 100644 --- a/openpilot/selfdrive/selfdrived/alerts_offroad.json +++ b/openpilot/selfdrive/selfdrived/alerts_offroad.json @@ -21,22 +21,22 @@ "text": "Chestnut detected! Switch to the %1 branch to use chestnut-class models.", "severity": -1 }, + "Offroad_ChestnutModelError": { + "text": "Chestnut model error. Restart the car to retry.", + "severity": 0 + }, "Offroad_ChestnutNotDetected": { - "text": "Chestnut not detected. Check USB and 12V connections.", + "text": "Chestnut not detected. Check USB and 12V connections, then restart the car.", "severity": 0 }, "Offroad_ChestnutOverheated": { - "text": "Chestnut overheated. Ensure good airflow. Current GPU temperature is %1.", + "text": "Chestnut overheated. Ensure good airflow.", "severity": 0 }, "Offroad_ChestnutPcieUnavailable": { "text": "%1", "severity": 0 }, - "Offroad_ChestnutUncompiled": { - "text": "Chestnut model not compiled. Keep ignition on and reboot the comma.", - "severity": 0 - }, "Offroad_ChestnutUpdateFailed": { "text": "Chestnut update failed. Check the USB cable.", "severity": 0 @@ -49,6 +49,10 @@ "text": "Failed to register with comma.ai backend. It will not connect or upload to comma.ai servers, and receives no support from comma.ai. If this is a device purchased at comma.ai/shop, open a ticket at https://comma.ai/support.", "severity": 1 }, + "Offroad_UploadsIgnored": { + "text": "Uploads ignored due to a potential detected safety policy violation. Contact comma.ai/support if you believe this was made in error.", + "severity": 1 + }, "Offroad_CarUnrecognized": { "text": "sunnypilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please select your vehicle manually at https://www.sunnylink.ai/. Need help? Visit https://community.sunnypilot.ai/", "severity": 0 @@ -74,5 +78,10 @@ "text": "Unsupported branch detected - The current version of %1 branch is no longer supported on the comma three. Please go to [Device > Software] and install a supported branch with -tici in the branch name for the comma three.", "severity": 1, "_comment": "Set extra field to the current branch name." + }, + "Offroad_Pairing": { + "text": "Finish setup. Pair your device with comma connect to view recent drives and bookmarked routes.", + "severity": -2, + "icon": "icons_mici/settings/device/green_settings.png" } } diff --git a/openpilot/selfdrive/selfdrived/events.py b/openpilot/selfdrive/selfdrived/events.py index bddd1661e..8b54392bf 100755 --- a/openpilot/selfdrive/selfdrived/events.py +++ b/openpilot/selfdrive/selfdrived/events.py @@ -196,7 +196,7 @@ def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messagi def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: personality = str(personality).title() - return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) + return NormalPermanentAlert(f"Following Distance: {personality}", duration=1.5) def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: @@ -835,6 +835,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { EventName.userBookmark: { ET.PERMANENT: NormalPermanentAlert(tr_noop("Bookmark Saved"), duration=1.5), }, + + EventName.userBookmarkNotPaired: { + ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", "pair to connect\nto view", duration=1.5), + }, }