dragonpilot beta3

date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295

selfdrive/controls/controlsd.py

@@ -1,32 +1,33 @@
 #!/usr/bin/env python3
 import os
 import math
+import time
 from typing import SupportsFloat
 
 from cereal import car, log
-from common.numpy_fast import clip
-from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL
-from common.profiler import Profiler
-from common.params import Params, put_nonblocking, put_bool_nonblocking
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
+from openpilot.common.profiler import Profiler
+from openpilot.common.params import Params, put_nonblocking, put_bool_nonblocking
 import cereal.messaging as messaging
 from cereal.visionipc import VisionIpcClient, VisionStreamType
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 from panda import ALTERNATIVE_EXPERIENCE
-from system.swaglog import cloudlog
-from system.version import is_release_branch, get_short_branch
-from selfdrive.boardd.boardd import can_list_to_can_capnp
-from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
-from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
-from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
-from selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
-from selfdrive.controls.lib.longcontrol import LongControl
-from selfdrive.controls.lib.latcontrol_pid import LatControlPID
-from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
-from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
-from selfdrive.controls.lib.events import Events, ET
-from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
-from selfdrive.controls.lib.vehicle_model import VehicleModel
-from system.hardware import HARDWARE
+from openpilot.system.swaglog import cloudlog
+from openpilot.system.version import is_release_branch, get_short_branch
+from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
+from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
+from openpilot.selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.controls.lib.events import Events, ET
+from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
+from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.system.hardware import HARDWARE
 
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
@@ -39,7 +40,6 @@ DP_LONG_MISSING_LEAD_SPEED = 22.22 # 80 kph
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
 TESTING_CLOSET = "TESTING_CLOSET" in os.environ
-NOSENSOR = "NOSENSOR" in os.environ
 IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
 # dp related processes
 IGNORE_PROCESSES.update({"mapd", "otisserv", "fileserv", "gpxd", "gpx_uploader"})
@@ -74,6 +74,7 @@ class Controls:
       self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
                                      'carControl', 'carEvents', 'carParams', 'controlsStateExt'])
 
+    self.sensor_packets = ["accelerometer", "gyroscope"]
     self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
     self.dp_device_dm_unavailable_once = False
     if dp_device_dm_unavailable:
@@ -98,12 +99,13 @@ class Controls:
     self.dp_device_disable_temp_check = self.params.get_bool("dp_device_disable_temp_check")
     self.sm = sm
     if self.sm is None:
-      ignore = ['testJoystick']
+      ignore = self.sensor_packets + ['testJoystick']
       if SIMULATION:
         ignore += ['driverCameraState', 'managerState']
       self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                      'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
-                                     'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick', 'longitudinalPlanExt'] + self.camera_packets,
+                                     'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
+                                     'testJoystick', 'longitudinalPlanExt'] + self.camera_packets + self.sensor_packets,
                                     ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'])
 
     if CI is None:
@@ -428,17 +430,20 @@ class Controls:
     else:
       self.logged_comm_issue = None
 
-    if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
-      self.events.add(EventName.vehicleModelInvalid)
     if not self.sm['lateralPlan'].mpcSolutionValid:
       self.events.add(EventName.plannerError)
-    if not (self.sm['liveParameters'].sensorValid or self.sm['liveLocationKalman'].sensorsOK) and not NOSENSOR:
-      if self.sm.frame > 5 / DT_CTRL:  # Give locationd some time to receive all the inputs
-        self.events.add(EventName.sensorDataInvalid)
     if not self.sm['liveLocationKalman'].posenetOK:
       self.events.add(EventName.posenetInvalid)
     if not self.sm['liveLocationKalman'].deviceStable:
       self.events.add(EventName.deviceFalling)
+    if not self.sm['liveLocationKalman'].inputsOK:
+      self.events.add(EventName.locationdTemporaryError)
+    if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
+      self.events.add(EventName.paramsdTemporaryError)
+
+    # conservative HW alert. if the data or frequency are off, locationd will throw an error
+    if any((self.sm.frame - self.sm.rcv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
+      self.events.add(EventName.sensorDataInvalid)
 
     if not REPLAY:
       # Check for mismatch between openpilot and car's PCM
@@ -467,15 +472,12 @@ class Controls:
 
     # TODO: fix simulator
     if not SIMULATION or REPLAY:
-      if not NOSENSOR:
-        if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000):
-          # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
-          self.events.add(EventName.noGps)
+      if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000):
+        # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
+        self.events.add(EventName.noGps)
 
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
-      if self.sm['liveLocationKalman'].excessiveResets:
-        self.events.add(EventName.localizerMalfunction)
 
   def data_sample(self):
     """Receive data from sockets and update carState"""
@@ -611,7 +613,7 @@ class Controls:
     # Check if openpilot is engaged and actuators are enabled
     self.enabled = self.state in ENABLED_STATES
     self.active = self.state in ACTIVE_STATES
-    if self.active:
+    if self.active or self._dp_alka_active:
       self.current_alert_types.append(ET.WARNING)
 
   def state_control(self, CS):
@@ -693,8 +695,8 @@ class Controls:
 
         if CC.latActive:
           steer = clip(self.sm['testJoystick'].axes[1], -1, 1)
-          # max angle is 45 for angle-based cars
-          actuators.steer, actuators.steeringAngleDeg = steer, steer * 45.
+          # max angle is 45 for angle-based cars, max curvature is 0.02
+          actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * 45., steer * -0.02
 
         lac_log.active = self.active
         lac_log.steeringAngleDeg = CS.steeringAngleDeg
@@ -808,7 +810,7 @@ class Controls:
 
     if not self.read_only and self.initialized:
       # send car controls over can
-      now_nanos = self.can_log_mono_time if REPLAY else int(sec_since_boot() * 1e9)
+      now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
       self.last_actuators, can_sends = self.CI.apply(CC, now_nanos)
       self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
       CC.actuatorsOutput = self.last_actuators
@@ -871,8 +873,6 @@ class Controls:
       controlsState.lateralControlState.pidState = lac_log
     elif lat_tuning == 'torque':
       controlsState.lateralControlState.torqueState = lac_log
-    elif lat_tuning == 'indi':
-      controlsState.lateralControlState.indiState = lac_log
 
     self.pm.send('controlsState', dat)
 
@@ -915,7 +915,7 @@ class Controls:
     self.CC = CC
 
   def step(self):
-    start_time = sec_since_boot()
+    start_time = time.monotonic()
     self.prof.checkpoint("Ratekeeper", ignore=True)
 
     self.is_metric = self.params.get_bool("IsMetric")

selfdrive/controls/lib/accel_controller.py

@@ -1,4 +1,27 @@
-from common.numpy_fast import interp
+#!/usr/bin/env python3
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+from openpilot.common.numpy_fast import interp
 
 DP_ACCEL_STOCK = 0
 DP_ACCEL_ECO = 1
@@ -6,18 +29,15 @@ DP_ACCEL_NORMAL = 2
 DP_ACCEL_SPORT = 3
 
 # accel profile by @arne182 modified by cgw
-_DP_CRUISE_MIN_V =       [-0.765, -0.765,  -0.80, -0.80, -0.75, -0.70]
-_DP_CRUISE_MIN_V_ECO =   [-0.760, -0.760,  -0.76, -0.76, -0.70, -0.65]
-_DP_CRUISE_MIN_V_SPORT = [-0.770, -0.770,  -0.90, -1.00, -0.90, -0.80]
-_DP_CRUISE_MIN_BP =      [0.,     15.66,  17.88, 20.,   30.,   55.]
-#DP_CRUISE_MIN_BP in mph=[0.,     18,     35,    40,    45,    67,    123]
+_DP_CRUISE_MIN_V =       [-0.750, -0.750, -0.72, -0.72, -0.52, -0.40, -0.36, -0.32, -0.32,  -0.25, -0.25]
+_DP_CRUISE_MIN_V_ECO =   [-0.730, -0.730, -0.68, -0.68, -0.50, -0.36, -0.32, -0.28, -0.28,  -0.25, -0.25]
+_DP_CRUISE_MIN_V_SPORT = [-0.750, -0.750, -0.75, -0.75, -0.54, -0.42, -0.40, -0.35, -0.35,  -0.30, -0.30]
+_DP_CRUISE_MIN_BP =      [0.,     2.0,    2.01,  11.,   11.01, 18.,   18.01, 28.,   28.01,  33.,   55.]
 
-_DP_CRUISE_MAX_V =       [3.4, 2.8, 1.8, 1.4, 1.06, .88, .68,  .46, .35, .13]
-_DP_CRUISE_MAX_V_ECO =   [3.2, 2.6, 1.6, 1.2, .76,  .62, .48,  .36, .28, .09]
-_DP_CRUISE_MAX_V_SPORT = [3.5, 3.0, 2.4, 2.9, 2.1,  1.7,  1.3, .9,  .7,  .5]
+_DP_CRUISE_MAX_V =       [3.5, 3.2, 2.6, 2.0, 1.3,  0.9, .71,  .48, .36, .14]
+_DP_CRUISE_MAX_V_ECO =   [3.5, 3.0, 1.6, 1.2, .76,  .62, .48,  .36, .28, .09]
+_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 3.5, 3.5, 2.8,  2.0,  1.5, 1.0, .7,  .5]
 _DP_CRUISE_MAX_BP =      [0.,  3,   6.,  8.,  11.,  15.,  20., 25., 30., 55.]
-#DP_CRUISE_MAX_BP in mph=[0.,  6.7, 13,  18,  25,   33,   45,  56,  67,  123]
-
 
 class AccelController:
 

selfdrive/controls/lib/alertmanager.py

@@ -5,9 +5,9 @@ from collections import defaultdict
 from dataclasses import dataclass
 from typing import List, Dict, Optional
 
-from common.basedir import BASEDIR
-from common.params import Params
-from selfdrive.controls.lib.events import Alert
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.events import Alert
 
 
 with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:

selfdrive/controls/lib/desire_helper.py

@@ -1,6 +1,6 @@
 from cereal import log
-from common.conversions import Conversions as CV
-from common.realtime import DT_MDL
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_MDL
 
 LaneChangeState = log.LateralPlan.LaneChangeState
 LaneChangeDirection = log.LateralPlan.LaneChangeDirection

selfdrive/controls/lib/drive_helpers.py

@@ -1,10 +1,10 @@
 import math
 
 from cereal import car, log
-from common.conversions import Conversions as CV
-from common.numpy_fast import clip, interp
-from common.realtime import DT_MDL
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.modeld.constants import T_IDXS
 
 # WARNING: this value was determined based on the model's training distribution,
 #          model predictions above this speed can be unpredictable

selfdrive/controls/lib/dynamic_endtoend_controller.py

@@ -1,129 +1,266 @@
+#!/usr/bin/env python3
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+#
+# Version = 0.1.2
 from common.numpy_fast import interp
-from common.params import Params
 
 # d-e2e, from modeldata.h
 TRAJECTORY_SIZE = 33
 
-_DP_E2E_LEAD_COUNT = 5
+LEAD_WINDOW_SIZE = 5
+LEAD_PROB = 0.6
 
-_DP_E2E_STOP_BP = [0., 10., 20., 30., 40., 50., 55.]
-_DP_E2E_STOP_DIST = [10, 30., 50., 70., 80., 90., 120.]
-_DP_E2E_STOP_COUNT = 3
+SLOW_DOWN_WINDOW_SIZE = 5
+SLOW_DOWN_PROB = 0.6
+SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55.]
+SLOW_DOWN_DIST = [10, 30., 50., 70., 80., 90., 120.]
 
-_DP_E2E_SNG_COUNT = 3
-_DP_E2E_SNG_ACC_COUNT = 3
-_DP_E2E_SWAP_COUNT = 5
+SLOWNESS_WINDOW_SIZE = 20
+SLOWNESS_PROB = 0.6
+SLOWNESS_CRUISE_OFFSET = 1.05
 
-_DP_E2E_TF_COUNT = 5
+DANGEROUS_TTC_WINDOW_SIZE = 5
+DANGEROUS_TTC = 1.55
+
+HIGHWAY_CRUISE_KPH = 75
+
+STOP_AND_GO_FRAME = 500
+
+MODE_SWITCH_DELAY_FRAME = 500
+
+MAX_YREL_THRESHOLD = 0.25
+
+class SNG_State:
+  off = 0
+  stopped = 1
+  going = 2
+
+class GenericMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.total = 0
+
+  def add_data(self, value):
+    if len(self.data) == self.window_size:
+      self.total -= self.data.pop(0)
+    self.data.append(value)
+    self.total += value
+
+  def get_moving_average(self):
+    if len(self.data) == 0:
+      return None
+    return self.total / len(self.data)
+
+  def reset_data(self):
+    self.data = []
+    self.total = 0
 
-_DP_E2E_BLINKER_COUNT = 5
 
 class DynamicEndtoEndController:
 
   def __init__(self):
-    self._params = Params()
     self._is_enabled = False
     self._mode = 'acc'
+    self._mode_prev = 'acc'
+    self._mode_switch_allowed = True
+    self._mode_switch_frame = 0
+    self._frame = 0
 
-    # conditional e2e
-    self.dp_e2e_has_lead = False
-    self.dp_e2e_lead_last = False
-    self.dp_e2e_lead_count = 0
-    self.dp_e2e_sng = False
-    self.dp_e2e_sng_count = 0
-    self.dp_e2e_standstill_last = False
-    self.dp_e2e_swap_count = 0
-    self.dp_e2e_stop_count = 0
-    self.dp_e2e_tf_count = 0
+    self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
+    self._has_lead_filtered = False
+    self._has_lead_filtered_prev = False
 
-    self.dp_e2e_blinker_count = 0
+    self._slow_down_gmac = GenericMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
+    self._has_slow_down = False
+    self._has_slow_down_prev = False
+
+    self._has_blinkers = False
+    self._has_blinkers_prev = False
+
+    self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._has_slowness = False
+    self._has_slowness_prev = False
+
+    self._has_nav_enabled = False
+    self._has_nav_enabled_prev = False
+
+    self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._has_dangerous_ttc = False
+    self._has_dangerous_ttc_prev = False
+
+    self._v_ego_kph = 0.
+    self._v_cruise_kph = 0.
+
+    self._has_lead = False
+    self._has_lead_prev = False
+
+    self._has_standstill = False
+    self._has_standstill_prev = False
+
+    self._sng_transit_frame = 0
+    self._sng_state = SNG_State.off
     pass
 
-  def _set_dp_e2e_mode(self, mode, force=False):
-    if force:
-      self.dp_e2e_swap_count = 0
-      self._mode = mode
+  def _update(self, car_state, lead_one, md, controls_state, radar_unavailable):
+    self._v_ego_kph = car_state.vEgo * 3.6
+    self._v_cruise_kph = controls_state.vCruise
+    self._has_lead = lead_one.status
+    self._has_standstill = car_state.standstill
+
+    # nav enable detection
+    self._has_nav_enabled = md.navEnabled
+
+    # lead detection
+    self._lead_gmac.add_data(lead_one.status)
+    self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_PROB
+    self._has_lead_yrel = lead_one.yRel > MAX_YREL_THRESHOLD
+
+  # slow down detection
+    self._slow_down_gmac.add_data(len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST))
+    self._has_slow_down = self._slow_down_gmac.get_moving_average() >= SLOW_DOWN_PROB
+
+    # blinker detection
+    self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
+
+    # sng detection
+    if self._has_standstill:
+      self._sng_state = SNG_State.stopped
+      self._sng_transit_frame = 0
+    else:
+      if self._sng_transit_frame == 0:
+        if self._sng_state == SNG_State.stopped:
+          self._sng_state = SNG_State.going
+          self._sng_transit_frame = STOP_AND_GO_FRAME
+        elif self._sng_state == SNG_State.going:
+          self._sng_state = SNG_State.off
+      elif self._sng_transit_frame > 0:
+        self._sng_transit_frame -= 1
+
+    # slowness detection
+    self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET))
+    self._has_slowness = self._slowness_gmac.get_moving_average() >= SLOWNESS_PROB
+
+    # dangerous TTC detection
+    if not self._has_lead_filtered and self._has_lead_filtered_prev:
+      self._dangerous_ttc_gmac.reset_data()
+      self._has_dangerous_ttc = False
+
+    if self._has_lead and car_state.vEgo >= 0.01:
+      self._dangerous_ttc_gmac.add_data(lead_one.dRel/car_state.vEgo)
+
+    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_moving_average() is not None and self._dangerous_ttc_gmac.get_moving_average() <= DANGEROUS_TTC
+
+
+    # keep prev values
+    self._has_standstill_prev = self._has_standstill
+    self._has_slowness_prev = self._has_slowness
+    self._has_slow_down_prev = self._has_slow_down
+    self._has_lead_filtered_prev = self._has_lead_filtered
+    self._frame += 1
+
+  def _blended_priority_mode(self):
+    # 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._mode = 'blended'
       return
 
-    else:
-      # prevent switching in a short period of time.
-      if self._mode == mode:
-        self.dp_e2e_swap_count = 0
-      else:
-        self.dp_e2e_swap_count += 1
+    # when at highway cruise and SNG: blended
+    # ensuring blended mode is used because acc is bad at catching SNG lead car
+    # especially those who accel very fast and then brake very hard.
+    if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
+      self._mode = 'blended'
+      return
 
-      if self.dp_e2e_swap_count >= _DP_E2E_SWAP_COUNT:
-        self._mode = mode
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    if self._has_standstill:
+      self._mode = 'blended'
+      return
 
-  def _process_conditional_e2e(self, radar_unavailable, car_state, lead_one, md):
-    v_ego_kph = car_state.vEgo * 3.6
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._mode = 'blended'
+      return
 
-    # when blinker is on, use blended
-    if car_state.leftBlinker or car_state.rightBlinker:
-      self.dp_e2e_blinker_count += 1
-    else:
-      self.dp_e2e_blinker_count = 0
+    # when detecting lead slow down: blended
+    # use blended for higher braking capability
+    if self._has_dangerous_ttc:
+      self._mode = 'blended'
+      return
 
-    if self.dp_e2e_blinker_count > _DP_E2E_BLINKER_COUNT:
-      return self._set_dp_e2e_mode('blended', True)
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._mode = 'acc'
+      return
 
-    # make sure it see lead enough time
-    if lead_one.status != self.dp_e2e_lead_last:
-      self.dp_e2e_lead_count = 0
-    else:
-      self.dp_e2e_lead_count += 1
-      if self.dp_e2e_lead_count >= _DP_E2E_LEAD_COUNT:
-        self.dp_e2e_has_lead = lead_one.status
-    self.dp_e2e_lead_last = lead_one.status
+    self._mode = 'blended'
 
-    # when standstill, always e2e
-    if car_state.standstill:
-      self.dp_e2e_sng_count = 0
-      self.dp_e2e_sng = False
-      return self._set_dp_e2e_mode('blended')
+  def _acc_priority_mode(self):
+    # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
+    # e,g. stopped at the red light and car crossing the intersection.
+    if self._has_lead_filtered and not self._has_standstill and self._has_lead_yrel:
+      self._mode = 'acc'
+      return
 
-    if self.dp_e2e_standstill_last and not car_state.standstill:
-      self.dp_e2e_sng = True
+    # 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._mode = 'blended'
+      return
 
-    # when sng, we e2e for 0.5 secs
-    if self.dp_e2e_sng:
-      self.dp_e2e_sng_count += 1
-      if self.dp_e2e_sng_count > _DP_E2E_SNG_COUNT:
-        if self.dp_e2e_sng_count > _DP_E2E_SNG_ACC_COUNT:
-          self.dp_e2e_sng = False
-        return self._set_dp_e2e_mode('acc', True)
-      return self._set_dp_e2e_mode('blended')
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode wont brake at traffic light.
+    if self._has_standstill:
+      self._mode = 'blended'
+      return
 
-    # when we see a lead
-    if self.dp_e2e_has_lead:
-      # radar unavailable car, use blended first when distance is too close
-      if radar_unavailable:
-        if lead_one.dRel <= car_state.vEgo * 1.22:
-          self.dp_e2e_tf_count += 1
-        else:
-          self.dp_e2e_tf_count = 0
-        if self.dp_e2e_tf_count > _DP_E2E_TF_COUNT:
-          return self._set_dp_e2e_mode('blended', True)
-      # always use acc
-      else:
-        return self._set_dp_e2e_mode('acc', True)
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._mode = 'blended'
+      return
 
-    # slow down detection
-    if len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < interp(v_ego_kph, _DP_E2E_STOP_BP, _DP_E2E_STOP_DIST):
-      self.dp_e2e_stop_count += 1
-    else:
-      self.dp_e2e_stop_count = 0
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._mode = 'acc'
+      return
 
-    if self.dp_e2e_stop_count >= _DP_E2E_STOP_COUNT:
-      return self._set_dp_e2e_mode('blended', True)
+    self._mode = 'acc'
 
-    return self._set_dp_e2e_mode('acc')
-
-  def get_mpc_mode(self, mode, radar_unavailable, car_state, lead_one, md):
-    self._mode = mode
+  def get_mpc_mode(self, radar_unavailable, car_state, lead_one, md, controls_state):
     if self._is_enabled:
-      self._process_conditional_e2e(radar_unavailable, car_state, lead_one, md)
+      self._update(car_state, lead_one, md, controls_state, radar_unavailable)
+      if self._frame > self._mode_switch_frame:
+        self._mode_switch_allowed = True
+      if radar_unavailable:
+        self._blended_priority_mode()
+      else:
+        self._acc_priority_mode()
 
+    self._mode_prev = self._mode
     return self._mode
 
   def set_enabled(self, enabled):

selfdrive/controls/lib/events.py

@@ -1,6 +1,7 @@
+#!/usr/bin/env python3
 # This Python file uses the following encoding: utf-8
 # -*- coding: utf-8 -*-
-from common.i18n import events
+from openpilot.common.i18n import events
 _ = events()
 
 import math
@@ -10,10 +11,10 @@ from typing import Dict, Union, Callable, List, Optional
 
 from cereal import log, car
 import cereal.messaging as messaging
-from common.conversions import Conversions as CV
-from common.realtime import DT_CTRL
-from selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
-from system.version import get_short_branch
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
+from openpilot.system.version import get_short_branch
 
 AlertSize = log.ControlsState.AlertSize
 AlertStatus = log.ControlsState.AlertStatus
@@ -243,7 +244,7 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S
     _("Steer Unavailable Below {speed}").format(speed=get_display_speed(CP.minSteerSpeed, metric)),
     "",
     AlertStatus.userPrompt, AlertSize.small,
-    Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
+    Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
 
 
 def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
@@ -428,19 +429,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
 
   # ********** events only containing alerts that display while engaged **********
 
-  # openpilot tries to learn certain parameters about your car by observing
-  # how the car behaves to steering inputs from both human and openpilot driving.
-  # This includes:
-  # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle
-  # - tire stiffness: how much grip your tires have
-  # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight
-  # This alert is thrown when any of these values exceed a sanity check. This can be caused by
-  # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
-  EventName.vehicleModelInvalid: {
-    ET.NO_ENTRY: NoEntryAlert(_("Vehicle Parameter Identification Failed")),
-    ET.SOFT_DISABLE: soft_disable_alert(_("Vehicle Parameter Identification Failed")),
-  },
-
   EventName.steerTempUnavailableSilent: {
     ET.WARNING: Alert(
       _("Steering Temporarily Unavailable"),
@@ -510,7 +498,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
       _("Press Resume to Exit Standstill"),
       "",
       AlertStatus.userPrompt, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
+      Priority.MID, VisualAlert.none, AudibleAlert.none, .2),
   },
 
   EventName.belowSteerSpeed: {
@@ -580,11 +568,34 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
     ET.PERMANENT: NormalPermanentAlert(_("GPS Malfunction"), _("Likely Hardware Issue")),
   },
 
-  # When the GPS position and localizer diverge the localizer is reset to the
-  # current GPS position. This alert is thrown when the localizer is reset
-  # more often than expected.
-  EventName.localizerMalfunction: {
-    # ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Hardware Malfunction"),
+  EventName.locationdTemporaryError: {
+    ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"),
+    ET.SOFT_DISABLE: soft_disable_alert("locationd Temporary Error"),
+  },
+
+  EventName.locationdPermanentError: {
+    ET.NO_ENTRY: NoEntryAlert("locationd Permanent Error"),
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("locationd Permanent Error"),
+    ET.PERMANENT: NormalPermanentAlert("locationd Permanent Error"),
+  },
+
+  # openpilot tries to learn certain parameters about your car by observing
+  # how the car behaves to steering inputs from both human and openpilot driving.
+  # This includes:
+  # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle
+  # - tire stiffness: how much grip your tires have
+  # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight
+  # This alert is thrown when any of these values exceed a sanity check. This can be caused by
+  # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
+  EventName.paramsdTemporaryError: {
+    ET.NO_ENTRY: NoEntryAlert("paramsd Temporary Error"),
+    ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"),
+  },
+
+  EventName.paramsdPermanentError: {
+    ET.NO_ENTRY: NoEntryAlert("paramsd Permanent Error"),
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("paramsd Permanent Error"),
+    ET.PERMANENT: NormalPermanentAlert("paramsd Permanent Error"),
   },
 
   # ********** events that affect controls state transitions **********
@@ -682,7 +693,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
   EventName.sensorDataInvalid: {
     ET.PERMANENT: Alert(
       _("Sensor Data Invalid"),
-      _("Ensure device is mounted securely"),
+      _("Possible Hardware Issue"),
       AlertStatus.normal, AlertSize.mid,
       Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.),
     ET.NO_ENTRY: NoEntryAlert(_("Sensor Data Invalid")),
@@ -963,7 +974,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
 
 if __name__ == '__main__':
   # print all alerts by type and priority
-  from cereal.services import service_list
+  from cereal.services import SERVICE_LIST
   from collections import defaultdict, OrderedDict
 
   event_names = {v: k for k, v in EventName.schema.enumerants.items()}
@@ -971,7 +982,7 @@ if __name__ == '__main__':
 
   CP = car.CarParams.new_message()
   CS = car.CarState.new_message()
-  sm = messaging.SubMaster(list(service_list.keys()))
+  sm = messaging.SubMaster(list(SERVICE_LIST.keys()))
 
   for i, alerts in EVENTS.items():
     for et, alert in alerts.items():

selfdrive/controls/lib/lane_planner.py

@@ -1,10 +1,10 @@
 import numpy as np
 from cereal import log
-from common.filter_simple import FirstOrderFilter
-from common.numpy_fast import interp
-from common.realtime import DT_MDL
-from selfdrive.hardware import TICI
-from system.swaglog import cloudlog
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.numpy_fast import interp
+from openpilot.common.realtime import DT_MDL
+from openpilot.system.hardware import TICI
+from openpilot.system.swaglog import cloudlog
 
 
 TRAJECTORY_SIZE = 33

selfdrive/controls/lib/latcontrol.py

@@ -1,7 +1,7 @@
 from abc import abstractmethod, ABC
 
-from common.numpy_fast import clip
-from common.realtime import DT_CTRL
+from openpilot.common.numpy_fast import clip
+from openpilot.common.realtime import DT_CTRL
 
 MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
 

selfdrive/controls/lib/latcontrol_angle.py

@@ -1,7 +1,7 @@
 import math
 
 from cereal import log
-from selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,8 +1,8 @@
 import math
 
 from cereal import log
-from selfdrive.controls.lib.latcontrol import LatControl
-from selfdrive.controls.lib.pid import PIDController
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.pid import PIDController
 
 
 class LatControlPID(LatControl):

selfdrive/controls/lib/latcontrol_torque.py

@@ -1,10 +1,10 @@
 import math
 
 from cereal import log
-from common.numpy_fast import interp
-from selfdrive.controls.lib.latcontrol import LatControl
-from selfdrive.controls.lib.pid import PIDController
-from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.numpy_fast import interp
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.pid import PIDController
+from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 
 # At higher speeds (25+mph) we can assume:
 # Lateral acceleration achieved by a specific car correlates to

selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json

@@ -1,7 +1,7 @@
 {
-    "acados_include_path": "/data/openpilot/third_party/acados/include",
-    "acados_lib_path": "/data/openpilot/third_party/acados/lib",
-    "code_export_directory": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
+    "acados_include_path": "/data/dp-private/third_party/acados/include",
+    "acados_lib_path": "/data/dp-private/third_party/acados/lib",
+    "code_export_directory": "/data/dp-private/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
     "constraints": {
         "C": [],
         "C_e": [],
@@ -261,7 +261,7 @@
         "ny_e": 3,
         "nz": 0
     },
-    "json_file": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
+    "json_file": "/data/dp-private/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
     "model": {
         "con_h_expr": null,
         "con_h_expr_e": null,

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -1,16 +1,16 @@
 #!/usr/bin/env python3
 import os
+import time
 import numpy as np
 
 from casadi import SX, vertcat, sin, cos
-from common.realtime import sec_since_boot
 # WARNING: imports outside of constants will not trigger a rebuild
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.selfdrive.modeld.constants import T_IDXS
 
 if __name__ == '__main__':  # generating code
-  from third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+  from openpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
@@ -182,9 +182,9 @@ class LateralMpc():
     self.solver.set(N, "p", p_cp[N])
     self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
 
-    t = sec_since_boot()
+    t = time.monotonic()
     self.solution_status = self.solver.solve()
-    self.solve_time = sec_since_boot() - t
+    self.solve_time = time.monotonic() - t
 
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')

selfdrive/controls/lib/lateral_planner.py

@@ -1,16 +1,17 @@
+import time
 import numpy as np
-from common.realtime import sec_since_boot, DT_MDL
-from common.numpy_fast import interp
-from system.swaglog import cloudlog
-from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
-from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
-from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
-from selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.numpy_fast import interp
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
+from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 import cereal.messaging as messaging
 from cereal import log
-from selfdrive.controls.lib.lane_planner import LanePlanner
-from common.params import Params
-from selfdrive.controls.lib.road_edge_detector import RoadEdgeDetector
+from openpilot.selfdrive.controls.lib.lane_planner import LanePlanner
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.road_edge_detector import RoadEdgeDetector
 
 TRAJECTORY_SIZE = 33
 CAMERA_OFFSET = 0.04
@@ -133,7 +134,7 @@ class LateralPlanner:
 
     #  Check for infeasible MPC solution
     mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
-    t = sec_since_boot()
+    t = time.monotonic()
     if mpc_nans or self.lat_mpc.solution_status != 0:
       self.reset_mpc()
       self.x0[3] = measured_curvature * self.v_ego
@@ -157,7 +158,7 @@ class LateralPlanner:
     lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
 
     lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
-    lateralPlan.curvatureRates = [float(x/self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
+    lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
 
     lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
     lateralPlan.solverExecutionTime = self.lat_mpc.solve_time

selfdrive/controls/lib/longcontrol.py

@@ -1,9 +1,9 @@
 from cereal import car
-from common.numpy_fast import clip, interp
-from common.realtime import DT_CTRL
-from selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
-from selfdrive.controls.lib.pid import PIDController
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone
+from openpilot.selfdrive.controls.lib.pid import PIDController
+from openpilot.selfdrive.modeld.constants import T_IDXS
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -1,19 +1,19 @@
 #!/usr/bin/env python3
 import os
+import time
 import numpy as np
 from cereal import log
-from common.realtime import sec_since_boot
-from common.numpy_fast import clip
-from system.swaglog import cloudlog
+from openpilot.common.numpy_fast import clip
+from openpilot.system.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
-from selfdrive.modeld.constants import index_function
-from selfdrive.car.interfaces import ACCEL_MIN
-from selfdrive.controls.radard import _LEAD_ACCEL_TAU
+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 third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+  from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
 
 from casadi import SX, vertcat
 
@@ -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,  5.55,  19.99, 20,   25,   40]
+    y_dist = [1.2,  1.5,   1.5,   1.7,  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,  5.55,  19.99, 20,   25,   40]
+    y_dist = [1.0,  1.35,  1.35,  1.5,  1.5,  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,  2.0,   5.55,  19.99, 20,    25,   40]
+    y_dist = [0.9,  1.0,   1.07,  1.09,  1.11,  1.11, 1.2]
   else:
     raise NotImplementedError("Dynamic Follow personality not supported")
   return np.interp(v_ego, x_vel, y_dist)
@@ -304,7 +304,7 @@ class LongitudinalMpc:
     self.x0[1] = v
     self.x0[2] = a
     if abs(v_prev - v) > 2.:  # probably only helps if v < v_prev
-      for i in range(0, N+1):
+      for i in range(N+1):
         self.solver.set(i, 'x', self.x0)
 
   @staticmethod
@@ -427,7 +427,7 @@ class LongitudinalMpc:
         self.source = 'lead1'
 
   def run(self):
-    # t0 = sec_since_boot()
+    # t0 = time.monotonic()
     # reset = 0
     for i in range(N+1):
       self.solver.set(i, 'p', self.params[i])
@@ -458,14 +458,14 @@ class LongitudinalMpc:
 
     self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
 
-    t = sec_since_boot()
+    t = time.monotonic()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
         cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
       self.reset()
       # reset = 1
-    # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(sec_since_boot() - t0):.2e} qp {self.time_qp_solution:.2e}, \
+    # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \
     # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}")
 
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,26 +1,26 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-from common.numpy_fast import clip, interp
-from common.params import Params
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.params import Params
 from cereal import log
 
 import cereal.messaging as messaging
-from common.conversions import Conversions as CV
-from common.filter_simple import FirstOrderFilter
-from common.realtime import DT_MDL
-from selfdrive.modeld.constants import T_IDXS
-from selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
-from selfdrive.controls.lib.longcontrol import LongCtrlState
-from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
-from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
-from system.swaglog import cloudlog
-from selfdrive.controls.lib.vision_turn_controller import VisionTurnController
-from selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, SpeedLimitResolver
-from selfdrive.controls.lib.turn_speed_controller import TurnSpeedController
-from selfdrive.controls.lib.accel_controller import AccelController
-from selfdrive.controls.lib.dynamic_endtoend_controller import DynamicEndtoEndController
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.modeld.constants import T_IDXS
+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.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.vision_turn_controller import VisionTurnController
+from openpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, SpeedLimitResolver
+from openpilot.selfdrive.controls.lib.turn_speed_controller import TurnSpeedController
+from openpilot.selfdrive.controls.lib.accel_controller import AccelController
+from openpilot.selfdrive.controls.lib.dynamic_endtoend_controller import DynamicEndtoEndController
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
@@ -122,7 +122,7 @@ class LongitudinalPlanner:
 
     self.param_read_counter += 1
     if self.dynamic_endtoend_controller.is_enabled():
-      self.mpc.mode = self.dynamic_endtoend_controller.get_mpc_mode(self.mpc.mode, self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'])
+      self.mpc.mode = self.dynamic_endtoend_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'])
     else:
       self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 

selfdrive/controls/lib/pid.py

@@ -1,7 +1,7 @@
 import numpy as np
 from numbers import Number
 
-from common.numpy_fast import clip, interp
+from openpilot.common.numpy_fast import clip, interp
 
 
 class PIDController():

selfdrive/controls/lib/road_edge_detector.py

@@ -1,3 +1,26 @@
+#!/usr/bin/env python3
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
 import numpy as np
 
 class RoadEdgeDetector:

selfdrive/controls/lib/speed_limit_controller.py

@@ -1,14 +1,14 @@
 import numpy as np
 import time
-from common.numpy_fast import interp
+from openpilot.common.numpy_fast import interp
 from enum import IntEnum
 from cereal import custom, car
-from common.params import Params
-from common.realtime import sec_since_boot
-from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_ACC, LIMIT_MAX_ACC, LIMIT_SPEED_OFFSET_TH, \
+from openpilot.common.params import Params
+#from openpilot.common.realtime import time.monotonic()
+from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_ACC, LIMIT_MAX_ACC, LIMIT_SPEED_OFFSET_TH, \
   LIMIT_MAX_MAP_DATA_AGE, CONTROL_N
 # from selfdrive.controls.lib.events import Events
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.selfdrive.modeld.constants import T_IDXS
 import cereal.messaging as messaging
 
 
@@ -281,7 +281,7 @@ class SpeedLimitController():
     # cause a temp inactive transition if the controller is updated before controlsd sets actual cruise
     # speed.
     if not self._op_enabled_prev and self._op_enabled:
-      self._last_op_enabled_time = sec_since_boot()
+      self._last_op_enabled_time = time.monotonic()
 
     # Update change tracking variables
     self._speed_limit_changed = self._speed_limit != self._speed_limit_prev
@@ -303,7 +303,7 @@ class SpeedLimitController():
     # Ignore if a minimum ammount of time has not passed since activation. This is to prevent temp inactivations
     # due to controlsd logic changing cruise setpoint when going active.
     if self._v_cruise_setpoint_changed and \
-       sec_since_boot() > (self._last_op_enabled_time + _TEMP_INACTIVE_GUARD_PERIOD):
+       time.monotonic() > (self._last_op_enabled_time + _TEMP_INACTIVE_GUARD_PERIOD):
       self.state = SpeedLimitControlState.tempInactive
       return
 

selfdrive/controls/lib/turn_speed_controller.py

@@ -1,11 +1,11 @@
 import numpy as np
 import time
-from common.params import Params
+from openpilot.common.params import Params
 from cereal import custom
 # from common.realtime import sec_since_boot
-from selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_SPEED, LIMIT_MAX_MAP_DATA_AGE, \
+from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_ADAPT_ACC, LIMIT_MIN_SPEED, LIMIT_MAX_MAP_DATA_AGE, \
   LIMIT_SPEED_OFFSET_TH, CONTROL_N, LIMIT_MIN_ACC, LIMIT_MAX_ACC
-from selfdrive.modeld.constants import T_IDXS
+from openpilot.selfdrive.modeld.constants import T_IDXS
 import cereal.messaging as messaging
 
 

selfdrive/controls/lib/vision_turn_controller.py

@@ -1,12 +1,12 @@
 import numpy as np
 import math
 from cereal import custom
-from common.numpy_fast import interp
+from openpilot.common.numpy_fast import interp
 # from common.params import Params
 # from common.realtime import sec_since_boot
-from common.conversions import Conversions as CV
+from openpilot.common.conversions import Conversions as CV
 # from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
 import cereal.messaging as messaging
 
 TRAJECTORY_SIZE = 33

selfdrive/controls/plannerd.py

@@ -2,12 +2,12 @@
 import os
 import numpy as np
 from cereal import car
-from common.params import Params
-from common.realtime import Priority, config_realtime_process
-from system.swaglog import cloudlog
-from selfdrive.modeld.constants import T_IDXS
-from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
-from selfdrive.controls.lib.lateral_planner import LateralPlanner
+from openpilot.common.params import Params
+from openpilot.common.realtime import Priority, config_realtime_process
+from openpilot.system.swaglog import cloudlog
+from openpilot.selfdrive.modeld.constants import T_IDXS
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
 import cereal.messaging as messaging
 
 def cumtrapz(x, t):

selfdrive/controls/radard.py

@@ -6,12 +6,12 @@ from typing import Optional, Dict, Any
 
 import capnp
 from cereal import messaging, log, car
-from common.numpy_fast import interp
-from common.params import Params
-from common.realtime import Ratekeeper, Priority, config_realtime_process, DT_MDL
-from system.swaglog import cloudlog
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper, Priority, config_realtime_process
+from openpilot.system.swaglog import cloudlog
 
-from common.kalman.simple_kalman import KF1D
+from openpilot.common.kalman.simple_kalman import KF1D
 
 
 # Default lead acceleration decay set to 50% at 1s
@@ -64,12 +64,8 @@ class Track:
     self.dRel = d_rel   # LONG_DIST
     self.yRel = y_rel   # -LAT_DIST
     self.vRel = v_rel   # REL_SPEED
-    self.measured = measured   # measured or estimate
-
-    self.update_vlead(v_lead)
-
-  def update_vlead(self, v_lead: float):
     self.vLead = v_lead
+    self.measured = measured   # measured or estimate
 
     # computed velocity and accelerations
     if self.cnt > 0:
@@ -173,8 +169,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
 def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capnp._DynamicStructReader,
              model_v_ego: float, low_speed_override: bool = True) -> Dict[str, Any]:
   # Determine leads, this is where the essential logic happens
-  lead_msg_empty = any(len(c) == 0 for c in [lead_msg.x, lead_msg.y, lead_msg.v])
-  if len(tracks) > 0 and ready and not lead_msg_empty and lead_msg.prob > .5:
+  if len(tracks) > 0 and ready and lead_msg.prob > .5:
     track = match_vision_to_track(v_ego, lead_msg, tracks)
   else:
     track = None
@@ -182,7 +177,7 @@ def get_lead(v_ego: float, ready: bool, tracks: Dict[int, Track], lead_msg: capn
   lead_dict = {'status': False}
   if track is not None:
     lead_dict = track.get_RadarState(lead_msg.prob)
-  elif track is None and ready and not lead_msg_empty and lead_msg.prob > .5:
+  elif (track is None) and ready and (lead_msg.prob > .5):
     lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
 
   if low_speed_override:
@@ -212,14 +207,14 @@ class RadarD:
 
     self.ready = False
 
-  def update(self, sm: messaging.SubMaster, radar_data: Optional[car.RadarData]):
+  def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]):
     self.current_time = 1e-9*max(sm.logMonoTime.values())
 
     radar_points = []
     radar_errors = []
-    if radar_data is not None:
-      radar_points = radar_data.points
-      radar_errors = radar_data.errors
+    if rr is not None:
+      radar_points = rr.points
+      radar_errors = rr.errors
 
     if sm.updated['carState']:
       self.v_ego = sm['carState'].vEgo
@@ -227,32 +222,26 @@ class RadarD:
     if sm.updated['modelV2']:
       self.ready = True
 
-    if radar_data is not None:
-      ar_pts = {}
-      for pt in radar_points:
-        ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
+    ar_pts = {}
+    for pt in radar_points:
+      ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured]
 
-      # *** remove missing points from meta data ***
-      for ids in list(self.tracks.keys()):
-        if ids not in ar_pts:
-          self.tracks.pop(ids, None)
+    # *** remove missing points from meta data ***
+    for ids in list(self.tracks.keys()):
+      if ids not in ar_pts:
+        self.tracks.pop(ids, None)
 
-      # *** compute the tracks ***
-      for ids in ar_pts:
-        rpt = ar_pts[ids]
+    # *** compute the tracks ***
+    for ids in ar_pts:
+      rpt = ar_pts[ids]
 
-        # align v_ego by a fixed time to align it with the radar measurement
-        v_lead = rpt[2] + self.v_ego_hist[0]
+      # align v_ego by a fixed time to align it with the radar measurement
+      v_lead = rpt[2] + self.v_ego_hist[0]
 
-        # create the track if it doesn't exist or it's a new track
-        if ids not in self.tracks:
-          self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
-        self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
-    else:
-      # *** no radar points, keep existing tracks, update v_lead
-      for track in self.tracks.values():
-        v_lead = track.vRel + self.v_ego_hist[0]
-        track.update_vlead(v_lead)
+      # create the track if it doesn't exist or it's a new track
+      if ids not in self.tracks:
+        self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
 
     # *** publish radarState ***
     self.radar_state_valid = sm.all_checks() and len(radar_errors) == 0
@@ -305,33 +294,32 @@ def radard_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messagi
   cloudlog.info("radard is importing %s", CP.carName)
   RadarInterface = importlib.import_module(f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
 
-  # setup messaging
+  # *** setup messaging
   if can_sock is None:
     can_sock = messaging.sub_sock('can')
   if sm is None:
-    sm = messaging.SubMaster(['modelV2', 'carState'], poll=["modelV2"])
+    sm = messaging.SubMaster(['modelV2', 'carState'], ignore_avg_freq=['modelV2', 'carState'])  # Can't check average frequency, since radar determines timing
   if pm is None:
     pm = messaging.PubMaster(['radarState', 'liveTracks'])
 
-  interface = RadarInterface(CP)
+  RI = RadarInterface(CP)
 
-  rk = Ratekeeper(1 / DT_MDL, print_delay_threshold=None)
-  radar = RadarD(DT_MDL, interface.delay)
+  rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
+  RD = RadarD(CP.radarTimeStep, RI.delay)
 
-  while True:
-    sm.update()
+  while 1:
+    can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
+    rr = RI.update(can_strings)
 
-    if sm.updated['modelV2']:
-      can_strings = messaging.drain_sock_raw(can_sock)
-      if len(can_strings) == 0:
-        radar_data = None
-      else:
-        radar_data = interface.update(can_strings)
+    if rr is None:
+      continue
 
-      radar.update(sm, radar_data)
-      radar.publish(pm, -rk.remaining*1000.0)
+    sm.update(0)
 
-      rk.monitor_time()
+    RD.update(sm, rr)
+    RD.publish(pm, -rk.remaining*1000.0)
+
+    rk.monitor_time()
 
 
 def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None, can_sock: messaging.SubSocket = None):