From d86004f249fa14fdd35c6b73b369810349098774 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 31 Jul 2024 19:09:53 -0700
Subject: [PATCH] Controls - Model Management

Manage openpilot's driving models.
---
 selfdrive/controls/controlsd.py               |   7 +-
 .../lib/longitudinal_mpc_lib/long_mpc.py      |  17 +-
 .../controls/lib/longitudinal_planner.py      | 106 ++++++-
 selfdrive/controls/radard.py                  |  68 +++-
 .../frogpilot/controls/frogpilot_planner.py   |  11 +-
 .../controls/lib/frogpilot_functions.py       |   3 +
 .../controls/lib/frogpilot_variables.py       |  38 +++
 .../frogpilot/controls/lib/model_manager.py   | 298 ++++++++++++++++++
 selfdrive/frogpilot/frogpilot_process.py      |  19 ++
 selfdrive/locationd/calibrationd.py           |   7 +-
 selfdrive/locationd/torqued.py                |   9 +-
 selfdrive/modeld/constants.py                 |   4 +
 selfdrive/modeld/fill_model_msg.py            |  16 +-
 selfdrive/modeld/modeld.py                    | 103 ++++--
 selfdrive/modeld/models/commonmodel.cc        |  11 +
 selfdrive/modeld/models/commonmodel.h         |   2 +
 selfdrive/modeld/models/commonmodel.pxd       |   2 +
 selfdrive/modeld/models/commonmodel_pyx.pyx   |  12 +
 .../models/secret-good-openpilot_metadata.pkl | Bin 0 -> 663 bytes
 selfdrive/modeld/parse_model_outputs.py       |   8 +-
 selfdrive/ui/qt/home.cc                       |   8 +-
 selfdrive/ui/qt/offroad/settings.cc           |   4 +-
 selfdrive/ui/qt/offroad/settings.h            |   1 +
 selfdrive/ui/qt/onroad/annotated_camera.cc    |  28 +-
 selfdrive/ui/qt/onroad/annotated_camera.h     |   2 +-
 selfdrive/ui/ui.cc                            |  31 +-
 selfdrive/ui/ui.h                             |   4 +-
 system/manager/manager.py                     |   1 +
 28 files changed, 732 insertions(+), 88 deletions(-)
 create mode 100644 selfdrive/frogpilot/controls/lib/model_manager.py
 create mode 100644 selfdrive/modeld/models/secret-good-openpilot_metadata.pkl

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 78f7ceb5e..61a75b270 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -101,6 +101,8 @@ class Controls:
     if REPLAY:
       # no vipc in replay will make them ignored anyways
       ignore += ['roadCameraState', 'wideRoadCameraState']
+    if FrogPilotVariables.toggles.radarless_model:
+      ignore += ['radarState']
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
@@ -338,8 +340,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 or REPLAY) and not self.sm.all_checks(['radarState'])):
-      self.events.add(EventName.radarFault)
+    if not self.frogpilot_toggles.radarless_model:
+      if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) 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:
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index ebd7b28c0..6c72daabf 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -9,7 +9,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
@@ -45,6 +44,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
@@ -339,7 +340,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
@@ -356,13 +357,13 @@ 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, trafficModeActive, frogpilot_toggles, personality=log.LongitudinalPersonality.standard):
+  def update(self, lead_one, lead_two, v_cruise, x, v, a, j, t_follow, trafficModeActive, frogpilot_toggles, 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
     increased_distance = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0) if not trafficModeActive else 0
 
-    lead_xv_0 = self.process_lead(radarstate.leadOne, increased_distance)
-    lead_xv_1 = self.process_lead(radarstate.leadTwo)
+    lead_xv_0 = self.process_lead(lead_one, increased_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
@@ -421,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 frogpilot_toggles.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
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 3d61166e1..b41379e0a 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -6,12 +6,13 @@ 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.simple_kalman import KF1D
 from openpilot.common.realtime import DT_MDL
 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
 
@@ -25,6 +26,8 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 _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)
@@ -63,6 +66,72 @@ def get_accel_from_plan(CP, speeds, accels):
     return a_target, should_stop
 
 
+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
@@ -74,6 +143,9 @@ 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)
@@ -103,6 +175,8 @@ class LongitudinalPlanner:
     return x, v, a, j
 
   def update(self, sm, frogpilot_toggles):
+    self.secret_good_openpilot = frogpilot_toggles.secretgoodopenpilot_model
+
     self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
     v_ego = sm['carState'].vEgo
@@ -132,7 +206,7 @@ class LongitudinalPlanner:
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
     # Compute model v_ego error
-    self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
+    self.v_model_error = 0. if self.secret_good_openpilot else get_speed_error(sm['modelV2'], v_ego)
 
     if force_slow_decel:
       v_cruise = 0.0
@@ -140,11 +214,25 @@ 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 frogpilot_toggles.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'].accelerationJerk, sm['frogpilotPlan'].dangerJerk, sm['frogpilotPlan'].speedJerk, 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, v_ego, frogpilot_toggles.taco_tune)
-    self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow,
+    self.mpc.update(self.lead_one, self.lead_two, sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow,
                     sm['frogpilotCarState'].trafficModeActive, frogpilot_toggles, personality=sm['controlsState'].personality)
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -180,13 +268,19 @@ 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
 
     a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels)
-    longitudinalPlan.aTarget = a_target
-    longitudinalPlan.shouldStop = should_stop
+    if self.secret_good_openpilot and sm['controlsState'].experimentalMode:
+      model_speeds = np.interp(CONTROL_N_T_IDX, ModelConstants.T_IDXS, sm['modelV2'].velocity.x)
+      model_accels = np.interp(CONTROL_N_T_IDX, ModelConstants.T_IDXS, sm['modelV2'].acceleration.x)
+      a_target_model, should_stop_model = get_accel_from_plan(self.CP, model_speeds, model_accels)
+      a_target = min(a_target, a_target_model)
+      should_stop |= should_stop_model
+    longitudinalPlan.aTarget = float(a_target)
+    longitudinalPlan.shouldStop = bool(should_stop)
     longitudinalPlan.allowBrake = True
     longitudinalPlan.allowThrottle = True
 
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 613cfbf1d..a82a1aea0 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -195,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] = {}
@@ -206,12 +208,14 @@ class RadarD:
 
     self.radar_state: capnp._DynamicStructBuilder | None = None
     self.radar_state_valid = False
+    self.radar_tracks_valid = False
 
     self.ready = False
 
     # FrogPilot variables
     self.frogpilot_toggles = FrogPilotVariables.toggles
 
+    self.secret_good_openpilot = self.frogpilot_toggles.secretgoodopenpilot_model
     self.update_toggles = False
 
   def update(self, sm: messaging.SubMaster, rr):
@@ -257,7 +261,7 @@ class RadarD:
     self.radar_state.radarErrors = list(radar_errors)
     self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
 
-    if len(sm['modelV2'].temporalPose.trans):
+    if len(sm['modelV2'].temporalPose.trans) and not self.secret_good_openpilot:
       model_v_ego = sm['modelV2'].temporalPose.trans[0]
     else:
       model_v_ego = self.v_ego
@@ -294,6 +298,31 @@ class RadarD:
       }
     pm.send('liveTracks', tracks_msg)
 
+  def update_radardless(self, rr):
+    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
 
 # fuses camera and radar data for best lead detection
 def main():
@@ -311,26 +340,41 @@ 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
+  if not FrogPilotVariables.toggles.radarless_model:
+    sm = messaging.SubMaster(['modelV2', 'carState'], frequency=int(1./DT_CTRL))
+    pm = messaging.PubMaster(['radarState', 'liveTracks'])
 
-    RD.update(sm, rr)
-    RD.publish(pm, -rk.remaining*1000.0)
+    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
 
-    rk.monitor_time()
+      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()
diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index e61b4abaf..d42292a71 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/selfdrive/frogpilot/controls/frogpilot_planner.py
@@ -44,6 +44,7 @@ class FrogPilotPlanner:
     self.params_memory = Params("/dev/shm/params")
 
     self.cem = ConditionalExperimentalMode(self)
+    self.lead_one = Lead()
     self.mtsc = MapTurnSpeedController()
 
     self.model_stopped = False
@@ -61,7 +62,15 @@ class FrogPilotPlanner:
     self.tracking_lead_mac = MovingAverageCalculator()
 
   def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles):
-    self.lead_one = radarState.leadOne
+    if frogpilot_toggles.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
 
     v_cruise = min(controlsState.vCruise, V_CRUISE_UNSET) * CV.KPH_TO_MS
     v_ego = max(carState.vEgo, 0)
diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py
index 0c8210161..911416253 100644
--- a/selfdrive/frogpilot/controls/lib/frogpilot_functions.py
+++ b/selfdrive/frogpilot/controls/lib/frogpilot_functions.py
@@ -19,6 +19,8 @@ from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName
 from openpilot.common.time import system_time_valid
 from openpilot.system.hardware import HARDWARE
 
+MODELS_PATH = "/data/models"
+
 def delete_file(file):
   try:
     os.remove(file)
@@ -151,6 +153,7 @@ def setup_frogpilot(build_metadata):
   run_cmd(remount_persist, "Successfully remounted /persist as read-write.", "Failed to remount /persist.")
 
   os.makedirs("/persist/params", exist_ok=True)
+  os.makedirs(MODELS_PATH, exist_ok=True)
 
   remount_root = ['sudo', 'mount', '-o', 'remount,rw', '/']
   run_cmd(remount_root, "File system remounted as read-write.", "Failed to remount file system.")
diff --git a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py
index cd23e228c..bdd49d2fc 100644
--- a/selfdrive/frogpilot/controls/lib/frogpilot_variables.py
+++ b/selfdrive/frogpilot/controls/lib/frogpilot_variables.py
@@ -1,3 +1,5 @@
+import os
+
 from types import SimpleNamespace
 
 from cereal import car
@@ -6,6 +8,9 @@ from openpilot.common.params import Params
 from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
 from openpilot.system.version import get_build_metadata
 
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MODELS_PATH
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME, process_model_name
+
 CITY_SPEED_LIMIT = 25  # 55mph is typically the minimum speed for highways
 CRUISING_SPEED = 5     # Roughly the speed cars go when not touching the gas while in drive
 PROBABILITY = 0.6      # 60% chance of condition being true
@@ -175,6 +180,39 @@ class FrogPilotVariables:
     toggle.mtsc_curvature_check = toggle.map_turn_speed_controller and self.params.get_bool("MTSCCurvatureCheck")
     self.params_memory.put_float("MapTargetLatA", 2 * (self.params.get_int("MTSCAggressiveness") / 100.))
 
+    toggle.model_manager = self.params.get_bool("ModelManagement", block=openpilot_installed)
+    available_models = self.params.get("AvailableModels", block=toggle.model_manager, encoding='utf-8')
+    available_model_names = self.params.get("AvailableModelsNames", block=toggle.model_manager, encoding='utf-8')
+    current_model = self.params_memory.get("CurrentModel", encoding='utf-8')
+    current_model_name = self.params_memory.get("CurrentModelName", encoding='utf-8')
+    if toggle.model_manager and available_models and current_model is None:
+      toggle.model = self.params.get("Model", block=True, encoding='utf-8')
+    else:
+      toggle.model = current_model
+    if not os.path.exists(os.path.join(MODELS_PATH, f"{toggle.model}.thneed")):
+      toggle.model = DEFAULT_MODEL
+      current_model_name = DEFAULT_MODEL_NAME
+      toggle.part_model_param = ""
+    elif available_model_names is None:
+      current_model_name = DEFAULT_MODEL_NAME
+      toggle.part_model_param = ""
+    else:
+      current_model_name = available_model_names.split(',')[available_models.split(',').index(toggle.model)]
+      toggle.part_model_param = process_model_name(current_model_name)
+    navigation_models = self.params.get("NavigationModels", encoding='utf-8')
+    if navigation_models is not None:
+      toggle.navigationless_model = toggle.model not in navigation_models.split(',')
+    else:
+      toggle.navigationless_model = False
+    radarless_model = self.params.get("RadarlessModels", encoding='utf-8')
+    if radarless_model is not None:
+      toggle.radarless_model = toggle.model in radarless_model.split(',')
+    else:
+      toggle.radarless_model = False
+    toggle.secretgoodopenpilot_model = toggle.model == "secret-good-openpilot"
+    self.params_memory.put("CurrentModel", toggle.model)
+    self.params_memory.put("CurrentModelName", current_model_name)
+
     quality_of_life_controls = self.params.get_bool("QOLControls")
     toggle.custom_cruise_increase = self.params.get_int("CustomCruise") if quality_of_life_controls and not pcm_cruise else 1
     toggle.custom_cruise_increase_long = self.params.get_int("CustomCruiseLong") if quality_of_life_controls and not pcm_cruise else 5
diff --git a/selfdrive/frogpilot/controls/lib/model_manager.py b/selfdrive/frogpilot/controls/lib/model_manager.py
new file mode 100644
index 000000000..bda0cbe94
--- /dev/null
+++ b/selfdrive/frogpilot/controls/lib/model_manager.py
@@ -0,0 +1,298 @@
+import json
+import os
+import re
+import requests
+import shutil
+import subprocess
+import time
+import urllib.request
+
+from openpilot.common.basedir import BASEDIR
+
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MODELS_PATH, delete_file, is_url_pingable
+
+VERSION = "v4"
+
+GITHUB_REPOSITORY_URL = "https://raw.githubusercontent.com/FrogAi/FrogPilot-Resources/"
+GITLAB_REPOSITORY_URL = "https://gitlab.com/FrogAi/FrogPilot-Resources/-/raw/"
+
+DEFAULT_MODEL = "north-dakota-v2"
+DEFAULT_MODEL_NAME = "North Dakota V2 (Default)"
+
+def get_repository_url():
+  if is_url_pingable("https://github.com"):
+    return GITHUB_REPOSITORY_URL
+  if is_url_pingable("https://gitlab.com"):
+    return GITLAB_REPOSITORY_URL
+  return None
+
+def get_remote_file_size(url):
+  try:
+    response = requests.head(url, timeout=5)
+    response.raise_for_status()
+    return int(response.headers.get('Content-Length', 0))
+  except requests.RequestException as e:
+    print(f"Error fetching file size: {e}")
+    return None
+
+def process_model_name(model_name):
+  model_cleaned = re.sub(r'[🗺️👀📡]', '', model_name).strip()
+  score_param = re.sub(r'[^a-zA-Z0-9()-]', '', model_cleaned).replace(' ', '').strip().replace('(Default)', '').replace('-', '')
+  cleaned_name = ''.join(score_param.split())
+  print(f'Processed Model Name: {cleaned_name}')
+  return cleaned_name
+
+def handle_error(destination, error_message, error, params_memory):
+  print(f"Error occurred: {error}")
+  params_memory.put("ModelDownloadProgress", error_message)
+  params_memory.remove("DownloadAllModels")
+  params_memory.remove("ModelToDownload")
+  delete_file(destination)
+
+def verify_download(file_path, model_url):
+  if not os.path.exists(file_path):
+    return False
+
+  remote_file_size = get_remote_file_size(model_url)
+  if remote_file_size is None:
+    return False
+
+  return remote_file_size == os.path.getsize(file_path)
+
+def download_file(destination, url, params_memory):
+  try:
+    with requests.get(url, stream=True, timeout=5) as r:
+      r.raise_for_status()
+      total_size = get_remote_file_size(url)
+      downloaded_size = 0
+
+      with open(destination, 'wb') as f:
+        for chunk in r.iter_content(chunk_size=8192):
+          if params_memory.get_bool("CancelModelDownload"):
+            handle_error(destination, "Download cancelled...", "Download cancelled...", params_memory)
+            return
+          if chunk:
+            f.write(chunk)
+            downloaded_size += len(chunk)
+            progress = (downloaded_size / total_size) * 100
+            if progress != 100:
+              params_memory.put("ModelDownloadProgress", f"{progress:.0f}%")
+            else:
+              params_memory.put("ModelDownloadProgress", "Verifying authenticity...")
+
+  except requests.HTTPError as http_error:
+    handle_error(destination, f"Failed: Server error ({http_error.response.status_code})", http_error, params_memory)
+  except requests.ConnectionError as connection_error:
+    handle_error(destination, "Failed: Connection dropped...", connection_error, params_memory)
+  except requests.Timeout as timeout_error:
+    handle_error(destination, "Failed: Download timed out...", timeout_error, params_memory)
+  except requests.RequestException as request_error:
+    handle_error(destination, "Failed: Network request error. Check connection.", request_error, params_memory)
+  except Exception as e:
+    handle_error(destination, "Failed: Unexpected error.", e, params_memory)
+
+def handle_existing_model(model, params_memory):
+  print(f"Model {model} already exists, skipping download...")
+  params_memory.put("ModelDownloadProgress", "Model already exists...")
+  params_memory.remove("ModelToDownload")
+
+def handle_verification_failure(model, model_path, model_url, params_memory):
+  if params_memory.get_bool("CancelModelDownload"):
+    handle_error(model_path, "Download cancelled...", "Download cancelled...", params_memory)
+    return
+
+  handle_error(model_path, "Issue connecting to Github, trying Gitlab", f"Model {model} verification failed. Redownloading from Gitlab...", params_memory)
+  second_model_url = f"{GITLAB_REPOSITORY_URL}Models/{model}.thneed"
+  download_file(model_path, second_model_url, params_memory)
+
+  if verify_download(model_path, second_model_url):
+    print(f"Model {model} redownloaded and verified successfully from Gitlab.")
+  else:
+    print(f"Model {model} redownload verification failed from Gitlab.")
+
+def download_model(model_to_download, params_memory):
+  model_path = os.path.join(MODELS_PATH, f"{model_to_download}.thneed")
+  if os.path.exists(model_path):
+    handle_existing_model(model_to_download, params_memory)
+    return
+
+  repo_url = get_repository_url()
+  if repo_url is None:
+    handle_error(model_path, "Github and Gitlab are offline...", "Github and Gitlab are offline...", params_memory)
+    return
+
+  model_url = f"{repo_url}Models/{model_to_download}.thneed"
+  download_file(model_path, model_url, params_memory)
+
+  if verify_download(model_path, model_url):
+    print(f"Model {model_to_download} downloaded and verified successfully!")
+    params_memory.put("ModelDownloadProgress", "Downloaded!")
+    params_memory.remove("ModelToDownload")
+  else:
+    handle_verification_failure(model_to_download, model_path, model_url, params_memory)
+
+def fetch_models(url):
+  try:
+    with urllib.request.urlopen(url) as response:
+      return json.loads(response.read().decode('utf-8'))['models']
+  except Exception as e:
+    print(f"Failed to update models list. Error: {e}")
+    return None
+
+def are_all_models_downloaded(available_models, available_model_names, repo_url, params, params_memory):
+  automatically_update_models = params.get_bool("AutomaticallyUpdateModels")
+  all_models_downloaded = True
+
+  for model in available_models:
+    model_path = os.path.join(MODELS_PATH, f"{model}.thneed")
+    model_url = f"{repo_url}Models/{model}.thneed"
+
+    if os.path.exists(model_path):
+      if automatically_update_models:
+        remote_file_size = get_remote_file_size(model_url)
+        try:
+          local_file_size = os.path.getsize(model_path)
+        except FileNotFoundError:
+          print(f"File not found: {model_path}. It may have been moved or deleted.")
+          local_file_size = 0
+
+        if remote_file_size is not None and remote_file_size != local_file_size:
+          print(f"Model {model} is outdated. Local size: {local_file_size}, Remote size: {remote_file_size}. Re-downloading...")
+          delete_file(model_path)
+          part_model_param = process_model_name(available_model_names[available_models.index(model)])
+          params.remove(part_model_param + "CalibrationParams")
+          params.remove(part_model_param + "LiveTorqueParameters")
+          while params_memory.get("ModelToDownload", encoding='utf-8') is not None:
+            time.sleep(1)
+          params_memory.put("ModelToDownload", model)
+          all_models_downloaded = False
+    else:
+      if automatically_update_models:
+        while params_memory.get("ModelToDownload", encoding='utf-8') is not None:
+          time.sleep(1)
+        print(f"Model {model} is missing. Re-downloading...")
+        params_memory.put("ModelToDownload", model)
+        part_model_param = process_model_name(available_model_names[available_models.index(model)])
+        params.remove(part_model_param + "CalibrationParams")
+        params.remove(part_model_param + "LiveTorqueParameters")
+      all_models_downloaded = False
+
+  return all_models_downloaded
+
+def update_model_params(model_info, repo_url, params, params_memory):
+  available_models = []
+  available_model_names = []
+  experimental_models = []
+  navigation_models = []
+  radarless_models = []
+
+  for model in model_info:
+    available_models.append(model['id'])
+    available_model_names.append(model['name'])
+    if model.get("experimental", False):
+      experimental_models.append(model['id'])
+    if "🗺️" in model['name']:
+      navigation_models.append(model['id'])
+    if "📡" not in model['name']:
+      radarless_models.append(model['id'])
+
+  params.put_nonblocking("AvailableModels", ','.join(available_models))
+  params.put_nonblocking("AvailableModelsNames", ','.join(available_model_names))
+  params.put_nonblocking("ExperimentalModels", ','.join(experimental_models))
+  params.put_nonblocking("NavigationModels", ','.join(navigation_models))
+  params.put_nonblocking("RadarlessModels", ','.join(radarless_models))
+  print("Models list updated successfully.")
+
+  if available_models is not None:
+    params.put_bool_nonblocking("ModelsDownloaded", are_all_models_downloaded(available_models, available_model_names, repo_url, params, params_memory))
+
+def validate_models(params):
+  current_model = params.get("Model", encoding='utf-8')
+  current_model_name = params.get("ModelName", encoding='utf-8')
+  if "(Default)" in current_model_name and current_model_name != DEFAULT_MODEL_NAME:
+    params.put_nonblocking("ModelName", current_model_name.replace(" (Default)", ""))
+
+  available_models = params.get("AvailableModels", encoding='utf-8')
+  if available_models is None:
+    return
+
+  for model_file in os.listdir(MODELS_PATH):
+    if model_file.endswith('.thneed') and model_file[:-7] not in available_models.split(','):
+      if model_file == current_model:
+        params.put_nonblocking("Model", DEFAULT_MODEL)
+        params.put_nonblocking("ModelName", DEFAULT_MODEL_NAME)
+      delete_file(os.path.join(MODELS_PATH, model_file))
+      print(f"Deleted model file: {model_file}")
+
+def copy_default_model():
+  default_model_path = os.path.join(MODELS_PATH, f"{DEFAULT_MODEL}.thneed")
+  if not os.path.exists(default_model_path):
+    source_path = os.path.join(BASEDIR, "selfdrive/modeld/models/supercombo.thneed")
+    if os.path.exists(source_path):
+      shutil.copyfile(source_path, default_model_path)
+      print(f"Copied default model from {source_path} to {default_model_path}")
+    else:
+      print(f"Source default model not found at {source_path}. Exiting...")
+
+def update_models(params, params_memory, boot_run=True):
+  try:
+    if boot_run:
+      copy_default_model()
+      validate_models(params)
+
+    repo_url = get_repository_url()
+    if repo_url is None:
+      return
+
+    model_info = fetch_models(f"{repo_url}Versions/model_names_{VERSION}.json")
+    if model_info is None:
+      return
+
+    update_model_params(model_info, repo_url, params, params_memory)
+  except subprocess.CalledProcessError as e:
+    print(f"Failed to update models. Error: {e}")
+
+def download_all_models(params, params_memory):
+  copy_default_model()
+
+  repo_url = get_repository_url()
+  if repo_url is None:
+    handle_error(None, "Github and Gitlab are offline...", "Github and Gitlab are offline...", params_memory)
+    return
+
+  model_info = fetch_models(f"{repo_url}Versions/model_names_{VERSION}.json")
+  if model_info is None:
+    handle_error(None, "Unable to update model list...", "Unable to update model list...", params_memory)
+    return
+
+  update_model_params(model_info, repo_url, params, params_memory)
+
+  available_models = params.get("AvailableModels", encoding='utf-8').split(',')
+  available_model_names = params.get("AvailableModelsNames", encoding='utf-8').split(',')
+
+  for model in available_models:
+    if params_memory.get_bool("CancelModelDownload"):
+      handle_error(None, "Download cancelled...", "Download cancelled...", params_memory)
+      return
+    model_path = os.path.join(MODELS_PATH, f"{model}.thneed")
+    if not os.path.exists(model_path):
+      model_index = available_models.index(model)
+      model_name = available_model_names[model_index]
+      cleaned_model_name = re.sub(r'[🗺️👀📡]', '', model_name).strip()
+      print(f"Downloading model: {cleaned_model_name}")
+      params_memory.put("ModelToDownload", model)
+      params_memory.put("ModelDownloadProgress", f"Downloading {cleaned_model_name}...")
+      while params_memory.get("ModelToDownload", encoding='utf-8') is not None:
+        time.sleep(1)
+
+  all_downloaded = False
+  while not all_downloaded:
+    if params_memory.get_bool("CancelModelDownload"):
+      handle_error(None, "Download cancelled...", "Download cancelled...", params_memory)
+      return
+    all_downloaded = all([os.path.exists(os.path.join(MODELS_PATH, f"{model}.thneed")) for model in available_models])
+    time.sleep(1)
+
+  params_memory.put("ModelDownloadProgress", "All models downloaded!")
+  params_memory.remove("DownloadAllModels")
+  params.put_bool_nonblocking("ModelsDownloaded", True)
diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py
index cd59be4b4..af6e79b31 100644
--- a/selfdrive/frogpilot/frogpilot_process.py
+++ b/selfdrive/frogpilot/frogpilot_process.py
@@ -11,13 +11,17 @@ from openpilot.system.hardware import HARDWARE
 from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import backup_toggles, is_url_pingable
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME, download_all_models, download_model, update_models
 
 OFFLINE = log.DeviceState.NetworkType.none
 
 locks = {
   "backup_toggles": threading.Lock(),
+  "download_all_models": threading.Lock(),
+  "download_model": threading.Lock(),
   "time_checks": threading.Lock(),
   "update_frogpilot_params": threading.Lock(),
+  "update_models": threading.Lock()
 }
 
 running_threads = {}
@@ -54,6 +58,9 @@ def time_checks(automatic_updates, deviceState, now, started, params, params_mem
 
   update_maps(now, params, params_memory)
 
+  with locks["update_models"]:
+    update_models(params, params_memory, False)
+
 def update_maps(now, params, params_memory):
   maps_selected = params.get("MapsSelected", encoding='utf8')
   if maps_selected is None:
@@ -114,11 +121,22 @@ def frogpilot_thread():
                                sm['frogpilotNavigation'], sm['modelV2'], sm['radarState'], frogpilot_toggles)
       frogpilot_planner.publish(sm, pm, frogpilot_toggles)
 
+    model_to_download = params_memory.get("ModelToDownload", encoding='utf-8')
+    if model_to_download:
+      run_thread_with_lock("download_model", locks["download_model"], download_model, (model_to_download, params_memory))
+
+    if params_memory.get_bool("DownloadAllModels"):
+      run_thread_with_lock("download_all_models", locks["download_all_models"], download_all_models, (params, params_memory))
+
     if FrogPilotVariables.toggles_updated:
       update_toggles = True
     elif update_toggles:
       run_thread_with_lock("update_frogpilot_params", locks["update_frogpilot_params"], FrogPilotVariables.update_frogpilot_params, (started,))
 
+      if not frogpilot_toggles.model_manager:
+        params.put_nonblocking("Model", DEFAULT_MODEL)
+        params.put_nonblocking("ModelName", DEFAULT_MODEL_NAME)
+
       if time_validated and not started:
         run_thread_with_lock("backup_toggles", locks["backup_toggles"], backup_toggles, (params, params_storage))
 
@@ -136,6 +154,7 @@ def frogpilot_thread():
         time_validated = system_time_valid()
         if not time_validated:
           continue
+        run_thread_with_lock("update_models", locks["update_models"], update_models, (params, params_memory))
 
 def main():
   frogpilot_thread()
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index aeeddd33e..d46fea28d 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -72,7 +72,10 @@ class Calibrator:
 
     # Read saved calibration
     self.params = Params()
-    calibration_params = self.params.get("CalibrationParams")
+    if self.params.check_key(self.frogpilot_toggles.part_model_param + "CalibrationParams"):
+      calibration_params = self.params.get(self.frogpilot_toggles.part_model_param + "CalibrationParams")
+    else:
+      calibration_params = self.params.get("CalibrationParams")
     rpy_init = RPY_INIT
     wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
     height = HEIGHT_INIT
@@ -171,7 +174,7 @@ class Calibrator:
 
     write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
     if self.param_put and write_this_cycle:
-      self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
+      self.params.put_nonblocking(self.frogpilot_toggles.part_model_param + "CalibrationParams", self.get_msg(True).to_bytes())
 
     # Update FrogPilot parameters
     if FrogPilotVariables.toggles_updated:
diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py
index 6c71a6865..2c09d0b37 100755
--- a/selfdrive/locationd/torqued.py
+++ b/selfdrive/locationd/torqued.py
@@ -100,7 +100,10 @@ class TorqueEstimator(ParameterEstimator):
     # try to restore cached params
     params = Params()
     params_cache = params.get("CarParamsPrevRoute")
-    torque_cache = params.get("LiveTorqueParameters")
+    if params.check_key(self.frogpilot_toggles.part_model_param + "LiveTorqueParameters"):
+      torque_cache = params.get(self.frogpilot_toggles.part_model_param + "LiveTorqueParameters")
+    else:
+      torque_cache = params.get("LiveTorqueParameters")
     if params_cache is not None and torque_cache is not None:
       try:
         with log.Event.from_bytes(torque_cache) as log_evt:
@@ -120,7 +123,7 @@ class TorqueEstimator(ParameterEstimator):
           cloudlog.info("restored torque params from cache")
       except Exception:
         cloudlog.exception("failed to restore cached torque params")
-        params.remove("LiveTorqueParameters")
+        params.remove(self.frogpilot_toggles.part_model_param + "LiveTorqueParameters")
 
     self.filtered_params = {}
     for param in initial_params:
@@ -258,7 +261,7 @@ def main(demo=False):
     # Cache points every 60 seconds while onroad
     if sm.frame % 240 == 0:
       msg = estimator.get_msg(valid=sm.all_checks(), with_points=True)
-      params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())
+      params.put_nonblocking(frogpilot_toggles.part_model_param + "LiveTorqueParameters", msg.to_bytes())
 
 if __name__ == "__main__":
   import argparse
diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py
index dda1ff5e3..7d2b314ac 100644
--- a/selfdrive/modeld/constants.py
+++ b/selfdrive/modeld/constants.py
@@ -15,7 +15,9 @@ class ModelConstants:
   # model inputs constants
   MODEL_FREQ = 20
   FEATURE_LEN = 512
+  FULL_HISTORY_BUFFER_LEN = 99
   HISTORY_BUFFER_LEN = 99
+  HISTORY_BUFFER_LEN_SECRET = 24
   DESIRE_LEN = 8
   TRAFFIC_CONVENTION_LEN = 2
   NAV_FEATURE_LEN = 256
@@ -24,6 +26,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 +45,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
diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py
index c39ec2da3..99d7a9360 100644
--- a/selfdrive/modeld/fill_model_msg.py
+++ b/selfdrive/modeld/fill_model_msg.py
@@ -44,7 +44,7 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std
 def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], publish_state: PublishState,
                    vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float,
                    timestamp_eof: int, timestamp_llk: int, model_execution_time: float,
-                   nav_enabled: bool, valid: bool) -> None:
+                   nav_enabled: bool, valid: bool, secret_good_openpilot: bool) -> None:
   frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
   msg.valid = valid
 
@@ -141,10 +141,16 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
 
   # temporal pose
   temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
-  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
-  temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
-  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
+  if secret_good_openpilot:
+    temporal_pose.trans = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
+    temporal_pose.transStd = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
+    temporal_pose.rot = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
+    temporal_pose.rotStd = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
+  else:
+    temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
+    temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
+    temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
+    temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
 
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py
index 6fca3fd17..992d58238 100755
--- a/selfdrive/modeld/modeld.py
+++ b/selfdrive/modeld/modeld.py
@@ -25,17 +25,29 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MODELS_PATH
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL
 
 frogpilot_toggles = FrogPilotVariables.toggles
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
+MODEL_NAME = frogpilot_toggles.model
+
+DISABLE_NAV = frogpilot_toggles.navigationless_model
+DISABLE_RADAR = frogpilot_toggles.radarless_model
+SECRET_GOOD_OPENPILOT = frogpilot_toggles.secretgoodopenpilot_model
+
 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'
+METADATA_PATH = Path(__file__).parent / ('models/supercombo_metadata.pkl' if not SECRET_GOOD_OPENPILOT else 'models/secret-good-openpilot_metadata.pkl')
+
+MODEL_WIDTH = 512
+MODEL_HEIGHT = 256
+MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 // 2
 
 class FrameMeta:
   frame_id: int = 0
@@ -58,14 +70,24 @@ class ModelState:
     self.frame = ModelFrame(context)
     self.wide_frame = ModelFrame(context)
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
+    self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
+    self.desire_20Hz =  np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
     self.inputs = {
-      'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
+      'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN_SECRET+1 if SECRET_GOOD_OPENPILOT else ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
       '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),
-      'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
+      'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN_SECRET+1 if SECRET_GOOD_OPENPILOT else 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)} if not DISABLE_NAV else {}),
+      'features_buffer': np.zeros((ModelConstants.HISTORY_BUFFER_LEN_SECRET if SECRET_GOOD_OPENPILOT else 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 {}),
     }
 
+    self.input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32)
+    self.big_input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32)
+    self.input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32)
+    self.big_input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32)
+
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
 
@@ -90,26 +112,61 @@ class ModelState:
                 inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
     # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
     inputs['desire'][0] = 0
-    self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
-    self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
+
+    if SECRET_GOOD_OPENPILOT:
+      new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
+      self.desire_20Hz[:-1] = self.desire_20Hz[1:]
+      self.desire_20Hz[-1] = new_desire
+      self.inputs['desire'][:] = self.desire_20Hz.reshape((25,4,-1)).max(axis=1).flatten()
+    else:
+      self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
+      self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
+
     self.prev_desire[:] = inputs['desire']
 
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
     self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
+    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")))
-    if wbuf is not None:
-      self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
+    if SECRET_GOOD_OPENPILOT:
+      new_img = self.frame.prepareSecret(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))
+      self.input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.input_imgs_20hz[MODEL_FRAME_SIZE:]
+      self.input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_img
+      self.input_imgs[:MODEL_FRAME_SIZE] = self.input_imgs_20hz[:MODEL_FRAME_SIZE]
+      self.input_imgs[MODEL_FRAME_SIZE:] = self.input_imgs_20hz[-MODEL_FRAME_SIZE:]
+      self.model.setInputBuffer("input_imgs", self.input_imgs)
+      if wbuf is not None:
+        new_big_img = self.wide_frame.prepareSecret(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs"))
+        self.big_input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[MODEL_FRAME_SIZE:]
+        self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_big_img
+        self.big_input_imgs[:MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[:MODEL_FRAME_SIZE]
+        self.big_input_imgs[MODEL_FRAME_SIZE:] = self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:]
+        self.model.setInputBuffer("big_input_imgs", self.big_input_imgs)
+    else:
+      # 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")))
+      if wbuf is not None:
+        self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
 
     if prepare_only:
       return None
 
     self.model.execute()
-    outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
+    outputs = self.parser.parse_outputs(self.slice_outputs(self.output), SECRET_GOOD_OPENPILOT)
+
+    if SECRET_GOOD_OPENPILOT:
+      self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
+      self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
+      idxs = np.arange(-4,-100,-4)[::-1]
+      self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten()
+    else:
+      self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
+      self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
 
-    self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
-    self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
     self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:]
     self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :]
     return outputs
@@ -154,7 +211,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()
@@ -245,7 +302,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
@@ -266,6 +323,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))
@@ -283,7 +348,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} 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)
@@ -294,7 +361,7 @@ def main(demo=False):
       modelv2_send = messaging.new_message('modelV2')
       posenet_send = messaging.new_message('cameraOdometry')
       fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio,
-                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen)
+                      meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen, SECRET_GOOD_OPENPILOT)
 
       desire_state = modelv2_send.modelV2.meta.desireState
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc
index 523ce00e4..552450f47 100644
--- a/selfdrive/modeld/models/commonmodel.cc
+++ b/selfdrive/modeld/models/commonmodel.cc
@@ -7,6 +7,7 @@
 #include "common/clutil.h"
 
 ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) {
+  frame = std::make_unique<float[]>(MODEL_FRAME_SIZE);
   input_frames = std::make_unique<float[]>(buf_size);
 
   q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
@@ -39,6 +40,16 @@ float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int
   }
 }
 
+float* ModelFrame::prepareSecret(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) {
+  transform_queue(&this->transform, q,
+                  yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
+                  y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection);
+  loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
+  CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &frame[0], 0, nullptr, nullptr));
+  clFinish(q);
+  return &frame[0];
+}
+
 ModelFrame::~ModelFrame() {
   transform_destroy(&transform);
   loadyuv_destroy(&loadyuv);
diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h
index 2cf79094a..0eb7b85fb 100644
--- a/selfdrive/modeld/models/commonmodel.h
+++ b/selfdrive/modeld/models/commonmodel.h
@@ -23,6 +23,7 @@ public:
   ModelFrame(cl_device_id device_id, cl_context context);
   ~ModelFrame();
   float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output);
+  float* prepareSecret(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output);
 
   const int MODEL_WIDTH = 512;
   const int MODEL_HEIGHT = 256;
@@ -34,5 +35,6 @@ private:
   LoadYUVState loadyuv;
   cl_command_queue q;
   cl_mem y_cl, u_cl, v_cl, net_input_cl;
+  std::unique_ptr<float[]> frame;
   std::unique_ptr<float[]> input_frames;
 };
diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd
index 7c3eb0b3d..c51acfe8f 100644
--- a/selfdrive/modeld/models/commonmodel.pxd
+++ b/selfdrive/modeld/models/commonmodel.pxd
@@ -16,5 +16,7 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h":
 
   cppclass ModelFrame:
     int buf_size
+    int MODEL_FRAME_SIZE
     ModelFrame(cl_device_id, cl_context)
     float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)
+    float * prepareSecret(cl_mem, int, int, int, int, mat3, cl_mem*)
diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx
index e292bb0d2..ed9356080 100644
--- a/selfdrive/modeld/models/commonmodel_pyx.pyx
+++ b/selfdrive/modeld/models/commonmodel_pyx.pyx
@@ -45,3 +45,15 @@ cdef class ModelFrame:
     if not data:
       return None
     return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)
+
+  def prepareSecret(self, VisionBuf buf, float[:] projection, CLMem output):
+    cdef mat3 cprojection
+    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
+    cdef float * data
+    if output is None:
+      data = self.frame.prepareSecret(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL)
+    else:
+      data = self.frame.prepareSecret(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
+    if not data:
+      return None
+    return np.asarray(<cnp.float32_t[:self.frame.MODEL_FRAME_SIZE]> data)
diff --git a/selfdrive/modeld/models/secret-good-openpilot_metadata.pkl b/selfdrive/modeld/models/secret-good-openpilot_metadata.pkl
new file mode 100644
index 0000000000000000000000000000000000000000..003280d46afc4fe9b0de865b030ec85cb20d7373
GIT binary patch
literal 663
zcmZ{iJx&8L5QUQ{KnMW@D7zqW04h$v0gzTsK?z8f6MGXYUVG)AP#`2K(zwGMFcKw)
zz<(hXF5frvJoD`L{+I1_;(2p7_E;F*8VwbrGooCO`Yl7;*}>FMrYTp>?nUZ8UDW|k
z7n8MnaCYd62xOG|uEoBW!E&6)>5jlwifO>hF;E!~r9c=GJWq{k3|@=W*k=UcQ2knf
zP1X*B_Ghyxz;^~COca#_DvdM=P2UCh*%~!OqoDm1;JQraN4dV0B;Ijdg1e0Rtx(b8
zt_1g4D_$rju$H2Mn5=v@kQhc}FugCqBv+lpU9?18)j~FbPD=2Y=~=oG!<msQq>YT1
zJeJ@&7mOAZW5Rbkco3&Gc0_r6mIZ3_vka3$o4Il~Rks>d@1WDw&Yn!^9R3IQ(+tix
zTvda$v*&)=x~4NY6MRLurh*69`*5~kK1zvLKw1h0TO?7Vw)o6PxAJL(*waqQwd-9^
mYZ4b!aBAw>=j1IfL8rHNX7|OmyV!&D>4GzOzWQ*=-2MXU)%WE9

literal 0
HcmV?d00001

diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py
index af57e11d0..d01e456d3 100644
--- a/selfdrive/modeld/parse_model_outputs.py
+++ b/selfdrive/modeld/parse_model_outputs.py
@@ -81,14 +81,15 @@ class Parser:
     outs[name] = pred_mu_final.reshape(final_shape)
     outs[name + '_stds'] = pred_std_final.reshape(final_shape)
 
-  def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+  def parse_outputs(self, outs: dict[str, np.ndarray], secret_good_openpilot) -> dict[str, np.ndarray]:
     self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION,
                    out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH))
     self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
     self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
     self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    if not secret_good_openpilot:
+      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
                    out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
@@ -98,6 +99,7 @@ class Parser:
       self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
     for k in ['lead_prob', 'lane_lines_prob', 'meta']:
       self.parse_binary_crossentropy(k, outs)
-    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
+    if not secret_good_openpilot:
+      self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
     self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
     return outs
diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc
index 08dfc772b..1cd7f3cd8 100644
--- a/selfdrive/ui/qt/home.cc
+++ b/selfdrive/ui/qt/home.cc
@@ -240,8 +240,14 @@ void OffroadHome::hideEvent(QHideEvent *event) {
 }
 
 void OffroadHome::refresh() {
+  QString model = QString::fromStdString(params.get("ModelName"));
+
+  if (model.contains("(Default)")) {
+    model = model.remove("(Default)").trimmed();
+  }
+
   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();
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index e3be6271b..6f15d6daf 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -234,7 +234,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
   connect(dcamBtn, &ButtonControl::clicked, [=]() { emit showDriverView(); });
   addItem(dcamBtn);
 
-  auto resetCalibBtn = new ButtonControl(tr("Reset Calibration"), tr("RESET"), "");
+  resetCalibBtn = new ButtonControl(tr("Reset Calibration"), tr("RESET"), "");
   connect(resetCalibBtn, &ButtonControl::showDescriptionEvent, this, &DevicePanel::updateCalibDescription);
   connect(resetCalibBtn, &ButtonControl::clicked, [&]() {
     if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("Reset"), this)) {
@@ -629,6 +629,8 @@ void DevicePanel::poweroff() {
 void DevicePanel::showEvent(QShowEvent *event) {
   pair_device->setVisible(uiState()->primeType() == PrimeType::UNPAIRED);
   ListWidget::showEvent(event);
+
+  resetCalibBtn->setVisible(!params.getBool("ModelManagement"));
 }
 
 void SettingsWindow::hideEvent(QHideEvent *event) {
diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h
index 93e24214d..0b5b2a35c 100644
--- a/selfdrive/ui/qt/offroad/settings.h
+++ b/selfdrive/ui/qt/offroad/settings.h
@@ -76,6 +76,7 @@ private:
   // FrogPilot variables
   Params paramsMemory{"/dev/shm/params"};
 
+  ButtonControl *resetCalibBtn;
   FrogPilotButtonsControl *forceStartedBtn;
 };
 
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
index 4477d996c..583f9afa8 100644
--- a/selfdrive/ui/qt/onroad/annotated_camera.cc
+++ b/selfdrive/ui/qt/onroad/annotated_camera.cc
@@ -337,13 +337,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 = 10.;
   const float leadBuff = 40.;
-  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) {
@@ -378,6 +378,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
   {
@@ -399,7 +400,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) {
-      float v_ego = sm["carState"].getCarState().getVEgo();
       if ((v_ego < 10) || available_streams.size() == 1) {
         wide_cam_requested = true;
       } else if (v_ego > 15) {
@@ -430,16 +430,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) {
-      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 (s->scene.has_lead && (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;
       }
     }
   }
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h
index 8f5ffc73b..094561093 100644
--- a/selfdrive/ui/qt/onroad/annotated_camera.h
+++ b/selfdrive/ui/qt/onroad/annotated_camera.h
@@ -84,7 +84,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); }
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index d5fcbdfc5..617615cce 100644
--- a/selfdrive/ui/ui.cc
+++ b/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 (s->scene.has_lead) {
+      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]);
     }
   }
 }
@@ -105,10 +108,14 @@ void update_model(UIState *s,
   }
 
   // update path
-  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];
+    scene.has_lead = lead_one.getProb() > scene.lead_detection_threshold;
+    if (scene.has_lead) {
+      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, 0.9, 1.22, &scene.track_vertices, max_idx, false);
@@ -288,6 +295,10 @@ void ui_update_frogpilot_params(UIState *s, Params &params) {
 
   scene.experimental_mode_via_screen = scene.longitudinal_control && params.getBool("ExperimentalModeActivation") && params.getBool("ExperimentalModeViaTap");
 
+  bool longitudinal_tune = scene.longitudinal_control && params.getBool("LongitudinalTune");
+  bool radarless_model = params.get("Model") == "radical-turtle";
+  scene.lead_detection_threshold = longitudinal_tune && !radarless_model ? params.getInt("LeadDetectionThreshold") / 100.0f : 0.5;
+
   scene.tethering_config = params.getInt("TetheringEnabled");
   if (scene.tethering_config == 2) {
     WifiManager(s).setTetheringEnabled(true);
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 31598179d..7e2554a23 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -128,6 +128,7 @@ typedef struct UIScene {
   bool enabled;
   bool experimental_mode;
   bool experimental_mode_via_screen;
+  bool has_lead;
   bool map_open;
   bool online;
   bool onroad_distance_button;
@@ -141,6 +142,7 @@ typedef struct UIScene {
   bool use_kaofui_icons;
 
   float adjusted_cruise;
+  float lead_detection_threshold;
 
   int alert_size;
   int conditional_speed;
@@ -240,7 +242,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);
 
diff --git a/system/manager/manager.py b/system/manager/manager.py
index f4f75226d..2b5aa4e49 100644
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@@ -20,6 +20,7 @@ from openpilot.common.swaglog import cloudlog, add_file_handler
 from openpilot.system.version import get_build_metadata, terms_version, training_version
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import frogpilot_boot_functions, setup_frogpilot, uninstall_frogpilot
+from openpilot.selfdrive.frogpilot.controls.lib.model_manager import DEFAULT_MODEL, DEFAULT_MODEL_NAME
 
 
 def manager_init() -> None: