test: sync core controls and location updates

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

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

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)
 

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)

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])
 

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

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

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": "<b>Unsupported branch detected</b> - The current version of <b><u>%1</u></b> branch is no longer supported on the comma three. Please go to <b>[Device > Software]</b> and install a supported branch with <b><u>-tici</u></b> 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"
   }
 }

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),
+  },
 }