Model switcher

Added a model switcher to swap between several different models on the fly.

common/params.cc

@@ -221,6 +221,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"AlwaysOnLateralMain", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
     {"AutomaticUpdates", PERSISTENT},
+    {"AvailableModels", PERSISTENT},
+    {"AvailableModelsNames", PERSISTENT},
     {"BigMap", PERSISTENT},
     {"BlindSpotPath", PERSISTENT},
     {"CameraFPS", PERSISTENT},
@@ -303,6 +305,11 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ManualUpdateInitiated", PERSISTENT},
     {"MapTargetLatA", PERSISTENT},
     {"MapTargetVelocities", PERSISTENT},
+    {"Model", PERSISTENT},
+    {"ModelDownloadProgress", PERSISTENT},
+    {"ModelName", PERSISTENT},
+    {"ModelSelector", PERSISTENT},
+    {"ModelToDownload", PERSISTENT},
     {"ModelUI", PERSISTENT},
     {"MTSCAggressiveness", PERSISTENT},
     {"MTSCCurvatureCheck", PERSISTENT},

release/files_common

@@ -569,5 +569,6 @@ selfdrive/frogpilot/controls/frogpilot_planner.py
 selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
 selfdrive/frogpilot/controls/lib/frogpilot_functions.py
 selfdrive/frogpilot/controls/lib/map_turn_speed_controller.py
+selfdrive/frogpilot/controls/lib/model_manager.py
 selfdrive/frogpilot/controls/lib/theme_manager.py
 selfdrive/frogpilot/fleet_manager/fleet_manager.py

selfdrive/controls/controlsd.py

@@ -35,6 +35,8 @@ from openpilot.system.version import get_short_branch
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED, PROBABILITY, MovingAverageCalculator
 
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
+
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
@@ -72,6 +74,8 @@ class Controls:
     self.params_memory = Params("/dev/shm/params")
     self.params_storage = Params("/persist/params")
 
+    self.radarless_model = self.params.get("Model", block=True, encoding='utf-8') in RADARLESS_MODELS
+
     with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg:
       # TODO: this shouldn't need to be a builder
       self.CP = msg.as_builder()
@@ -93,6 +97,8 @@ class Controls:
     ignore = self.sensor_packets + ['testJoystick']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
+    if self.radarless_model:
+      ignore += ['radarState']
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
@@ -340,8 +346,9 @@ class Controls:
           self.events.add(EventName.cameraFrameRate)
     if not REPLAY and self.rk.lagging:
       self.events.add(EventName.controlsdLagging)
-    if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
-      self.events.add(EventName.radarFault)
+    if not self.radarless_model:
+      if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
+        self.events.add(EventName.radarFault)
     if not self.sm.valid['pandaStates']:
       self.events.add(EventName.usbError)
     if CS.canTimeout:
@@ -969,7 +976,7 @@ class Controls:
 
     custom_alerts = self.params.get_bool("CustomAlerts")
     self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert")
-    self.lead_departing_alert = custom_alerts and self.params.get_bool("LeadDepartingAlert")
+    self.lead_departing_alert = not self.radarless_model and custom_alerts and self.params.get_bool("LeadDepartingAlert")
     self.loud_blindspot_alert = custom_alerts and self.params.get_bool("LoudBlindspotAlert")
 
     custom_theme = self.params.get_bool("CustomTheme")

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -10,7 +10,6 @@ from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.car.interfaces import ACCEL_MIN
-from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -44,7 +43,8 @@ CRASH_DISTANCE = .25
 LEAD_DANGER_FACTOR = 0.75
 LIMIT_COST = 1e6
 ACADOS_SOLVER_TYPE = 'SQP_RTI'
-
+# Default lead acceleration decay set to 50% at 1s
+LEAD_ACCEL_TAU = 1.5
 
 # Fewer timestamps don't hurt performance and lead to
 # much better convergence of the MPC with low iterations
@@ -341,7 +341,7 @@ class LongitudinalMpc:
       x_lead = 50.0
       v_lead = v_ego + 10.0
       a_lead = 0.0
-      a_lead_tau = _LEAD_ACCEL_TAU
+      a_lead_tau = LEAD_ACCEL_TAU
 
     # MPC will not converge if immediate crash is expected
     # Clip lead distance to what is still possible to brake for
@@ -358,12 +358,12 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, t_follow, personality=log.LongitudinalPersonality.standard):
+  def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_follow, personality=log.LongitudinalPersonality.standard):
     v_ego = self.x0[1]
-    self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+    self.status = lead_one.status or lead_two.status
 
-    lead_xv_0 = self.process_lead(radarstate.leadOne, self.increased_stopping_distance)
-    lead_xv_1 = self.process_lead(radarstate.leadTwo)
+    lead_xv_0 = self.process_lead(lead_one, self.increased_stopping_distance)
+    lead_xv_1 = self.process_lead(lead_two)
 
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
@@ -422,8 +422,8 @@ class LongitudinalMpc:
     self.params[:,4] = t_follow
 
     self.run()
-    if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
-            radarstate.leadOne.modelProb > 0.9):
+    lead_probability = lead_one.prob if radarless_model else lead_one.modelProb
+    if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and lead_probability > 0.9):
       self.crash_cnt += 1
     else:
       self.crash_cnt = 0

selfdrive/controls/lib/longitudinal_planner.py

@@ -6,14 +6,18 @@ from openpilot.common.numpy_fast import clip, interp
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
+from openpilot.common.simple_kalman import KF1D
 from openpilot.common.realtime import DT_MDL
+from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC, LEAD_ACCEL_TAU
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
-from openpilot.common.swaglog import cloudlog
+
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
@@ -24,6 +28,9 @@ A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
+# Kalman filter states enum
+LEAD_KALMAN_SPEED, LEAD_KALMAN_ACCEL = 0, 1
+
 
 def get_max_accel(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
@@ -44,6 +51,72 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
+def lead_kf(v_lead: float, dt: float = 0.05):
+  # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
+  # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
+  assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
+  A = [[1.0, dt], [0.0, 1.0]]
+  C = [1.0, 0.0]
+  #Q = np.matrix([[10., 0.0], [0.0, 100.]])
+  #R = 1e3
+  #K = np.matrix([[ 0.05705578], [ 0.03073241]])
+  dts = [dt * 0.01 for dt in range(1, 21)]
+  K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689,  0.21372394,
+        0.22761098, 0.24069424, 0.253096,   0.26491023, 0.27621103, 0.28705801,
+        0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
+        0.35353899, 0.36200124]
+  K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
+        0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
+        0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
+        0.26393339, 0.26278425]
+  K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
+
+  kf = KF1D([[v_lead], [0.0]], A, C, K)
+  return kf
+
+
+class Lead:
+  def __init__(self):
+    self.dRel = 0.0
+    self.yRel = 0.0
+    self.vLead = 0.0
+    self.aLead = 0.0
+    self.vLeadK = 0.0
+    self.aLeadK = 0.0
+    self.aLeadTau = LEAD_ACCEL_TAU
+    self.prob = 0.0
+    self.status = False
+
+    self.kf: KF1D | None = None
+
+  def reset(self):
+    self.status = False
+    self.kf = None
+    self.aLeadTau = LEAD_ACCEL_TAU
+
+  def update(self, dRel: float, yRel: float, vLead: float, aLead: float, prob: float):
+    self.dRel = dRel
+    self.yRel = yRel
+    self.vLead = vLead
+    self.aLead = aLead
+    self.prob = prob
+    self.status = True
+
+    if self.kf is None:
+      self.kf = lead_kf(self.vLead)
+    else:
+      self.kf.update(self.vLead)
+
+    self.vLeadK = float(self.kf.x[LEAD_KALMAN_SPEED][0])
+    self.aLeadK = float(self.kf.x[LEAD_KALMAN_ACCEL][0])
+
+    # Learn if constant acceleration
+    if abs(self.aLeadK) < 0.5:
+      self.aLeadTau = LEAD_ACCEL_TAU
+    else:
+      self.aLeadTau *= 0.9
+
+
 class LongitudinalPlanner:
   def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
@@ -55,11 +128,17 @@ class LongitudinalPlanner:
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
     self.v_model_error = 0.0
 
+    self.lead_one = Lead()
+    self.lead_two = Lead()
+
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
     self.solverExecutionTime = 0.0
 
+    # FrogPilot variables
+    self.radarless_model = self.params.get("Model", block=True, encoding='utf-8') in RADARLESS_MODELS
+
   @staticmethod
   def parse_model(model_msg, model_error):
     if (len(model_msg.position.x) == 33 and
@@ -114,11 +193,26 @@ class LongitudinalPlanner:
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
+    if self.radarless_model:
+      model_leads = list(sm['modelV2'].leadsV3)
+      # TODO lead state should be invalidated if its different point than the previous one
+      lead_states = [self.lead_one, self.lead_two]
+      for index in range(len(lead_states)):
+        if len(model_leads) > index:
+          model_lead = model_leads[index]
+          lead_states[index].update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob)
+        else:
+          lead_states[index].reset()
+    else:
+      self.lead_one = sm['radarState'].leadOne
+      self.lead_two = sm['radarState'].leadTwo
+
     self.mpc.set_weights(sm['frogpilotPlan'].jerk, prev_accel_constraint, personality=sm['controlsState'].personality)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow, personality=sm['controlsState'].personality)
+    self.mpc.update(self.lead_one, self.lead_two, sm['frogpilotPlan'].vCruise, x, v, a, j, self.radarless_model, sm['frogpilotPlan'].tFollow,
+                    sm['frogpilotCarControl'].trafficModeActive, personality=sm['controlsState'].personality)
 
     self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
@@ -149,7 +243,7 @@ class LongitudinalPlanner:
     longitudinalPlan.accels = self.a_desired_trajectory.tolist()
     longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
 
-    longitudinalPlan.hasLead = sm['radarState'].leadOne.status
+    longitudinalPlan.hasLead = self.lead_one.status
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 

selfdrive/controls/radard.py

@@ -13,6 +13,7 @@ from openpilot.common.swaglog import cloudlog
 
 from openpilot.common.simple_kalman import KF1D
 
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
 
 # Default lead acceleration decay set to 50% at 1s
 _LEAD_ACCEL_TAU = 1.5
@@ -194,6 +195,8 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
 
 class RadarD:
   def __init__(self, radar_ts: float, delay: int = 0):
+    self.points: dict[int, tuple[float, float, float]] = {}
+
     self.current_time = 0.0
 
     self.tracks: dict[int, Track] = {}
@@ -205,6 +208,7 @@ class RadarD:
 
     self.radar_state: capnp._DynamicStructBuilder | None = None
     self.radar_state_valid = False
+    self.radar_tracks_valid = False
 
     self.ready = False
 
@@ -291,6 +295,32 @@ class RadarD:
       }
     pm.send('liveTracks', tracks_msg)
 
+  def update_radardless(self, rr: Optional[car.RadarData]):
+    radar_points = []
+    radar_errors = []
+    if rr is not None:
+      radar_points = rr.points
+      radar_errors = rr.errors
+
+    self.radar_tracks_valid = len(radar_errors) == 0
+
+    self.points = {}
+    for pt in radar_points:
+      self.points[pt.trackId] = (pt.dRel, pt.yRel, pt.vRel)
+
+  def publish_radardless(self):
+    tracks_msg = messaging.new_message('liveTracks', len(self.points))
+    tracks_msg.valid = self.radar_tracks_valid
+    for index, tid in enumerate(sorted(self.points.keys())):
+      tracks_msg.liveTracks[index] = {
+        "trackId": tid,
+        "dRel": float(self.points[tid][0]) + RADAR_TO_CAMERA,
+        "yRel": -float(self.points[tid][1]),
+        "vRel": float(self.points[tid][2]),
+      }
+
+    return tracks_msg
+
   def update_frogpilot_params(self):
     longitudinal_tune = self.params.get_bool("LongitudinalTune")
 
@@ -310,26 +340,43 @@ def main():
 
   # *** setup messaging
   can_sock = messaging.sub_sock('can')
-  sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL))
-  pm = messaging.PubMaster(['radarState', 'liveTracks'])
+  pub_sock = messaging.pub_sock('liveTracks')
 
   RI = RadarInterface(CP)
 
+  # TODO timing is different between cars, need a single time step for all cars
+  # TODO just take the fastest one for now, and keep resending same messages for slower radars
   rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
   RD = RadarD(CP.radarTimeStep, RI.delay)
 
-  while 1:
-    can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
-    rr = RI.update(can_strings)
-    sm.update(0)
-    if rr is None:
-      continue
+  RADARLESS = Params().get("Model", block=True, encoding='utf-8') in RADARLESS_MODELS
 
-    RD.update(sm, rr)
-    RD.publish(pm, -rk.remaining*1000.0)
+  if not RADARLESS:
+    sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL))
+    pm = messaging.PubMaster(['radarState', 'liveTracks'])
 
-    rk.monitor_time()
+    while True:
+      can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+      rr = RI.update(can_strings)
+      sm.update(0)
+      if rr is None:
+        continue
 
+      RD.update(sm, rr)
+      RD.publish(pm, -rk.remaining*1000.0)
+      rk.monitor_time()
+  else:
+    while True:
+      can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+      rr = RI.update(can_strings)
+      if rr is None:
+        continue
+
+      RD.update_radardless(rr)
+      msg = RD.publish_radardless()
+      pub_sock.send(msg.to_bytes())
+
+      rk.monitor_time()
 
 if __name__ == "__main__":
   main()

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -13,13 +13,14 @@ from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, J_EGO_COST, COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, \
                                                                            get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
-from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, Lead, get_max_accel
 
 from openpilot.system.version import get_short_branch
 
 from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
-from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, calculate_lane_width, calculate_road_curvature
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, STAGING_BRANCHES, calculate_lane_width, calculate_road_curvature
 from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
 
 # Acceleration profiles - Credit goes to the DragonPilot team!
                  # MPH = [0., 18,  36,  63,  94]
@@ -53,9 +54,12 @@ class FrogPilotPlanner:
     self.params_memory = Params("/dev/shm/params")
 
     self.cem = ConditionalExperimentalMode()
+    self.lead_one = Lead()
     self.mtsc = MapTurnSpeedController()
 
-    self.staging = get_short_branch() in ["FrogPilot-Development", "FrogPilot-Staging", "FrogPilot-Testing"]
+    self.staging = get_short_branch() in STAGING_BRANCHES
+
+    self.radarless_model = self.params.get("Model", block=True, encoding='utf-8') in RADARLESS_MODELS
 
     self.jerk = 0
     self.mtsc_target = 0
@@ -65,7 +69,7 @@ class FrogPilotPlanner:
     v_cruise_kph = min(controlsState.vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
     v_ego = max(carState.vEgo, 0)
-    v_lead = radarState.leadOne.vLead
+    v_lead = self.lead_one.vLead
 
     if self.acceleration_profile == 1:
       self.max_accel = get_max_accel_eco(v_ego)
@@ -97,13 +101,13 @@ class FrogPilotPlanner:
 
     road_curvature = calculate_road_curvature(modelData, v_ego)
 
-    if radarState.leadOne.status and self.CP.openpilotLongitudinalControl:
+    if self.lead_one.status and self.CP.openpilotLongitudinalControl:
       base_jerk = get_jerk_factor(self.custom_personalities, self.aggressive_jerk, self.standard_jerk, self.relaxed_jerk, controlsState.personality)
       base_t_follow = get_T_FOLLOW(self.custom_personalities, self.aggressive_follow, self.standard_follow, self.relaxed_follow, controlsState.personality)
       self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow)))
       self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, base_t_follow)))
       self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(v_lead)))
-      self.jerk, self.t_follow = self.update_follow_values(base_jerk, radarState, base_t_follow, v_ego, v_lead)
+      self.jerk, self.t_follow = self.update_follow_values(base_jerk, self.lead_one, base_t_follow, v_ego, v_lead)
     else:
       self.safe_obstacle_distance = 0
       self.safe_obstacle_distance_stock = 0
@@ -113,9 +117,19 @@ class FrogPilotPlanner:
     self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego)
 
     if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl or self.green_light_alert:
-      self.cem.update(carState, controlsState.enabled, frogpilotNavigation, modelData, radarState, road_curvature, self.t_follow, v_ego)
+      self.cem.update(carState, controlsState.enabled, frogpilotNavigation, self.lead_one, modelData, road_curvature, self.t_follow, v_ego)
 
-  def update_follow_values(self, jerk, radarState, t_follow, v_ego, v_lead):
+    if self.radarless_model:
+      model_leads = list(modelData.leadsV3)
+      if len(model_leads) > 0:
+        model_lead = model_leads[0]
+        self.lead_one.update(model_lead.x[0], model_lead.y[0], model_lead.v[0], model_lead.a[0], model_lead.prob)
+      else:
+        self.lead_one.reset()
+    else:
+      self.lead_one = radarState.leadOne
+
+  def update_follow_values(self, jerk, lead_one, t_follow, v_ego, v_lead):
     stopping_distance = STOP_DISTANCE + max(self.increased_stopping_distance - v_ego, 0)
     lead_distance = self.lead_one.dRel + stopping_distance
 

selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py

@@ -33,8 +33,7 @@ class ConditionalExperimentalMode:
 
     self.update_frogpilot_params()
 
-  def update(self, carState, enabled, frogpilotNavigation, modelData, radarState, road_curvature, t_follow, v_ego):
-    lead = radarState.leadOne
+  def update(self, carState, enabled, frogpilotNavigation, lead, modelData, road_curvature, t_follow, v_ego):
     lead_distance = lead.dRel
     standstill = carState.standstill
     v_lead = lead.vLead

selfdrive/frogpilot/controls/lib/model_manager.py

@@ -0,0 +1,102 @@
+import os
+import stat
+import time
+import urllib.request
+
+from openpilot.common.params import Params
+from openpilot.system.version import get_short_branch
+
+VERSION = 'v1' if get_short_branch() == "FrogPilot" else 'v2'
+REPOSITORY_URL = 'https://github.com/FrogAi/FrogPilot-Resources/releases/download'
+
+DEFAULT_MODEL = "wd-40"
+DEFAULT_MODEL_NAME = "WD40 (Default)"
+MODELS_PATH = '/data/models'
+
+NAVIGATION_MODELS = {"certified-herbalist", "duck-amigo", "los-angeles", "recertified-herbalist"}
+RADARLESS_MODELS = {"radical-turtle"}
+
+params = Params()
+params_memory = Params("/dev/shm/params")
+
+def delete_deprecated_models():
+  populate_models()
+
+  available_models = params.get("AvailableModels", encoding='utf-8').split(',')
+
+  if available_models:
+    current_model = params.get("Model", block=True, encoding='utf-8')
+    current_model_file = os.path.join(MODELS_PATH, f"{current_model}.thneed")
+
+    if current_model not in available_models or not os.path.exists(current_model_file):
+      params.put("Model", DEFAULT_MODEL)
+      params.put("ModelName", DEFAULT_MODEL_NAME)
+
+    for model_file in os.listdir(MODELS_PATH):
+      if model_file.endswith('.thneed') and model_file[:-7] not in available_models:
+        os.remove(os.path.join(MODELS_PATH, model_file))
+  else:
+    params.put("Model", DEFAULT_MODEL)
+    params.put("ModelName", DEFAULT_MODEL_NAME)
+
+def download_model():
+  model = params_memory.get("ModelToDownload", encoding='utf-8')
+  model_path = os.path.join(MODELS_PATH, f"{model}.thneed")
+  url = f"{REPOSITORY_URL}/{model}/{model}.thneed"
+
+  os.makedirs(MODELS_PATH, exist_ok=True)
+
+  for attempt in range(5):
+    try:
+      with urllib.request.urlopen(url) as f:
+        total_file_size = int(f.getheader('Content-Length'))
+        if total_file_size == 0:
+          raise ValueError("File is empty")
+
+        with open(model_path, 'wb') as output:
+          current_file_size = 0
+          while chunk := f.read(8192):
+            output.write(chunk)
+            current_file_size += len(chunk)
+            progress = (current_file_size / total_file_size) * 100
+            params_memory.put_int("ModelDownloadProgress", int(progress))
+          os.fsync(output)
+
+      if os.path.getsize(model_path) == total_file_size:
+        print(f"Successfully downloaded the {model} model!")
+        break
+      else:
+        raise Exception("Downloaded model file size does not match expected size. Retrying...")
+
+    except Exception as e:
+      print(f"Attempt {attempt + 1} failed with error: {e}. Retrying...")
+      if os.path.exists(model_path):
+        os.remove(model_path)
+      time.sleep(5)
+  else:
+    print(f"Failed to download the {model} model after {attempt + 1} attempts. Giving up... :(")
+
+def populate_models():
+  model_names_url = f"https://raw.githubusercontent.com/FrogAi/FrogPilot-Resources/master/model_names_{VERSION}.txt"
+
+  for attempt in range(5):
+    try:
+      with urllib.request.urlopen(model_names_url) as response:
+        model_info = [line.decode('utf-8').strip().split(' - ') for line in response.readlines() if ' - ' in line.decode('utf-8')]
+
+      available_models = ','.join(model[0] for model in model_info)
+      available_models_names = [model[1] for model in model_info]
+
+      params.put("AvailableModels", available_models)
+      params.put("AvailableModelsNames", ','.join(available_models_names))
+
+      current_model_name = params.get("ModelName", encoding='utf-8')
+      if current_model_name not in available_models_names and "(Default)" in current_model_name:
+        updated_model_name = current_model_name.replace("(Default)", "").strip()
+        params.put("ModelName", updated_model_name)
+
+    except Exception as e:
+      print(f"Failed to update models list. Error: {e}. Retrying...")
+      time.sleep(5)
+  else:
+    print(f"Failed to update models list after 5 attempts. Giving up... :(")

selfdrive/frogpilot/frogpilot_process.py

@@ -16,6 +16,7 @@ from openpilot.system.hardware import HARDWARE
 
 from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME, download_model, populate_models
 from openpilot.selfdrive.frogpilot.controls.lib.theme_manager import ThemeManager
 
 WIFI = log.DeviceState.NetworkType.wifi
@@ -41,6 +42,8 @@ def github_pinged(url="https://github.com", timeout=5):
 
 def time_checks(automatic_updates, deviceState, params):
   if github_pinged():
+    populate_models()
+
     screen_off = deviceState.screenBrightnessPercent == 0
     wifi_connection = deviceState.networkType == WIFI
 
@@ -60,6 +63,7 @@ def frogpilot_thread():
 
   automatic_updates = params.get_bool("AutomaticUpdates")
   first_run = True
+  model_list_empty = params.get("AvailableModelsNames", encoding='utf-8') is None
   time_validated = system_time_valid()
 
   pm = messaging.PubMaster(['frogpilotPlan'])
@@ -85,9 +89,16 @@ def frogpilot_thread():
                                   sm['liveLocationKalman'], sm['modelV2'], sm['radarState'])
         frogpilot_planner.publish(sm, pm)
 
+    if params_memory.get("ModelToDownload", encoding='utf-8') is not None and github_pinged():
+      download_model()
+
     if params_memory.get_bool("FrogPilotTogglesUpdated"):
       automatic_updates = params.get_bool("AutomaticUpdates")
 
+      if not params.get_bool("ModelSelector"):
+        params.put("Model", DEFAULT_MODEL)
+        params.put("ModelName", DEFAULT_MODEL_NAME)
+
       if started:
         frogpilot_planner.update_frogpilot_params()
       else:
@@ -98,9 +109,10 @@ def frogpilot_thread():
       if not time_validated:
         continue
 
-    if datetime.datetime.now().second == 0 or first_run or params_memory.get_bool("ManualUpdateInitiated"):
-      if not started:
+    if datetime.datetime.now().second == 0 or first_run or model_list_empty or params_memory.get_bool("ManualUpdateInitiated"):
+      if not started or model_list_empty:
         time_checks(automatic_updates, deviceState, params)
+        model_list_empty = params.get("AvailableModelsNames", encoding='utf-8') is None
 
       theme_manager.update_holiday()
 

selfdrive/frogpilot/ui/qt/offroad/control_settings.cc

@@ -48,6 +48,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"MTSCCurvatureCheck",  tr("Model Curvature Detection Failsafe"), tr("Only trigger MTSC when the model detects a curve in the road. Purely used as a failsafe to prevent false positives. Leave this off if you never experience false positives."), ""},
     {"MTSCAggressiveness", tr("Turn Speed Aggressiveness"), tr("Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.\n\nA change of +- 1% results in the speed being raised or lowered by about 1 mph."), ""},
 
+    {"ModelSelector", tr("Model Selector"), tr("Manage openpilot's driving models."), "../assets/offroad/icon_calibration.png"},
+
     {"QOLControls", tr("Quality of Life"), tr("Miscellaneous quality of life changes to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.png"},
     {"CustomCruise", tr("Cruise Increase Interval"), tr("Set a custom interval to increase the max set speed by."), ""},
     {"CustomCruiseLong", tr("Cruise Increase Interval (Long Press)"), tr("Set a custom interval to increase the max set speed by when holding down the cruise increase button."), ""},
@@ -209,7 +211,13 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
       QObject::connect(longitudinalTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
         openParentToggle();
         for (auto &[key, toggle] : toggles) {
-          toggle->setVisible(longitudinalTuneKeys.find(key.c_str()) != longitudinalTuneKeys.end());
+          std::set<QString> modifiedLongitudinalTuneKeys = longitudinalTuneKeys;
+
+          if (params.get("Model") == "radical-turtle") {
+            modifiedLongitudinalTuneKeys.erase("LeadDetectionThreshold");
+          }
+
+          toggle->setVisible(modifiedLongitudinalTuneKeys.find(key.c_str()) != modifiedLongitudinalTuneKeys.end());
         }
       });
       toggle = longitudinalTuneToggle;
@@ -243,6 +251,193 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     } else if (param == "MTSCAggressiveness") {
       toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, std::map<int, QString>(), this, false, "%");
 
+    } else if (param == "ModelSelector") {
+      FrogPilotParamManageControl *modelsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
+      QObject::connect(modelsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
+        openParentToggle();
+        for (auto &[key, toggle] : toggles) {
+          toggle->setVisible(false);
+        }
+
+        deleteModelBtn->setVisible(true);
+        downloadModelBtn->setVisible(true);
+        selectModelBtn->setVisible(true);
+      });
+      toggle = modelsToggle;
+
+      QDir modelDir("/data/models/");
+
+      deleteModelBtn = new ButtonControl(tr("Delete Model"), tr("DELETE"), "");
+      QObject::connect(deleteModelBtn, &ButtonControl::clicked, [=]() {
+        std::string currentModel = params.get("Model") + ".thneed";
+
+        QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(",");
+        QStringList modelLabels = QString::fromStdString(params.get("AvailableModelsNames")).split(",");
+
+        QStringList existingModelFiles = modelDir.entryList({"*.thneed"}, QDir::Files);
+        QMap<QString, QString> labelToFileMap;
+        QStringList deletableModelLabels;
+        for (int i = 0; i < availableModels.size(); ++i) {
+          QString modelFileName = availableModels[i] + ".thneed";
+          if (existingModelFiles.contains(modelFileName) && modelFileName != QString::fromStdString(currentModel)) {
+            QString readableName = modelLabels[i];
+            deletableModelLabels.append(readableName);
+            labelToFileMap[readableName] = modelFileName;
+          }
+        }
+
+        QString selectedModel = MultiOptionDialog::getSelection(tr("Select a model to delete"), deletableModelLabels, "", this);
+        if (!selectedModel.isEmpty() && ConfirmationDialog::confirm(tr("Are you sure you want to delete this model?"), tr("Delete"), this)) {
+          std::thread([=]() {
+            deleteModelBtn->setValue(tr("Deleting..."));
+
+            deleteModelBtn->setEnabled(false);
+            downloadModelBtn->setEnabled(false);
+            selectModelBtn->setEnabled(false);
+
+            QString modelToDelete = labelToFileMap[selectedModel];
+
+            QFile::remove(modelDir.absoluteFilePath(modelToDelete));
+
+            deleteModelBtn->setEnabled(true);
+            downloadModelBtn->setEnabled(true);
+            selectModelBtn->setEnabled(true);
+
+            deleteModelBtn->setValue(tr("Deleted!"));
+            std::this_thread::sleep_for(std::chrono::seconds(3));
+            deleteModelBtn->setValue("");
+          }).detach();
+        }
+      });
+      addItem(deleteModelBtn);
+
+      downloadModelBtn = new ButtonControl(tr("Download Model"), tr("DOWNLOAD"), "");
+      QObject::connect(downloadModelBtn, &ButtonControl::clicked, [=]() {
+        QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(",");
+        QStringList modelLabels = QString::fromStdString(params.get("AvailableModelsNames")).split(",");
+
+        QMap<QString, QString> labelToModelMap;
+        QStringList downloadableModelLabels;
+        QStringList existingModelFiles = modelDir.entryList({"*.thneed"}, QDir::Files);
+        for (int i = 0; i < availableModels.size(); ++i) {
+          QString modelFileName = availableModels.at(i) + ".thneed";
+          if (!existingModelFiles.contains(modelFileName)) {
+            QString readableName = modelLabels.at(i);
+            if (!readableName.endsWith("(Default)")) {
+              downloadableModelLabels.append(readableName);
+              labelToModelMap.insert(readableName, availableModels.at(i));
+            }
+          }
+        }
+
+        QString modelToDownload = MultiOptionDialog::getSelection(tr("Select a driving model to download"), downloadableModelLabels, "", this);
+        if (!modelToDownload.isEmpty()) {
+          QString selectedModelValue = labelToModelMap.value(modelToDownload);
+          paramsMemory.put("ModelToDownload", selectedModelValue.toStdString());
+
+          deleteModelBtn->setEnabled(false);
+          downloadModelBtn->setEnabled(false);
+          selectModelBtn->setEnabled(false);
+
+          QTimer *failureTimer = new QTimer(this);
+          failureTimer->setSingleShot(true);
+
+          QTimer *progressTimer = new QTimer(this);
+          progressTimer->setInterval(100);
+
+          connect(failureTimer, &QTimer::timeout, this, [=]() {
+            deleteModelBtn->setEnabled(true);
+            downloadModelBtn->setEnabled(true);
+            selectModelBtn->setEnabled(true);
+
+            downloadModelBtn->setValue(tr("Download failed..."));
+            paramsMemory.remove("ModelDownloadProgress");
+            paramsMemory.remove("ModelToDownload");
+
+            progressTimer->stop();
+            progressTimer->deleteLater();
+
+            QTimer::singleShot(3000, this, [this]() {
+              downloadModelBtn->setValue("");
+            });
+          });
+
+          connect(progressTimer, &QTimer::timeout, this, [=]() mutable {
+            static int lastProgress = -1;
+            int progress = paramsMemory.getInt("ModelDownloadProgress");
+
+            if (progress == lastProgress) {
+              if (!failureTimer->isActive()) {
+                failureTimer->start(30000);
+              }
+            } else {
+              lastProgress = progress;
+              downloadModelBtn->setValue(QString::number(progress) + "%");
+              failureTimer->stop();
+
+              if (progress == 100) {
+                deleteModelBtn->setEnabled(true);
+                downloadModelBtn->setEnabled(true);
+                selectModelBtn->setEnabled(true);
+
+                downloadModelBtn->setValue(tr("Downloaded!"));
+                paramsMemory.remove("ModelDownloadProgress");
+                paramsMemory.remove("ModelToDownload");
+
+                progressTimer->stop();
+                progressTimer->deleteLater();
+
+                QTimer::singleShot(3000, this, [this]() {
+                  if (paramsMemory.get("ModelDownloadProgress").empty()) {
+                    downloadModelBtn->setValue("");
+                  }
+                });
+              }
+            }
+          });
+          progressTimer->start();
+        }
+      });
+      addItem(downloadModelBtn);
+
+      selectModelBtn = new ButtonControl(tr("Select Model"), tr("SELECT"), "");
+      QObject::connect(selectModelBtn, &ButtonControl::clicked, [=]() {
+        QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(",");
+        QStringList modelLabels = QString::fromStdString(params.get("AvailableModelsNames")).split(",");
+
+        QStringList modelFiles = modelDir.entryList({"*.thneed"}, QDir::Files);
+        QSet<QString> modelFilesBaseNames;
+        for (const QString &modelFile : modelFiles) {
+          modelFilesBaseNames.insert(modelFile.section('.', 0, 0));
+        }
+
+        QStringList selectableModelLabels;
+        for (int i = 0; i < availableModels.size(); ++i) {
+          if (modelFilesBaseNames.contains(availableModels[i]) || modelLabels[i].endsWith("(Default)")) {
+            selectableModelLabels.append(modelLabels[i]);
+          }
+        }
+
+        QString modelToSelect = MultiOptionDialog::getSelection(tr("Select a model"), selectableModelLabels, "", this);
+        if (!modelToSelect.isEmpty()) {
+          selectModelBtn->setValue(modelToSelect);
+
+          int modelIndex = modelLabels.indexOf(modelToSelect);
+          if (modelIndex != -1) {
+            QString selectedModel = availableModels.at(modelIndex);
+            params.put("Model", selectedModel.toStdString());
+            params.put("ModelName", modelToSelect.toStdString());
+          }
+
+          if (FrogPilotConfirmationDialog::yesorno(tr("Do you want to start with a fresh calibration for the newly selected model?"), this)) {
+            params.remove("CalibrationParams");
+            params.remove("LiveTorqueParameters");
+          }
+        }
+      });
+      addItem(selectModelBtn);
+      selectModelBtn->setValue(QString::fromStdString(params.get("ModelName")));
+
     } else if (param == "QOLControls") {
       FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);
       QObject::connect(qolToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() {
@@ -341,6 +536,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     });
   }
 
+  modelManagerToggle = static_cast<FrogPilotParamManageControl*>(toggles["ModelSelector"]);
+
   QObject::connect(parent, &SettingsWindow::closeParentToggle, this, &FrogPilotControlsPanel::hideToggles);
   QObject::connect(parent, &SettingsWindow::closeSubParentToggle, this, &FrogPilotControlsPanel::hideSubToggles);
   QObject::connect(parent, &SettingsWindow::updateMetric, this, &FrogPilotControlsPanel::updateMetric);
@@ -352,14 +549,15 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
 
 void FrogPilotControlsPanel::showEvent(QShowEvent *event, const UIState &s) {
   hasOpenpilotLongitudinal = hasOpenpilotLongitudinal && !params.getBool("DisableOpenpilotLongitudinal");
-
-  online = s.scene.online;
-  bool parked = s.scene.parked;
-  started = s.scene.started;
 }
 
-void FrogPilotControlsPanel::updateState() {
+void FrogPilotControlsPanel::updateState(const UIState &s) {
   if (!isVisible()) return;
+
+  started = s.scene.started;
+
+  downloadModelBtn->setEnabled(s.scene.online);
+  modelManagerToggle->setEnabled(!s.scene.started);
 }
 
 void FrogPilotControlsPanel::updateToggles() {
@@ -440,6 +638,9 @@ void FrogPilotControlsPanel::hideToggles() {
   aggressiveProfile->setVisible(false);
   conditionalSpeedsImperial->setVisible(false);
   conditionalSpeedsMetric->setVisible(false);
+  deleteModelBtn->setVisible(false);
+  downloadModelBtn->setVisible(false);
+  selectModelBtn->setVisible(false);
   standardProfile->setVisible(false);
   relaxedProfile->setVisible(false);
 

selfdrive/frogpilot/ui/qt/offroad/control_settings.h

@@ -22,15 +22,21 @@ private:
   void showEvent(QShowEvent *event, const UIState &s);
   void updateCarToggles();
   void updateMetric();
-  void updateState();
+  void updateState(const UIState &s);
   void updateToggles();
 
+  ButtonControl *deleteModelBtn;
+  ButtonControl *downloadModelBtn;
+  ButtonControl *selectModelBtn;
+
   FrogPilotDualParamControl *aggressiveProfile;
   FrogPilotDualParamControl *conditionalSpeedsImperial;
   FrogPilotDualParamControl *conditionalSpeedsMetric;
   FrogPilotDualParamControl *standardProfile;
   FrogPilotDualParamControl *relaxedProfile;
 
+  FrogPilotParamManageControl *modelManagerToggle;
+
   std::set<QString> aolKeys = {"AlwaysOnLateralMain", "HideAOLStatusBar", "PauseAOLOnBrake"};
   std::set<QString> conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CENavigation", "CESignal", "CESlowerLead", "CEStopLights", "HideCEMStatusBar"};
   std::set<QString> deviceManagementKeys = {"DeviceShutdown", "HigherBitrate", "IncreaseThermalLimits", "LowVoltageShutdown", "NoLogging", "NoUploads", "OfflineMode"};
@@ -60,6 +66,5 @@ private:
   bool isMetric = params.getBool("IsMetric");
   bool isStaging;
   bool isToyota;
-  bool online;
   bool started;
 };

selfdrive/manager/manager.py

@@ -25,10 +25,13 @@ from openpilot.system.version import is_dirty, get_commit, get_version, get_orig
                            is_tested_branch, is_release_branch, get_commit_date
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import FrogPilotFunctions
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME, delete_deprecated_models
 
 
 def frogpilot_boot_functions(frogpilot_functions):
   try:
+    delete_deprecated_models()
+
     while not system_time_valid():
       print("Waiting for system time to become valid...")
       time.sleep(1)

selfdrive/manager/process_config.py

@@ -5,6 +5,7 @@ from openpilot.common.params import Params
 from openpilot.system.hardware import PC, TICI
 from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
 
+
 WEBCAM = os.getenv("USE_WEBCAM") is not None
 
 def driverview(started: bool, params: Params, CP: car.CarParams) -> bool:

selfdrive/modeld/constants.py

@@ -24,6 +24,7 @@ class ModelConstants:
   LAT_PLANNER_STATE_LEN = 4
   LATERAL_CONTROL_PARAMS_LEN = 2
   PREV_DESIRED_CURV_LEN = 1
+  RADAR_TRACKS_LEN = 64
 
   # model outputs constants
   FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
@@ -42,6 +43,7 @@ class ModelConstants:
   DESIRE_PRED_WIDTH = 8
   LAT_PLANNER_SOLUTION_WIDTH = 4
   DESIRED_CURV_WIDTH = 1
+  RADAR_TRACKS_WIDTH = 3
 
   NUM_LANE_LINES = 4
   NUM_ROAD_EDGES = 2

selfdrive/modeld/modeld.py

@@ -24,11 +24,18 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
 
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, MODELS_PATH, NAVIGATION_MODELS, RADARLESS_MODELS
+
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
+MODEL_NAME = Params().get("Model", block=True, encoding='utf-8')
+
+DISABLE_NAV = MODEL_NAME not in NAVIGATION_MODELS
+DISABLE_RADAR = MODEL_NAME in RADARLESS_MODELS
+
 MODEL_PATHS = {
-  ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
+  ModelRunner.THNEED: Path(__file__).parent / ('models/supercombo.thneed' if MODEL_NAME == DEFAULT_MODEL else f'{MODELS_PATH}/{MODEL_NAME}.thneed'),
   ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
 
 METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
@@ -59,9 +66,10 @@ class ModelState:
       'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
       'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),
       'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
-      'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32),
-      'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32),
+      **({'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32),
+          'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)} if not DISABLE_NAV else {}),
       'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
+      **({'radar_tracks': np.zeros(ModelConstants.RADAR_TRACKS_LEN * ModelConstants.RADAR_TRACKS_WIDTH, dtype=np.float32)} if DISABLE_RADAR else {}),
     }
 
     with open(METADATA_PATH, 'rb') as f:
@@ -94,8 +102,11 @@ class ModelState:
 
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
     self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
-    self.inputs['nav_features'][:] = inputs['nav_features']
-    self.inputs['nav_instructions'][:] = inputs['nav_instructions']
+    if not DISABLE_NAV:
+      self.inputs['nav_features'][:] = inputs['nav_features']
+      self.inputs['nav_instructions'][:] = inputs['nav_instructions']
+    if DISABLE_RADAR:
+      self.inputs['radar_tracks'][:] = inputs['radar_tracks']
 
     # if getCLBuffer is not None, frame will be None
     self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
@@ -154,7 +165,7 @@ def main(demo=False):
 
   # messaging
   pm = PubMaster(["modelV2", "cameraOdometry"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "frogpilotPlan"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl", "liveTracks", "frogpilotPlan"])
 
   publish_state = PublishState()
   params = Params()
@@ -243,7 +254,7 @@ def main(demo=False):
     # Enable/disable nav features
     timestamp_llk = sm["navModel"].locationMonoTime
     nav_valid = sm.valid["navModel"] # and (nanos_since_boot() - timestamp_llk < 1e9)
-    nav_enabled = nav_valid and params.get_bool("ExperimentalMode")
+    nav_enabled = nav_valid and not DISABLE_NAV
 
     if not nav_enabled:
       nav_features[:] = 0
@@ -264,6 +275,14 @@ def main(demo=False):
         if 0 <= distance_idx < 50:
           nav_instructions[distance_idx*3 + direction_idx] = 1
 
+    radar_tracks = np.zeros(ModelConstants.RADAR_TRACKS_LEN * ModelConstants.RADAR_TRACKS_WIDTH, dtype=np.float32)
+    if sm.updated["liveTracks"]:
+      for i, track in enumerate(sm["liveTracks"]):
+        if i >= ModelConstants.RADAR_TRACKS_LEN:
+          break
+        vec_index = i * ModelConstants.RADAR_TRACKS_WIDTH
+        radar_tracks[vec_index:vec_index+ModelConstants.RADAR_TRACKS_WIDTH] = [track.dRel, track.yRel, track.vRel]
+
     # tracked dropped frames
     vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1)
     frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10))
@@ -281,8 +300,9 @@ def main(demo=False):
       'desire': vec_desire,
       'traffic_convention': traffic_convention,
       'lateral_control_params': lateral_control_params,
-      'nav_features': nav_features,
-      'nav_instructions': nav_instructions}
+      **({'nav_features': nav_features, 'nav_instructions': nav_instructions} if not DISABLE_NAV else {}),
+      **({'radar_tracks': radar_tracks,} if DISABLE_RADAR else {}),
+    }
 
     mt1 = time.perf_counter()
     model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only)

selfdrive/ui/qt/home.cc

@@ -240,8 +240,10 @@ void OffroadHome::hideEvent(QHideEvent *event) {
 }
 
 void OffroadHome::refresh() {
+  QString model = QString::fromStdString(params.get("ModelName"));
+
   date->setText(QLocale(uiState()->language.mid(5)).toString(QDateTime::currentDateTime(), "dddd, MMMM d"));
-  version->setText(getBrand() + " v" + getVersion().left(14).trimmed());
+  version->setText(getBrand() + " v" + getVersion().left(14).trimmed() + " - " + model);
 
   bool updateAvailable = update_widget->refresh();
   int alerts = alerts_widget->refresh();

selfdrive/ui/qt/onroad.cc

@@ -888,13 +888,13 @@ void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s)
   painter.restore();
 }
 
-void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd) {
+void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd, const float v_ego) {
   painter.save();
 
   const float speedBuff = currentHolidayTheme != 0 || customColors != 0 ? 25. : 10.;  // Make the center of the chevron appear sooner if a theme is active
   const float leadBuff = currentHolidayTheme != 0 || customColors != 0 ? 100. : 40.;  // Make the center of the chevron appear sooner if a theme is active
-  const float d_rel = lead_data.getDRel();
-  const float v_rel = lead_data.getVRel();
+  const float d_rel = lead_data.getX()[0];
+  const float v_rel = lead_data.getV()[0] - v_ego;
 
   float fillAlpha = 0;
   if (d_rel < leadBuff) {
@@ -928,7 +928,7 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState
   painter.drawPolygon(chevron, std::size(chevron));
 
   if (leadInfo) {
-    float lead_speed = std::max(lead_data.getVLead(), 0.0f);
+    float lead_speed = std::max(v_rel + v_ego, 0.0f);
 
     painter.setPen(Qt::white);
     painter.setFont(InterFont(35, QFont::Bold));
@@ -953,6 +953,7 @@ void AnnotatedCameraWidget::paintGL() {
   SubMaster &sm = *(s->sm);
   const double start_draw_t = millis_since_boot();
   const cereal::ModelDataV2::Reader &model = sm["modelV2"].getModelV2();
+  const float v_ego = sm["carState"].getCarState().getVEgo();
 
   // draw camera frame
   {
@@ -974,7 +975,6 @@ void AnnotatedCameraWidget::paintGL() {
     // Wide or narrow cam dependent on speed
     bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD);
     if (has_wide_cam && cameraView == 0) {
-      float v_ego = sm["carState"].getCarState().getVEgo();
       if ((v_ego < 10) || available_streams.size() == 1) {
         wide_cam_requested = true;
       } else if (v_ego > 15) {
@@ -1007,16 +1007,16 @@ void AnnotatedCameraWidget::paintGL() {
     update_model(s, model, sm["uiPlan"].getUiPlan());
     drawLaneLines(painter, s);
 
-    if (s->scene.longitudinal_control && sm.rcv_frame("radarState") > s->scene.started_frame && !scene.hide_lead_marker) {
-      auto radar_state = sm["radarState"].getRadarState();
-      update_leads(s, radar_state, model.getPosition());
-      auto lead_one = radar_state.getLeadOne();
-      auto lead_two = radar_state.getLeadTwo();
-      if (lead_one.getStatus()) {
-        drawLead(painter, lead_one, s->scene.lead_vertices[0]);
-      }
-      if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
-        drawLead(painter, lead_two, s->scene.lead_vertices[1]);
+    if (s->scene.longitudinal_control && sm.rcv_frame("modelV2") > s->scene.started_frame) {
+      update_leads(s, model);
+      float prev_drel = -1;
+      for (int i = 0; i < model.getLeadsV3().size() && i < 2; i++) {
+        const auto &lead = model.getLeadsV3()[i];
+        auto lead_drel = lead.getX()[0];
+        if (lead.getProb() > 0.5 && (prev_drel < 0 || std::abs(lead_drel - prev_drel) > 3.0)) {
+          drawLead(painter, lead, s->scene.lead_vertices[i], v_ego);
+        }
+        prev_drel = lead_drel;
       }
     }
   }

selfdrive/ui/qt/onroad.h

@@ -171,7 +171,7 @@ protected:
   void showEvent(QShowEvent *event) override;
   void updateFrameMat() override;
   void drawLaneLines(QPainter &painter, const UIState *s);
-  void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd);
+  void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd, const float v_ego);
   void drawHud(QPainter &p);
   void drawDriverState(QPainter &painter, const UIState *s);
   inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); }

selfdrive/ui/ui.cc

@@ -44,12 +44,15 @@ int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_h
   return max_idx;
 }
 
-void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line) {
-  for (int i = 0; i < 2; ++i) {
-    auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
-    if (lead_data.getStatus()) {
-      float z = line.getZ()[get_path_length_idx(line, lead_data.getDRel())];
-      calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]);
+void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model_data) {
+  const cereal::XYZTData::Reader &line = model_data.getPosition();
+  for (int i = 0; i < model_data.getLeadsV3().size() && i < 2; ++i) {
+    const auto &lead = model_data.getLeadsV3()[i];
+    if (lead.getProb() > 0.5) {
+      float d_rel = lead.getX()[0];
+      float y_rel = lead.getY()[0];
+      float z = line.getZ()[get_path_length_idx(line, d_rel)];
+      calib_frame_to_full_frame(s, d_rel, y_rel, z + 1.22, &s->scene.lead_vertices[i]);
     }
   }
 }
@@ -117,10 +120,13 @@ void update_model(UIState *s,
     path = scene.path_width;
   }
 
-  auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
-  if (lead_one.getStatus()) {
-    const float lead_d = lead_one.getDRel() * 2.;
-    max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
+  auto lead_count = model.getLeadsV3().size();
+  if (lead_count > 0) {
+    auto lead_one = model.getLeadsV3()[0];
+    if (lead_one.getProb() > 0.5) {
+      const float lead_d = lead_one.getX()[0] * 2.;
+      max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
+    }
   }
   max_idx = get_path_length_idx(plan_position, max_distance);
   update_line_data(s, plan_position, scene.model_ui ? path * (1 - scene.path_edge_width / 100) : 0.9, 1.22, &scene.track_vertices, max_idx, false);
@@ -238,6 +244,7 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("deviceState")) {
     auto deviceState = sm["deviceState"].getDeviceState();
+    scene.online = deviceState.getNetworkType() != cereal::DeviceState::NetworkType::NONE;
   }
   if (sm.updated("frogpilotCarControl")) {
     auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();

selfdrive/ui/ui.h

@@ -197,6 +197,7 @@ typedef struct UIScene {
   bool live_valid;
   bool map_open;
   bool model_ui;
+  bool online;
   bool reverse;
   bool reverse_cruise;
   bool reverse_cruise_ui;
@@ -334,7 +335,7 @@ void update_model(UIState *s,
                   const cereal::ModelDataV2::Reader &model,
                   const cereal::UiPlan::Reader &plan);
 void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &driverstate, float dm_fade_state, bool is_rhd);
-void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, const cereal::XYZTData::Reader &line);
+void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model_data);
 void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line,
                       float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert);