Update

cereal/custom.capnp

@@ -11,11 +11,12 @@ using Car = import "car.capnp";
 
 # you can rename the struct, but don't change the identifier
 struct FrogPilotCarControl @0x81c2f05a394cf4af {
-  alwaysOnLateralActive @0 :Bool;
-  fcwEventTriggered @1 :Bool;
-  noEntryEventTriggered @2 :Bool;
-  resumePressed @3 :Bool;
-  steerSaturatedEventTriggered @4 :Bool;
+  accelPressed @0 :Bool;
+  alwaysOnLateralActive @1 :Bool;
+  decelPressed @2 :Bool;
+  fcwEventTriggered @3 :Bool;
+  noEntryEventTriggered @4 :Bool;
+  steerSaturatedEventTriggered @5 :Bool;
 }
 
 struct FrogPilotCarState @0xaedffd8f31e7b55d {

cereal/log.capnp

@@ -618,11 +618,11 @@ struct RadarState @0x9a185389d6fdd05f {
 
   leadOne @3 :LeadData;
   leadTwo @4 :LeadData;
-  leadLeft @13 :LeadData;
-  leadRight @14 :LeadData;
-  leadLeftFar @15 :LeadData;
-  leadRightFar @16 :LeadData;
-  leadsLead @17 :LeadData;
+  leadFar @13 :LeadData;
+  leadLeft @14 :LeadData;
+  leadRight @15 :LeadData;
+  leadLeftFar @16 :LeadData;
+  leadRightFar @17 :LeadData;
   cumLagMs @5 :Float32;
 
   struct LeadData {

common/params.cc

@@ -234,7 +234,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"AutomaticallyUpdateModels", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"AutomaticUpdates", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_OTHER},
     {"AvailableModels", PERSISTENT},
-    {"AvailableModelsNames", PERSISTENT},
+    {"AvailableModelNames", PERSISTENT},
     {"BigMap", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"BlacklistedModels", PERSISTENT | FROGPILOT_CONTROLS},
     {"BlindSpotMetrics", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
@@ -264,12 +264,12 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CEStatus", CLEAR_ON_OFFROAD_TRANSITION},
     {"CEStoppedLead", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"ClassicModels", PERSISTENT},
+    {"ClippedCurvatureModels", PERSISTENT},
     {"ClusterOffset", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES},
     {"ColorToDownload", PERSISTENT},
     {"Compass", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"ConditionalExperimental", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"CrosstrekTorque", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES},
-    {"CurrentHolidayTheme", CLEAR_ON_MANAGER_START},
     {"CurveSensitivity", PERSISTENT | FROGPILOT_CONTROLS},
     {"CurveSpeedControl", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"CustomAlerts", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
@@ -283,6 +283,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CustomSounds", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"CustomUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"DecelerationProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
+    {"DesiredCurvatureModels", PERSISTENT},
     {"DeveloperUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"DeviceManagement", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"DeviceShutdown", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
@@ -385,10 +386,10 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"MinimumBackupSize", PERSISTENT},
     {"MinimumLaneChangeSpeed", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"Model", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
+    {"ModelDrivesAndScores", PERSISTENT | FROGPILOT_CONTROLS},
     {"ModelDownloadProgress", PERSISTENT},
     {"ModelName", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"ModelRandomizer", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
-    {"ModelsDownloaded", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"ModelToDownload", PERSISTENT},
     {"ModelUI", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"MTSCCurvatureCheck", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
@@ -450,7 +451,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RelaxedJerkSpeed", PERSISTENT | FROGPILOT_CONTROLS},
     {"RelaxedJerkSpeedDecrease", PERSISTENT | FROGPILOT_CONTROLS},
     {"RelaxedPersonalityProfile", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
-    {"ResetFrogTheme", PERSISTENT},
     {"ReverseCruise", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS},
     {"RoadEdgesWidth", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VISUALS},
     {"RoadName", PERSISTENT},

selfdrive/car/toyota/carcontroller.py

@@ -51,10 +51,13 @@ def get_long_tune(CP, params):
   kiBP = [0.]
   kdBP = [0.]
   kdV = [0.]
-  if CP.carFingerprint in TSS2_CAR:
+
+  if CP.enableGasInterceptor:
+    kiBP = [0., 5., 20.]
+    kiV = [1.3, 1.0, 0.7]
+  elif CP.carFingerprint in TSS2_CAR:
     kiV = [0.25]
     kdV = [0.25 / 4]
-
   else:
     kiBP = [0., 5., 35.]
     kiV = [3.6, 2.4, 1.5]

selfdrive/classic_modeld/classic_modeld.py

@@ -312,7 +312,7 @@ def main(demo=False):
       'traffic_convention': traffic_convention,
       'lateral_control_params': lateral_control_params,
       **({'nav_features': nav_features, 'nav_instructions': nav_instructions} if enable_navigation else {}),
-      **({'radar_tracks': radar_tracks,} if radarless else {}),
+      **({'radar_tracks': radar_tracks} if radarless else {}),
     }
 
     mt1 = time.perf_counter()

selfdrive/classic_modeld/models/README.md

@@ -0,0 +1,62 @@
+## Neural networks in openpilot
+To view the architecture of the ONNX networks, you can use [netron](https://netron.app/)
+
+## Supercombo
+### Supercombo input format (Full size: 799906 x float32)
+* **image stream**
+  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
+    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
+      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
+      * Channel 4 represents the half-res U channel
+      * Channel 5 represents the half-res V channel
+* **wide image stream**
+  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
+    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
+      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
+      * Channel 4 represents the half-res U channel
+      * Channel 5 represents the half-res V channel
+* **desire**
+  * one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8
+* **traffic convention**
+  * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
+* **feature buffer**
+  * A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512
+
+
+### Supercombo output format (Full size: XXX x float32)
+Read [here](https://github.com/commaai/openpilot/blob/90af436a121164a51da9fa48d093c29f738adf6a/selfdrive/modeld/models/driving.h#L236) for more.
+
+
+## Driver Monitoring Model
+* .onnx model can be run with onnx runtimes
+* .dlc file is a pre-quantized model and only runs on qualcomm DSPs
+
+### input format
+* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format:
+  * full input size is 1440 * 960 = 1382400
+  * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner)
+* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs
+
+### output format
+* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33))
+  * for each person in the front seats (2 * 41)
+    * face pose: 12 = 6 + 6
+      * face orientation [pitch, yaw, roll] in camera frame: 3
+      * face position [dx, dy] relative to image center: 2
+      * normalized face size: 1
+      * standard deviations for above outputs: 6
+    * face visible probability: 1
+    * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1
+      * eye position and size, and their standard deviations: 8
+      * eye visible probability: 1
+      * eye closed probability: 1
+    * wearing sunglasses probability: 1
+    * face occluded probability: 1
+    * touching wheel probability: 1
+    * paying attention probability: 1
+    * (deprecated) distracted probabilities: 2
+    * using phone probability: 1
+    * distracted probability: 1
+  * common outputs 2
+    * poor camera vision probability: 1
+    * left hand drive probability: 1

selfdrive/modeld/navmodeld.py → selfdrive/classic_modeld/navmodeld.py

@@ -12,8 +12,8 @@
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.realtime import set_realtime_priority
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
+from openpilot.selfdrive.classic_modeld.constants import ModelConstants
+from openpilot.selfdrive.classic_modeld.runners import ModelRunner, Runtime
 
 NAV_INPUT_SIZE = 256*256
 NAV_FEATURE_LEN = 256
@@ -81,12 +81,6 @@ def main():
   gc.disable()
   set_realtime_priority(1)
 
-  # there exists a race condition when two processes try to create a
-  # SNPE model runner at the same time, wait for dmonitoringmodeld to finish
-  cloudlog.warning("waiting for dmonitoringmodeld to initialize")
-  if not Params().get_bool("DmModelInitialized", True):
-    return
-
   model = ModelState()
   cloudlog.warning("models loaded, navmodeld starting")
 

selfdrive/controls/controlsd.py

@@ -185,15 +185,16 @@ def __init__(self, CI=None):
     # FrogPilot variables
     self.frogpilot_toggles = get_frogpilot_toggles()
 
+    self.accel_pressed = False
     self.always_on_lateral_active = False
     self.always_on_lateral_active_previously = False
+    self.decel_pressed = False
     self.fcw_event_triggered = False
     self.no_entry_alert_triggered = False
     self.onroad_distance_pressed = False
-    self.resume_pressed = False
-    self.resume_previously_pressed = False
     self.steer_saturated_event_triggered = False
 
+    self.clip_curves = self.frogpilot_toggles.clipped_curvature_model
     self.radarless_model = self.frogpilot_toggles.radarless_model
     self.use_old_long = self.frogpilot_toggles.old_long_api
 
@@ -234,8 +235,8 @@ def update_events(self, CS):
       return
 
     # Block resume if cruise never previously enabled
-    self.resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
-    if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and self.resume_pressed:
+    resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
+    if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed:
       self.events.add(EventName.resumeBlocked)
 
     if not self.CP.notCar:
@@ -632,7 +633,7 @@ def state_control(self, CS):
         actuators.speed = long_plan.speeds[-1]
 
       # Steering PID loop and lateral MPC
-      self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature)
+      self.desired_curvature = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature, self.clip_curves)
       actuators.curvature = self.desired_curvature
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                              self.steer_limited, self.desired_curvature,
@@ -744,14 +745,18 @@ def update_frogpilot_variables(self, CS):
           self.experimental_mode = not self.experimental_mode
           self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
 
-    if self.sm.frame * DT_CTRL % DT_MDL == 0 or self.resume_pressed:
-      self.resume_previously_pressed = self.resume_pressed
+    if self.sm.updated['frogpilotPlan'] or any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents):
+      self.accel_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
+
+    if self.sm.updated['frogpilotPlan'] or any(be.type == ButtonType.decelCruise for be in CS.buttonEvents):
+      self.decel_pressed = any(be.type == ButtonType.decelCruise for be in CS.buttonEvents)
 
     FPCC = custom.FrogPilotCarControl.new_message()
     FPCC.alwaysOnLateralActive = self.always_on_lateral_active
+    FPCC.accelPressed = self.accel_pressed
+    FPCC.decelPressed = self.decel_pressed
     FPCC.fcwEventTriggered = self.fcw_event_triggered
     FPCC.noEntryEventTriggered = self.no_entry_alert_triggered
-    FPCC.resumePressed = self.resume_previously_pressed
     FPCC.steerSaturatedEventTriggered = self.steer_saturated_event_triggered
 
     return FPCC

selfdrive/controls/lib/drive_helpers.py

@@ -18,6 +18,8 @@
 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
 
 # EU guidelines
 MAX_LATERAL_JERK = 5.0
@@ -177,7 +179,9 @@ def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
 
-def clip_curvature(v_ego, prev_curvature, new_curvature):
+def clip_curvature(v_ego, prev_curvature, new_curvature, clip_curves):
+  if clip_curves:
+    new_curvature = clip(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
   v_ego = max(MIN_SPEED, v_ego)
   max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
   safe_desired_curvature = clip(new_curvature,

selfdrive/controls/lib/events.py

@@ -380,7 +380,7 @@ def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
 
 def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, frogpilot_toggles: SimpleNamespace) -> Alert:
   model_name = params.get("NNFFModelName", encoding='utf-8')
-  if model_name == "":
+  if model_name is None:
     return Alert(
       "NNFF Torque Controller not available",
       "Donate logs to Twilsonco to get your car supported!",

selfdrive/controls/lib/longitudinal_planner.py

@@ -52,24 +52,20 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
-def get_accel_from_plan(CP, speeds, accels):
+def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
   if len(speeds) == CONTROL_N:
-    v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
-    a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels)
+    v_now = speeds[0]
+    a_now = accels[0]
 
-    v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
-    if v_target != v_target_now:
-      a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
-    else:
-      a_target = a_target_now
-
-    v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
+    v_target = interp(action_t, CONTROL_N_T_IDX, speeds)
+    a_target = 2 * (v_target - v_now) / (action_t) - a_now
+    v_target_1sec = interp(action_t + 1.0, CONTROL_N_T_IDX, speeds)
   else:
     v_target = 0.0
     v_target_1sec = 0.0
     a_target = 0.0
-  should_stop = (v_target < CP.vEgoStopping and
-                 v_target_1sec < CP.vEgoStopping)
+  should_stop = (v_target < vEgoStopping and
+                 v_target_1sec < vEgoStopping)
   return a_target, should_stop
 
 
@@ -295,7 +291,9 @@ def publish(self, sm, pm):
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
-    a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels)
+    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+    a_target, should_stop = get_accel_from_plan(longitudinalPlan.speeds, longitudinalPlan.accels,
+                                                action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
     longitudinalPlan.aTarget = a_target
     longitudinalPlan.shouldStop = should_stop
     longitudinalPlan.allowBrake = True

selfdrive/controls/radard.py

@@ -221,13 +221,15 @@ def get_lead_adjacent(tracks: dict[int, Track], model_data: capnp._DynamicStruct
   return lead_dict
 
 
-def get_leads_lead(tracks: dict[int, Track], leadOne) -> dict[str, Any]:
+def get_lead_far(tracks: dict[int, Track], leadOne) -> dict[str, Any]:
   lead_dict = {'status': False}
 
-  toyota_corolla_length = 4.6355
-  leads_lead_tracks = [c for c in tracks.values() if leadOne.status and c.dRel > leadOne.dRel + toyota_corolla_length]
-  if len(leads_lead_tracks) > 0:
-    closest_track = min(leads_lead_tracks, key=lambda c: c.dRel)
+  if leadOne.status:
+    return lead_dict
+
+  far_lead_tracks = [c for c in tracks.values() if c.vLead > 1 and abs(c.yRel) < 1.5]
+  if len(far_lead_tracks) > 0:
+    closest_track = min(far_lead_tracks, key=lambda c: c.dRel)
     lead_dict = closest_track.get_RadarState()
 
   return lead_dict
@@ -312,11 +314,11 @@ def update(self, sm: messaging.SubMaster, rr):
       self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.frogpilot_toggles, sm['frogpilotCarState'], low_speed_override=False)
 
     if self.frogpilot_toggles.adjacent_lead_tracking and self.ready:
+      self.radar_state.leadFar = get_lead_far(self.tracks, self.radar_state.leadOne)
       self.radar_state.leadLeft = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True)
       self.radar_state.leadLeftFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthLeft, left=True, far=True)
       self.radar_state.leadRight = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False)
       self.radar_state.leadRightFar = get_lead_adjacent(self.tracks, sm['modelV2'], sm['frogpilotPlan'].laneWidthRight, left=False, far=True)
-      self.radar_state.leadsLead = get_leads_lead(self.tracks, self.radar_state.leadOne)
 
     # Update FrogPilot parameters
     if sm['frogpilotPlan'].togglesUpdated: