version: dragonpilot development version for EON/C2

date: 2024-03-01T18:48:48
commit: f1bee6b8e2fc94eee0ab1b1db78e42a9c8e2f4aa

CHANGELOGS-D2.md

@@ -1,3 +1,18 @@
+2024-03-01
+========================
+* Display correct changelogs.
+
+
+2024-02-29
+========================
+* Added Lead Vehicle Warning
+* Added Disable Auto Updates toggle
+* Reverted panda back to last working version for honda with minimal changes from op master. (breaks red panda support for now)
+* Added EON Installer (https://raw.githubusercontent.com/dragonpilot-community/dragonpilot/d2/scripts/eon_installer.sh)
+* Conditionally include red panda firmware.
+* Enabled branch selector.
+* Reverted transform patch.
+
 2024-02-27
 ========================
 * Fixed door lock/unlock for Toyotas.

common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "2024.02.28"
+#define COMMA_VERSION "2024.03.01"

scripts/eon_installer.sh

@@ -2,7 +2,7 @@
 rm -fr /data/openpilot ;
 cd /data/ &&
 
-git clone https://github.com/dragonpilot-community/dragonpilot -b $1 --single-branch --depth=1 openpilot &&
+git clone https://github.com/dragonpilot-community/dragonpilot -b "$1" --single-branch --depth=1 openpilot &&
 
 touch /data/data/com.termux/files/continue.sh &&
 echo "#!/usr/bin/bash" >> /data/data/com.termux/files/continue.sh &&

selfdrive/car/toyota/carstate.py

@@ -9,7 +9,7 @@ from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
-                                                  TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
+  TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 
 from openpilot.common.params import Params
 
@@ -97,7 +97,7 @@ class CarState(CarStateBase):
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.vEgoCluster = ret.vEgo * 1.015  # minimum of all the cars
 
-    ret.standstill = ret.vEgoRaw == 0
+    ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
     ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
@@ -149,6 +149,9 @@ class CarState(CarStateBase):
     ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
     ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
 
+    if self.CP.carFingerprint != CAR.MIRAI:
+      ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
+
     ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
     ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
     # we could use the override bit from dbc, but it's triggered at too high torque values
@@ -191,7 +194,7 @@ class CarState(CarStateBase):
     # TODO: it is possible to avoid the lockout and gain stop and go if you
     # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
     if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR) or \
-       (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
+      (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
       self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
 
     self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
@@ -279,6 +282,9 @@ class CarState(CarStateBase):
       ("STEER_TORQUE_SENSOR", 50),
     ]
 
+    if CP.carFingerprint != CAR.MIRAI:
+      messages.append(("ENGINE_RPM", 42))
+
     if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
       messages.append(("DSU_CRUISE", 5))
       messages.append(("PCM_CRUISE_ALT", 1))

selfdrive/car/toyota/fingerprints.py

@@ -38,6 +38,7 @@ FW_VERSIONS = {
       b'F152607180\x00\x00\x00\x00\x00\x00',
       b'F152641040\x00\x00\x00\x00\x00\x00',
       b'F152641050\x00\x00\x00\x00\x00\x00',
+      b'F152641060\x00\x00\x00\x00\x00\x00',
       b'F152641061\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.dsu, 0x791, None): [
@@ -55,10 +56,12 @@ FW_VERSIONS = {
       b'\x01896630725100\x00\x00\x00\x00',
       b'\x01896630725200\x00\x00\x00\x00',
       b'\x01896630725300\x00\x00\x00\x00',
+      b'\x01896630725400\x00\x00\x00\x00',
       b'\x01896630735100\x00\x00\x00\x00',
       b'\x01896630738000\x00\x00\x00\x00',
       b'\x02896630724000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
       b'\x02896630728000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x02896630734000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
       b'\x02896630737000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
@@ -570,6 +573,7 @@ FW_VERSIONS = {
       b'\x018821F6201400\x00\x00\x00\x00',
     ],
     (Ecu.fwdCamera, 0x750, 0x6d): [
+      b'\x028646F12010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
@@ -840,6 +844,7 @@ FW_VERSIONS = {
       b'8965B47023\x00\x00\x00\x00\x00\x00',
       b'8965B47050\x00\x00\x00\x00\x00\x00',
       b'8965B47060\x00\x00\x00\x00\x00\x00',
+      b'8965B47070\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
       b'F152647290\x00\x00\x00\x00\x00\x00',
@@ -1021,6 +1026,7 @@ FW_VERSIONS = {
       b'\x02896634A13000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02896634A13001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
       b'\x02896634A13101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
+      b'\x02896634A13201\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
       b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
       b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
       b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@@ -1361,16 +1367,19 @@ FW_VERSIONS = {
       b'\x018966378G3000\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
+      b'\x0237881000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0237887000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02378A0000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02378F4000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.abs, 0x7b0, None): [
       b'\x01F152678221\x00\x00\x00\x00\x00\x00',
+      b'F152678200\x00\x00\x00\x00\x00\x00',
       b'F152678210\x00\x00\x00\x00\x00\x00',
       b'F152678211\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.eps, 0x7a1, None): [
+      b'8965B78110\x00\x00\x00\x00\x00\x00',
       b'8965B78120\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
@@ -1417,6 +1426,7 @@ FW_VERSIONS = {
   },
   CAR.LEXUS_RX: {
     (Ecu.engine, 0x700, None): [
+      b'\x01896630E36100\x00\x00\x00\x00',
       b'\x01896630E36200\x00\x00\x00\x00',
       b'\x01896630E36300\x00\x00\x00\x00',
       b'\x01896630E37100\x00\x00\x00\x00',

selfdrive/car/toyota/interface.py

@@ -3,13 +3,11 @@ from openpilot.common.conversions import Conversions as CV
 from panda import Panda
 from panda.python import uds
 from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
-                                        MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
+  MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR
 from openpilot.selfdrive.car import get_safety_config
 from openpilot.selfdrive.car.disable_ecu import disable_ecu
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
-from openpilot.common.params import Params
-
 EventName = car.CarEvent.EventName
 SteerControlType = car.CarParams.SteerControlType
 
@@ -220,24 +218,18 @@ class CarInterface(CarInterfaceBase):
     # In TSS2 cars, the camera does long control
     found_ecus = [fw.ecu for fw in car_fw]
     ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \
-                                        and not (ret.flags & ToyotaFlags.SMART_DSU)
-    ret.enableGasInterceptor = 0x201 in fingerprint[0]
-
-    if ret.enableGasInterceptor:
-      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_GAS_INTERCEPTOR
+                    and not (ret.flags & ToyotaFlags.SMART_DSU)
 
     # if the smartDSU is detected, openpilot can send ACC_CONTROL and the smartDSU will block it from the DSU or radar.
     # since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle
     use_sdsu = bool(ret.flags & ToyotaFlags.SMART_DSU)
     if candidate in (RADAR_ACC_CAR | NO_DSU_CAR):
-      ret.experimentalLongitudinalAvailable = use_sdsu
+      ret.experimentalLongitudinalAvailable = use_sdsu or candidate in RADAR_ACC_CAR
 
       if not use_sdsu:
         # Disabling radar is only supported on TSS2 radar-ACC cars
-        if experimental_long and candidate in RADAR_ACC_CAR: # and False:  # TODO: disabling radar isn't supported yet
+        if experimental_long and candidate in RADAR_ACC_CAR:
           ret.flags |= ToyotaFlags.DISABLE_RADAR.value
-          # rick - looks like this is only for doc, we still want to set this to true to prevent removing ExperimentalLongitudinalEnabled in controlsd.py
-          ret.experimentalLongitudinalAvailable = True
       else:
         use_sdsu = use_sdsu and experimental_long
 
@@ -251,10 +243,14 @@ class CarInterface(CarInterfaceBase):
     #  - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet)
     ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
     ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
+    ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl
 
     if not ret.openpilotLongitudinalControl:
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
 
+    if ret.enableGasInterceptor:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_GAS_INTERCEPTOR
+
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED

selfdrive/controls/controlsd.py

@@ -2,6 +2,7 @@
 import os
 import math
 import time
+import threading
 from typing import SupportsFloat
 
 from cereal import car, log
@@ -65,6 +66,9 @@ DP_VAG_TIMEBOMB_BYPASS_WARNING = 34000
 DP_VAG_TIMEBOMB_BYPASS_START = 345000
 DP_VAG_TIMEBOMB_BYPASS_END = 348000
 
+DP_LONG_MISSING_LEAD_COUNT = 2. / DT_CTRL
+DP_LONG_MISSING_LEAD_SPEED = 19.44  # 70 kph
+
 class Controls:
   def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
     config_realtime_process(4 if TICI else 3, Priority.CTRL_HIGH)
@@ -103,6 +107,9 @@ class Controls:
     self._dp_vag_timebomb_bypass = self.params.get_bool("dp_vag_timebomb_bypass")
     self._dp_lat_lane_change_assist_disabled = int(self.params.get("dp_lat_lane_change_assist_speed", encoding="utf-8")) == 0
     self._dp_lat_lane_change_assist_disabled_active = False
+    self._dp_long_missing_lead_warning = self.params.get_bool("dp_long_missing_lead_warning")
+    self._dp_long_missing_lead_count = 0
+    self._dp_long_missing_lead_prev = False
     self.sm = sm
     if self.sm is None:
       ignore = ['testJoystick']
@@ -271,6 +278,26 @@ class Controls:
     if self.read_only:
       return
 
+    # lead missing alert
+    # when driving on highway and the lead car suddenly gone missing, hazard ahead?
+    if self._dp_long_missing_lead_warning and CS.vEgo >= DP_LONG_MISSING_LEAD_SPEED:
+      _dp_long_missing_lead = not self.sm['longitudinalPlan'].hasLead
+
+      # lead vehicle missing started
+      if not self._dp_long_missing_lead_prev and _dp_long_missing_lead:
+        self._dp_long_missing_lead_count = DP_LONG_MISSING_LEAD_COUNT
+
+      # only send event when counter reach 0
+      if self._dp_long_missing_lead_count > 0:
+        if CS.steeringPressed or CS.brakePressed or not _dp_long_missing_lead:
+          self._dp_long_missing_lead_count = 0
+        else:
+          self._dp_long_missing_lead_count -= 1
+          if self._dp_long_missing_lead_count == 0:
+            self.events.add(EventName.promptDriverDistracted)
+
+      self._dp_long_missing_lead_prev = _dp_long_missing_lead
+
     # ALKA combination
     if self._dp_alka and CS.brakePressed:
       # rick - allow ALKA to be enabled/disabled when brake + main pressed twice in 0.5 secs
@@ -963,18 +990,10 @@ class Controls:
 
   def step(self):
     start_time = time.monotonic()
-    self.prof.checkpoint("Ratekeeper", ignore=True)
-
-    self.is_metric = self.params.get_bool("IsMetric")
-    self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
-    # rick - we should disable experimental mode on radarless car w/ 0.8.13 model
-    if self.CP.radarUnavailable and self.dp_0813:
-      self.experimental_mode = False
 
     # Sample data from sockets and get a carState
     CS = self.data_sample()
     cloudlog.timestamp("Data sampled")
-    self.prof.checkpoint("Sample")
 
     self.update_events(CS)
     cloudlog.timestamp("Events updated")
@@ -982,24 +1001,37 @@ class Controls:
     if not self.read_only and self.initialized:
       # Update control state
       self.state_transition(CS)
-      self.prof.checkpoint("State transition")
 
     # Compute actuators (runs PID loops and lateral MPC)
     CC, lac_log = self.state_control(CS)
 
-    self.prof.checkpoint("State Control")
-
     # Publish data
     self.publish_logs(CS, start_time, CC, lac_log)
-    self.prof.checkpoint("Sent")
 
     self.CS_prev = CS
 
+  def params_thread(self, evt):
+    while not evt.is_set():
+      self.is_metric = self.params.get_bool("IsMetric")
+      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      # rick - we should disable experimental mode on radarless car w/ 0.8.13 model
+      if self.CP.radarUnavailable and self.dp_0813:
+        self.experimental_mode = False
+      if self.CP.notCar:
+        self.joystick_mode = self.params.get_bool("JoystickDebugMode")
+      time.sleep(0.1)
+
   def controlsd_thread(self):
-    while True:
-      self.step()
-      self.rk.monitor_time()
-      self.prof.display()
+    e = threading.Event()
+    t = threading.Thread(target=self.params_thread, args=(e, ))
+    try:
+      t.start()
+      while True:
+        self.step()
+        self.rk.monitor_time()
+    except SystemExit:
+      e.set()
+      t.join()
 
 
 def main(sm=None, pm=None, logcan=None):

selfdrive/controls/lib/dynamic_endtoend_controller.py

@@ -30,7 +30,7 @@ TRAJECTORY_SIZE = 33
 LEAD_WINDOW_SIZE = 3
 LEAD_PROB = 0.5
 
-SLOW_DOWN_WINDOW_SIZE = 3
+SLOW_DOWN_WINDOW_SIZE = 5
 SLOW_DOWN_PROB = 0.5
 SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55.]
 SLOW_DOWN_DIST = [10, 30., 50., 70., 80., 90., 120.]
@@ -51,6 +51,8 @@ SET_MODE_TIMEOUT = 10
 MPC_FCW_WINDOW_SIZE = 10
 MPC_FCW_PROB = 0.5
 
+V_ACC_MIN = 9.72
+
 class SNG_State:
   off = 0
   stopped = 1
@@ -187,11 +189,11 @@ class DynamicEndtoEndController:
       self._set_mode('blended')
       return
 
-    # # when blinker is on and speed is driving below highway cruise speed: blended
-    # # we dont want it to switch mode at higher speed, blended may trigger hard brake
-    # if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
-    #   self._set_mode('blended')
-    #   return
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we dont want it to switch mode at higher speed, blended may trigger hard brake
+    if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+      self._set_mode('blended')
+      return
 
     # when at highway cruise and SNG: blended
     # ensuring blended mode is used because acc is bad at catching SNG lead car
@@ -223,7 +225,7 @@ class DynamicEndtoEndController:
       self._set_mode('acc')
       return
 
-    self._set_mode('blended')
+    self._set_mode('acc')
 
   def _radar_mode(self):
     # when mpc fcw crash prob is high

selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json

@@ -345,7 +345,7 @@
     },
     "json_file": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
     "model": {
-        "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachaaaaaaaaaaaagbaeegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
+        "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
         "con_h_expr_e": null,
         "con_phi_expr": null,
         "con_phi_expr_e": null,
@@ -368,9 +368,9 @@
         "cost_r_in_psi_expr": null,
         "cost_r_in_psi_expr_0": null,
         "cost_r_in_psi_expr_e": null,
-        "cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachaaaaaaaaaaaagbaeegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
-        "cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachaaaaaaaaaaaagbaeegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
-        "cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachaaaaaaaaaaaagbaeegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbg",
+        "cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbg",
         "disc_dyn_expr": null,
         "dyn_disc_fun": null,
         "dyn_disc_fun_jac": null,

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -54,7 +54,7 @@ T_IDXS = np.array(T_IDXS_LST)
 FCW_IDXS = T_IDXS < 5.0
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 COMFORT_BRAKE = 2.5
-STOP_DISTANCE = 5.5
+STOP_DISTANCE = 6.0
 
 def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed:
@@ -79,14 +79,14 @@ def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
 
 def get_dynamic_follow(v_ego, personality=log.LongitudinalPersonality.standard):
   if personality==log.LongitudinalPersonality.relaxed:
-    x_vel =  [0,    3.05,   3.61,   4.16,   7.14,   11.11]
-    y_dist = [1.75, 1.75, 1.77, 1.75, 1.8,  1.8]
+    x_vel =  [0.0,  3.0,  8.33,  13.90,  20,    25,    40]
+    y_dist = [1.2,  1.25, 1.40,  1.40,   1.50,  1.85,  2.0]
   elif personality==log.LongitudinalPersonality.standard:
-    x_vel =  [0,    3.05,   3.61,   4.16,   7.14,   11.11]
-    y_dist = [1.5,  1.5,  1.51,  1.5,  1.5,  1.45]
+    x_vel =  [0.0,  3.0,  8.33,  13.90,  20,    25,    40]
+    y_dist = [1.00,  1.00, 1.20,  1.20,   1.25,  1.45,  1.5]
   elif personality==log.LongitudinalPersonality.aggressive:
-    x_vel =  [0,    3.05,   3.61,   4.16,   7.14,   11.11]
-    y_dist = [1.12, 1.12, 1.13, 1.12, 1.22, 1.22]
+    x_vel =  [0.0,  4.00, 8.33,  13.89,  20,    25,    40]
+    y_dist = [0.8,  0.80, 0.90,  0.90,   0.9,  1.105, 1.12]
   else:
     raise NotImplementedError("Dynamic Follow personality not supported")
   return np.interp(v_ego, x_vel, y_dist)
@@ -109,8 +109,8 @@ def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego):
   if np.all(v_lead - v_ego > 0):
     v_diff_offset = ((v_lead - v_ego) * 1.)
     v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2)
-    v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego**2)/10), 0)
-  distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset * 1.2
+    v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0)
+  distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
   return distance
 
 def gen_long_model():

selfdrive/controls/lib/longitudinal_planner.py

@@ -30,6 +30,7 @@ A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
+V_ACC_MIN = 9.72  # 35 kph
 
 def get_max_accel(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
@@ -174,7 +175,7 @@ class LongitudinalPlanner:
     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, taco=True)
-    self.dp_long_use_krkeegen_tune_active = self.dp_long_use_krkeegen_tune and v_ego <= 7.5
+    self.dp_long_use_krkeegen_tune_active = self.dp_long_use_krkeegen_tune and v_ego <= V_ACC_MIN
     self.dp_long_use_df_tune_active = self.dp_long_use_df_tune and sm['radarState'].leadOne.status
     self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality, use_df_tune=self.dp_long_use_df_tune_active, use_krkeegen_tune=self.dp_long_use_krkeegen_tune_active)
 

selfdrive/hybrid_modeld/transforms/transform.cl

@@ -22,17 +22,19 @@ __kernel void warpPerspective(__global const uchar * src,
         W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f;
         int X = rint(X0 * W), Y = rint(Y0 * W);
 
-        // Clamp coordinates to stay within valid range
-        int sx = clamp(convert_int(X >> INTER_BITS), 0, src_cols - 1);
-        int sy = clamp(convert_int(Y >> INTER_BITS), 0, src_rows - 1);
-
+        short sx = convert_short_sat(X >> INTER_BITS);
+        short sy = convert_short_sat(Y >> INTER_BITS);
         short ay = (short)(Y & (INTER_TAB_SIZE - 1));
         short ax = (short)(X & (INTER_TAB_SIZE - 1));
 
-        int v0 = convert_int(src[mad24(sy, src_step, src_offset + sx)]);
-        int v1 = convert_int(src[mad24(sy, src_step, src_offset + (sx+1))]);
-        int v2 = convert_int(src[mad24(sy+1, src_step, src_offset + sx)]);
-        int v3 = convert_int(src[mad24(sy+1, src_step, src_offset + (sx+1))]);
+        int v0 = (sx >= 0 && sx < src_cols && sy >= 0 && sy < src_rows) ?
+            convert_int(src[mad24(sy, src_step, src_offset + sx)]) : 0;
+        int v1 = (sx+1 >= 0 && sx+1 < src_cols && sy >= 0 && sy < src_rows) ?
+            convert_int(src[mad24(sy, src_step, src_offset + (sx+1))]) : 0;
+        int v2 = (sx >= 0 && sx < src_cols && sy+1 >= 0 && sy+1 < src_rows) ?
+            convert_int(src[mad24(sy+1, src_step, src_offset + sx)]) : 0;
+        int v3 = (sx+1 >= 0 && sx+1 < src_cols && sy+1 >= 0 && sy+1 < src_rows) ?
+            convert_int(src[mad24(sy+1, src_step, src_offset + (sx+1))]) : 0;
 
         float taby = 1.f/INTER_TAB_SIZE*ay;
         float tabx = 1.f/INTER_TAB_SIZE*ax;

selfdrive/locationd/models/generated/car.h

@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_8989390406176655642);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_872949834645227037);
-void car_H_mod_fun(double *state, double *out_6190064514277074980);
-void car_f_fun(double *state, double dt, double *out_5737342647877877229);
-void car_F_fun(double *state, double dt, double *out_6498734987852944578);
-void car_h_25(double *state, double *unused, double *out_358371148521877751);
-void car_H_25(double *state, double *unused, double *out_3752758241963538269);
-void car_h_24(double *state, double *unused, double *out_4557978488111553664);
-void car_H_24(double *state, double *unused, double *out_993751085002970765);
-void car_h_30(double *state, double *unused, double *out_7522901502232171434);
-void car_H_30(double *state, double *unused, double *out_6271091200470786896);
-void car_h_26(double *state, double *unused, double *out_2521228234571325789);
-void car_H_26(double *state, double *unused, double *out_7057284211724338870);
-void car_h_27(double *state, double *unused, double *out_1301881001423540186);
-void car_H_27(double *state, double *unused, double *out_8494685271654730113);
-void car_h_29(double *state, double *unused, double *out_2895729189507225245);
-void car_H_29(double *state, double *unused, double *out_6781322544785179080);
-void car_h_28(double *state, double *unused, double *out_9068663034437199427);
-void car_H_28(double *state, double *unused, double *out_1698923527715648506);
-void car_h_31(double *state, double *unused, double *out_3995820676775900101);
-void car_H_31(double *state, double *unused, double *out_6431076109490987394);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5511008102036755857);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_8300600328275299875);
+void car_H_mod_fun(double *state, double *out_3128679903101307724);
+void car_f_fun(double *state, double dt, double *out_3553875974525626115);
+void car_F_fun(double *state, double dt, double *out_353016053043653311);
+void car_h_25(double *state, double *unused, double *out_6199961577350591037);
+void car_H_25(double *state, double *unused, double *out_5728077160425784390);
+void car_h_24(double *state, double *unused, double *out_4286546153885119283);
+void car_H_24(double *state, double *unused, double *out_5275992320295050906);
+void car_h_30(double *state, double *unused, double *out_570873648999759899);
+void car_H_30(double *state, double *unused, double *out_8190970583156159028);
+void car_h_26(double *state, double *unused, double *out_372636203901902536);
+void car_H_26(double *state, double *unused, double *out_8977163594409711002);
+void car_h_27(double *state, double *unused, double *out_7245109228698114934);
+void car_H_27(double *state, double *unused, double *out_8032179419369449371);
+void car_h_29(double *state, double *unused, double *out_4430799618661085746);
+void car_H_29(double *state, double *unused, double *out_8701201927470551212);
+void car_h_28(double *state, double *unused, double *out_2243435961037269289);
+void car_H_28(double *state, double *unused, double *out_7781911874673674153);
+void car_h_31(double *state, double *unused, double *out_4483517239538288138);
+void car_H_31(double *state, double *unused, double *out_8350955492176359526);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);

selfdrive/locationd/models/generated/gnss.h

@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_8157223566737921768);
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_2044125793472420016);
-void gnss_H_mod_fun(double *state, double *out_9173041979963174697);
-void gnss_f_fun(double *state, double dt, double *out_1618057822244112223);
-void gnss_F_fun(double *state, double dt, double *out_2095238804530870251);
-void gnss_h_6(double *state, double *sat_pos, double *out_8758992661908707843);
-void gnss_H_6(double *state, double *sat_pos, double *out_3561767187659398853);
-void gnss_h_20(double *state, double *sat_pos, double *out_2943069687929850059);
-void gnss_H_20(double *state, double *sat_pos, double *out_2968919167798348741);
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_4774871104966573529);
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_149136905432068571);
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_4774871104966573529);
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_149136905432068571);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_1997882145746187898);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_7977352512819017738);
+void gnss_H_mod_fun(double *state, double *out_8329454369015276923);
+void gnss_f_fun(double *state, double dt, double *out_5966521461067562449);
+void gnss_F_fun(double *state, double dt, double *out_3265364062710352632);
+void gnss_h_6(double *state, double *sat_pos, double *out_8740291259499609875);
+void gnss_H_6(double *state, double *sat_pos, double *out_6994153009562781871);
+void gnss_h_20(double *state, double *sat_pos, double *out_8702166267876168587);
+void gnss_H_20(double *state, double *sat_pos, double *out_2501154242205932671);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_5603600966869776746);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_950292981380994835);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_5603600966869776746);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_950292981380994835);
 void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/locationd/models/generated/live.h

@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
 void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_6974567924666299102);
-void live_err_fun(double *nom_x, double *delta_x, double *out_2493100912041570996);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_7217190665066253352);
-void live_H_mod_fun(double *state, double *out_6899062083944887710);
-void live_f_fun(double *state, double dt, double *out_252278658732009113);
-void live_F_fun(double *state, double dt, double *out_4169121626430309510);
-void live_h_4(double *state, double *unused, double *out_2184045668377535913);
-void live_H_4(double *state, double *unused, double *out_5091087556127760923);
-void live_h_9(double *state, double *unused, double *out_8158653303070436890);
-void live_H_9(double *state, double *unused, double *out_2196131379136686547);
-void live_h_10(double *state, double *unused, double *out_6830468309703972597);
-void live_H_10(double *state, double *unused, double *out_8221089086801312150);
-void live_h_12(double *state, double *unused, double *out_3698550038738074472);
-void live_H_12(double *state, double *unused, double *out_6974398140539057697);
-void live_h_35(double *state, double *unused, double *out_3507821483290070437);
-void live_H_35(double *state, double *unused, double *out_1724425498755153547);
-void live_h_32(double *state, double *unused, double *out_1049805558840064954);
-void live_H_32(double *state, double *unused, double *out_1593260713814413078);
-void live_h_13(double *state, double *unused, double *out_1881416958735212550);
-void live_H_13(double *state, double *unused, double *out_5911629335895004065);
-void live_h_14(double *state, double *unused, double *out_8158653303070436890);
-void live_H_14(double *state, double *unused, double *out_2196131379136686547);
-void live_h_33(double *state, double *unused, double *out_7532622185414820130);
-void live_H_33(double *state, double *unused, double *out_8472160794518560882);
+void live_H(double *in_vec, double *out_8523600387998743663);
+void live_err_fun(double *nom_x, double *delta_x, double *out_397224092559137986);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_2649753097327329786);
+void live_H_mod_fun(double *state, double *out_3318911277463317602);
+void live_f_fun(double *state, double dt, double *out_5229533073546206660);
+void live_F_fun(double *state, double dt, double *out_8885458061524429284);
+void live_h_4(double *state, double *unused, double *out_5760213005657753023);
+void live_H_4(double *state, double *unused, double *out_5017816733173915125);
+void live_h_9(double *state, double *unused, double *out_3101841756117904645);
+void live_H_9(double *state, double *unused, double *out_6141708405271189021);
+void live_h_10(double *state, double *unused, double *out_4743256218283672467);
+void live_H_10(double *state, double *unused, double *out_8791731730799357953);
+void live_h_12(double *state, double *unused, double *out_2579169362312866661);
+void live_H_12(double *state, double *unused, double *out_5638915758221508792);
+void live_h_35(double *state, double *unused, double *out_7312630217986212032);
+void live_H_35(double *state, double *unused, double *out_8384478790546522501);
+void live_h_32(double *state, double *unused, double *out_117205148134792667);
+void live_H_32(double *state, double *unused, double *out_232577830109754697);
+void live_h_13(double *state, double *unused, double *out_8001914803084542081);
+void live_H_13(double *state, double *unused, double *out_3456886129192004094);
+void live_h_14(double *state, double *unused, double *out_3101841756117904645);
+void live_H_14(double *state, double *unused, double *out_6141708405271189021);
+void live_h_33(double *state, double *unused, double *out_505168331143758912);
+void live_H_33(double *state, double *unused, double *out_6911708278524171511);
 void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/manager/manager.py

@@ -87,6 +87,7 @@ def manager_init() -> None:
     ("dp_lat_lane_change_assist_speed", "20"),
     ("dp_device_display_flight_panel", "0"),
     ("dp_ui_rainbow", "0"),
+    ("dp_long_missing_lead_warning", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))

selfdrive/updated.py

@@ -17,8 +17,8 @@ from markdown_it import MarkdownIt
 from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
 from openpilot.common.time import system_time_valid
-from openpilot.system.hardware import AGNOS, HARDWARE
-from openpilot.system.swaglog import cloudlog
+from openpilot.system.hardware import AGNOS, HARDWARE, EON
+from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
 from openpilot.system.version import is_tested_branch
 
@@ -37,26 +37,42 @@ OVERLAY_INIT = Path(os.path.join(BASEDIR, ".overlay_init"))
 DAYS_NO_CONNECTIVITY_MAX = 14     # do not allow to engage after this many days
 DAYS_NO_CONNECTIVITY_PROMPT = 10  # send an offroad prompt after this many days
 
+class UserRequest:
+  NONE = 0
+  CHECK = 1
+  FETCH = 2
+
 class WaitTimeHelper:
   def __init__(self):
     self.ready_event = threading.Event()
-    self.only_check_for_update = False
+    self.user_request = UserRequest.NONE
     signal.signal(signal.SIGHUP, self.update_now)
     signal.signal(signal.SIGUSR1, self.check_now)
 
   def update_now(self, signum: int, frame) -> None:
     cloudlog.info("caught SIGHUP, attempting to downloading update")
-    self.only_check_for_update = False
+    self.user_request = UserRequest.FETCH
     self.ready_event.set()
 
   def check_now(self, signum: int, frame) -> None:
     cloudlog.info("caught SIGUSR1, checking for updates")
-    self.only_check_for_update = True
+    self.user_request = UserRequest.CHECK
     self.ready_event.set()
 
   def sleep(self, t: float) -> None:
     self.ready_event.wait(timeout=t)
 
+def write_time_to_param(params, param) -> None:
+  t = datetime.datetime.utcnow()
+  params.put(param, t.isoformat().encode('utf8'))
+
+def read_time_from_param(params, param) -> datetime.datetime:
+  t = params.get(param, encoding='utf8')
+  try:
+    return datetime.datetime.fromisoformat(t)
+  except (TypeError, ValueError):
+    pass
+  return None
 
 def run(cmd: List[str], cwd: Optional[str] = None) -> str:
   return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8')
@@ -73,7 +89,8 @@ def set_consistent_flag(consistent: bool) -> None:
 
 def parse_release_notes(basedir: str) -> bytes:
   try:
-    with open(os.path.join(basedir, "RELEASES.md"), "rb") as f:
+    version_type = "R2" if os.path.isfile(os.path.join(basedir, "prebuilt")) else "D2"
+    with open(os.path.join(basedir, f"CHANGELOGS-{version_type}.md"), "rb") as f:
       r = f.read().split(b'\n\n', 1)[0]  # Slice latest release notes
     try:
       return bytes(MarkdownIt().render(r.decode("utf-8")), encoding="utf-8")
@@ -135,8 +152,10 @@ def init_overlay() -> None:
   params.put_bool("UpdateAvailable", False)
   set_consistent_flag(False)
   dismount_overlay()
+  args = ["rm", "-rf", STAGING_ROOT]
   if AGNOS:
-    run(["sudo", "rm", "-rf", STAGING_ROOT])
+    args = ["sudo"] + args
+    run(args)
   if os.path.isdir(STAGING_ROOT):
     shutil.rmtree(STAGING_ROOT)
 
@@ -244,7 +263,7 @@ def handle_neos_update() -> None:
 class Updater:
   def __init__(self):
     self.params = Params()
-    self.branches = defaultdict(lambda: '')
+    self.branches = defaultdict(str)
     self._has_internet: bool = False
 
   @property
@@ -253,10 +272,9 @@ class Updater:
 
   @property
   def target_branch(self) -> str:
-    b: Union[str, None] = self.params.get("UpdaterTargetBranch", encoding='utf-8')
+    b: str | None = self.params.get("UpdaterTargetBranch", encoding='utf-8')
     if b is None:
       b = self.get_branch(BASEDIR)
-      self.params.put("UpdaterTargetBranch", b)
     return b
 
   @property
@@ -271,7 +289,7 @@ class Updater:
 
   @property
   def update_available(self) -> bool:
-    if os.path.isdir(OVERLAY_MERGED):
+    if os.path.isdir(OVERLAY_MERGED) and len(self.branches) > 0:
       hash_mismatch = self.get_commit_hash(OVERLAY_MERGED) != self.branches[self.target_branch]
       branch_mismatch = self.get_branch(OVERLAY_MERGED) != self.target_branch
       return hash_mismatch or branch_mismatch
@@ -285,20 +303,19 @@ class Updater:
 
   def set_params(self, update_success: bool, failed_count: int, exception: Optional[str]) -> None:
     self.params.put("UpdateFailedCount", str(failed_count))
+    self.params.put("UpdaterTargetBranch", self.target_branch)
 
     self.params.put_bool("UpdaterFetchAvailable", self.update_available)
-    self.params.put("UpdaterAvailableBranches", ','.join(self.branches.keys()))
+    if len(self.branches):
+      self.params.put("UpdaterAvailableBranches", ','.join(self.branches.keys()))
 
     last_update = datetime.datetime.utcnow()
     if update_success:
-      t = last_update.isoformat()
-      self.params.put("LastUpdateTime", t.encode('utf8'))
+      write_time_to_param(self.params, "LastUpdateTime")
     else:
-      try:
-        t = self.params.get("LastUpdateTime", encoding='utf8')
-        last_update = datetime.datetime.fromisoformat(t)
-      except (TypeError, ValueError):
-        pass
+      t = read_time_from_param(self.params, "LastUpdateTime")
+      if t is not None:
+        last_update = t
 
     if exception is None:
       self.params.remove("LastUpdateException")
@@ -344,17 +361,17 @@ class Updater:
       else:
         extra_text = exception
       set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text)
-    elif failed_count > 0:
-      if dt.days > DAYS_NO_CONNECTIVITY_MAX:
-        set_offroad_alert("Offroad_ConnectivityNeeded", True)
-      elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
-        remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1)
-        set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.")
+    # elif failed_count > 0:
+    #   if dt.days > DAYS_NO_CONNECTIVITY_MAX:
+    #     set_offroad_alert("Offroad_ConnectivityNeeded", True)
+    #   elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
+    #     remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1)
+    #     set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.")
 
   def check_for_update(self) -> None:
     cloudlog.info("checking for updates")
 
-    excluded_branches = ('release2', 'release2-staging', 'dashcam', 'dashcam-staging')
+    excluded_branches = ('beta3', 'r3', 'd3')
 
     try:
       run(["git", "ls-remote", "origin", "HEAD"], OVERLAY_MERGED)
@@ -422,97 +439,100 @@ class Updater:
 def main() -> None:
   params = Params()
 
-  updater = Updater()
-  update_failed_count = 0  # TODO: Load from param?
-  exception = None
-
   if params.get_bool("DisableUpdates"):
-    updater.set_params(False, update_failed_count, exception)
     cloudlog.warning("updates are disabled by the DisableUpdates param")
     exit(0)
 
-  ov_lock_fd = open(LOCK_FILE, 'w')
-  try:
-    fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
-  except OSError as e:
-    raise RuntimeError("couldn't get overlay lock; is another instance running?") from e
-
-  # Set low io priority
-  proc = psutil.Process()
-  if psutil.LINUX:
-    proc.ionice(psutil.IOPRIO_CLASS_BE, value=7)
-
-  # Check if we just performed an update
-  if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir():
-    cloudlog.event("update installed")
-
-  if not params.get("InstallDate"):
-    t = datetime.datetime.utcnow().isoformat()
-    params.put("InstallDate", t.encode('utf8'))
-
-  # updater = Updater()
-  # update_failed_count = 0 # TODO: Load from param?
-
-  # no fetch on the first time
-  wait_helper = WaitTimeHelper()
-  wait_helper.only_check_for_update = True
-
-  # invalidate old finalized update
-  set_consistent_flag(False)
-
-  # Run the update loop
-  while True:
-    wait_helper.ready_event.clear()
-
-    # Attempt an update
-    # exception = None
+  with open(LOCK_FILE, 'w') as ov_lock_fd:
     try:
-      # TODO: reuse overlay from previous updated instance if it looks clean
-      init_overlay()
+      fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+    except OSError as e:
+      raise RuntimeError("couldn't get overlay lock; is another instance running?") from e
 
-      # ensure we have some params written soon after startup
-      updater.set_params(False, update_failed_count, exception)
+    # Set low io priority
+    proc = psutil.Process()
+    if psutil.LINUX:
+      proc.ionice(psutil.IOPRIO_CLASS_BE, value=7)
 
-      if not system_time_valid():
-        wait_helper.sleep(60)
-        continue
+    # Check if we just performed an update
+    if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir():
+      cloudlog.event("update installed")
 
-      update_failed_count += 1
+    if not params.get("InstallDate"):
+      t = datetime.datetime.utcnow().isoformat()
+      params.put("InstallDate", t.encode('utf8'))
 
-      # check for update
-      params.put("UpdaterState", "checking...")
-      updater.check_for_update()
+    updater = Updater()
+    update_failed_count = 0 # TODO: Load from param?
+    wait_helper = WaitTimeHelper()
 
-      # download update
-      if wait_helper.only_check_for_update:
-        cloudlog.info("skipping fetch this cycle")
-      else:
-        updater.fetch_update()
-      update_failed_count = 0
-    except subprocess.CalledProcessError as e:
-      cloudlog.event(
-        "update process failed",
-        cmd=e.cmd,
-        output=e.output,
-        returncode=e.returncode
-      )
-      exception = f"command failed: {e.cmd}\n{e.output}"
-      OVERLAY_INIT.unlink(missing_ok=True)
-    except Exception as e:
-      cloudlog.exception("uncaught updated exception, shouldn't happen")
-      exception = str(e)
-      OVERLAY_INIT.unlink(missing_ok=True)
+    # invalidate old finalized update
+    set_consistent_flag(False)
 
-    try:
-      params.put("UpdaterState", "idle")
-      update_successful = (update_failed_count == 0)
-      updater.set_params(update_successful, update_failed_count, exception)
-    except Exception:
-      cloudlog.exception("uncaught updated exception while setting params, shouldn't happen")
+    # set initial state
+    params.put("UpdaterState", "idle")
 
-    # infrequent attempts if we successfully updated recently
-    wait_helper.only_check_for_update = False
-    wait_helper.sleep(5*60 if update_failed_count > 0 else 1.5*60*60)
+    # Run the update loop
+    first_run = True
+    while True:
+      wait_helper.ready_event.clear()
+
+      # Attempt an update
+      exception = None
+      try:
+        # TODO: reuse overlay from previous updated instance if it looks clean
+        init_overlay()
+
+        # ensure we have some params written soon after startup
+        updater.set_params(False, update_failed_count, exception)
+
+        if not system_time_valid() or first_run:
+          first_run = False
+          wait_helper.sleep(60)
+          continue
+
+        update_failed_count += 1
+
+        # check for update
+        params.put("UpdaterState", "checking...")
+        updater.check_for_update()
+
+        # download update
+        last_fetch = read_time_from_param(params, "UpdaterLastFetchTime")
+        timed_out = last_fetch is None or (datetime.datetime.utcnow() - last_fetch > datetime.timedelta(days=3))
+        user_requested_fetch = wait_helper.user_request == UserRequest.FETCH
+        if params.get_bool("NetworkMetered") and not timed_out and not user_requested_fetch:
+          cloudlog.info("skipping fetch, connection metered")
+        elif wait_helper.user_request == UserRequest.CHECK:
+          cloudlog.info("skipping fetch, only checking")
+        else:
+          updater.fetch_update()
+          write_time_to_param(params, "UpdaterLastFetchTime")
+        update_failed_count = 0
+      except subprocess.CalledProcessError as e:
+        cloudlog.event(
+          "update process failed",
+          cmd=e.cmd,
+          output=e.output,
+          returncode=e.returncode
+        )
+        exception = f"command failed: {e.cmd}\n{e.output}"
+        OVERLAY_INIT.unlink(missing_ok=True)
+      except Exception as e:
+        cloudlog.exception("uncaught updated exception, shouldn't happen")
+        exception = str(e)
+        OVERLAY_INIT.unlink(missing_ok=True)
+
+      try:
+        params.put("UpdaterState", "idle")
+        update_successful = (update_failed_count == 0)
+        updater.set_params(update_successful, update_failed_count, exception)
+      except Exception:
+        cloudlog.exception("uncaught updated exception while setting params, shouldn't happen")
+
+      # infrequent attempts if we successfully updated recently
+      wait_helper.user_request = UserRequest.NONE
+      wait_helper.sleep(5*60 if update_failed_count > 0 else 1.5*60*60)
 
 
 if __name__ == "__main__":

system/version.py

@@ -51,13 +51,16 @@ def get_branch() -> str:
 
 @cache
 def get_origin() -> str:
+  origin = ""
   try:
     local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"])
     tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"])
-    return run_cmd(["git", "config", "remote." + tracking_remote + ".url"])
+    origin = run_cmd(["git", "config", "remote." + tracking_remote + ".url"])
   except subprocess.CalledProcessError:  # Not on a branch, fallback
-    return run_cmd_default(["git", "config", "--get", "remote.origin.url"])
-
+    origin = run_cmd_default(["git", "config", "--get", "remote.origin.url"])
+  except TypeError:
+    pass
+  return "" if origin is None else origin
 
 @cache
 def get_normalized_origin() -> str: