sunnypilot v0.9.5.3

2026-06-23 10:02:06 +08:00 · 2023-12-24 20:30:25 +00:00
184 changed files with 5887 additions and 9201 deletions
@@ -1,12 +0,0 @@
-# These are supported funding model platforms
-
-github: [sunnyhaibin] # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2]
-patreon: sunnyhaibin # Replace with a single Patreon username
-open_collective: # Replace with a single Open Collective username
-ko_fi: # Replace with a single Ko-fi username
-tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel
-community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry
-liberapay: # Replace with a single Liberapay username
-issuehunt: # Replace with a single IssueHunt username
-otechie: # Replace with a single Otechie username
-custom: ['https://paypal.me/sunnyhaibin0850'] # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2']
@@ -1,3 +1,43 @@
+sunnypilot - 0.9.5.3 (2023-12-24)
+========================
+* UPDATED: Dynamic Experimental Control (DEC)
+  * Synced with dragonpilot-community/dragonpilot:lp-dp-beta2 commit 578d38b
+* UPDATED: Driving Model Selector v2
+  * Driving models sort in descending order based on availability date
+  * Experimental/unmerged driving models are only available in "dev-c3" branch
+    * To select and use experimental driving models, navigate to "Software" panel, select the "dev-c3" branch, and check for update
+* UPDATED: Vision-based Turn Speed Control (V-TSC) implementation
+  * Refactored implementation thanks to pfeiferj!
+  * More accurate and consistent velocity calculation to achieve smoother longitudinal control in curves
+* NEW❗: Speed Limit Warning
+  * Display alert and/or chime to warn the driver when the cruising speed is faster than the speed limit plus the Warning Offset
+  * Customizable Warning Offset, independent of Speed Limit Control (SLC)'s Limit Offset
+* UPDATED: Speed Limit Source Policy
+  * Selectable speed limit source for Speed Limit Control and Speed Limit Warning
+  * Applicable to: Speed Limit Control, Speed Limit Warning
+* UPDATED: Speed Limit Control (SLC)
+  * Engage Mode: Removed "Warning Only" mode - this has been replaced by the new Speed Limit Warning sub-menu
+* UPDATED: OpenStreetMap (OSM) implementation
+  * Refactored implementation thanks to pfeiferj!
+    * Less resource impact
+    * Significantly smaller sizes with databases
+    * All regions are available to download
+    * Weekly map updates thanks to pfeiferj!
+    * Increased the font size of the road name
+  * C3X-specific changes
+    * Altitude (ALT.) display on Developer UI
+    * Current street name on top of driving screen when "OSM Debug UI" is enabled
+* UPDATED: Map-based Turn Speed Control (M-TSC) implementation
+  * Only available in "staging-c3" and "dev-c3" branches. If you are using "release-c3" branch, navigate to "Software" panel, select the desired target branch, and check for update
+  * Refactored implementation thanks to pfeiferj!
+  * Based on the new OpenStreetMap implementation
+  * Improved predicted curvature calculations from OpenStreetMap data
+* UI updates
+  * RE-ENABLED: Navigation: Full screen support
+    * Display the map view in full screen
+    * To switch back to driving view, tap on the border edge
+* Hyundai Bayon Non-SCC 2019 support thanks to polein78!
+
 sunnypilot - 0.9.5.2 (2023-12-07)
 ========================
 * NEW❗: MADS: Allow Navigate on openpilot in Chill Mode
@@ -1 +1 @@
-const uint8_t gitversion[8] = "5bcb1122";
+const uint8_t gitversion[8] = "00000000";
@@ -1 +1 @@
-#define COMMA_VERSION "0.9.5.2-staging"
+#define COMMA_VERSION "0.9.5.3-release"
@@ -1,35 +0,0 @@
-# OSM Installation instructions:
-# https://wiki.openstreetmap.org/wiki/Overpass_API/Installation
-
-# Install expat. All other needed libraries are already installed.
-# g++ make expat libexpat1-dev zlib1g-dev
-#sudo apt-get update
-#sudo apt-get install expat
-
-# Add required path variables to environment
-export OSM_ROOT=/data/media/0/osm
-export OSM_VERSION=0.7.57
-export GZ_FILE=osm-3s_v${OSM_VERSION}.tar.xz
-export OSM_DIR=${OSM_ROOT}/v${OSM_VERSION}
-# export DB_DIR=/data/osm/db/
-
-# Download and extract overpass library
-
-if [ ! -d ${OSM_ROOT} ]; then
-  mkdir -p ${OSM_ROOT}
-fi
-tar -vxf /data/openpilot/selfdrive/mapd/assets/${GZ_FILE} -C ${OSM_ROOT}
-
-# Configure and install overpass
-#cd $(ls | grep $SOURCE_FILE_ROOT\.[0-9]*)
-#cd osm-3s_v0.7.56
-#./configure CXXFLAGS="-O2" --prefix=$EXEC_DIR
-#make install
-
-# Remove source files after installation
-#cd ..
-if [ -d ${OSM_DIR} ]; then
-  rm -rf ${OSM_DIR}
-fi
-
-mv ${OSM_ROOT}/osm-3s_v${OSM_VERSION} ${OSM_ROOT}/v${OSM_VERSION}
@@ -1,35 +0,0 @@
-export OSM_DIR=/data/media/0/osm
-export DB_DIR=${OSM_DIR}/db
-
-export OSM_LOCATION=$(cat /data/params/d/OsmLocationUrl)
-
-export OSM_LOCATION_TEXT=$(cat /data/params/d/OsmLocationName)
-
-export XZ_MAP_FILE_NAME=${OSM_LOCATION_TEXT}.tar.xz
-export XZ_MAP_FILE=${OSM_DIR}/${XZ_MAP_FILE_NAME}
-
-# WD
-cd $OSM_DIR
-
-# Remove legacy compressed map file if existing
-rm -rf $XZ_MAP_FILE
-
-# Download map file
-wget -O ${XZ_MAP_FILE_NAME} ${OSM_LOCATION}
-
-if [[ "$?" != 0 ]]; then
-  echo "Error downloading map file"
-else
-  echo "Successfully downloaded map file"
-  # Remove current db dir if existing
-  rm -rf $DB_DIR
-  if [ -d ${OSM_DIR}/${OSM_LOCATION_TEXT} ]; then
-    rm -rf ${OSM_DIR}/${OSM_LOCATION_TEXT}
-  fi
-  # Decompressing
-  tar -vxf ${XZ_MAP_FILE_NAME}
-  mv ${OSM_LOCATION_TEXT} db
-
-  # Remove compressed map files after expanding
-  rm -rf $XZ_MAP_FILE
-fi
@@ -84,12 +84,7 @@ function launch {

  # start manager
  cd selfdrive/manager
-  if [ ! -f "/data/params/d/OsmLocal" ]; then
-    ./custom_dep.py && ./build.py && ./manager.py
-  else
-    ./custom_dep.py && ./build.py && ./local_osm_install.py && ./manager.py
-  fi
-
+  ./build.py && ./mapd_installer.py && ./manager.py
  # if broken, keep on screen error
  while true; do sleep 1; done
 }
@@ -207,6 +207,7 @@ def crash_log(candidate):
  no_internet = 0
  while True:
    if is_connected_to_internet():
+      sentry.get_init()
      sentry.capture_warning("fingerprinted %s" % candidate)
      break
    else:
@@ -220,6 +221,7 @@ def crash_log2(fingerprints, fw):
  no_internet = 0
  while True:
    if is_connected_to_internet():
+      sentry.get_init()
      sentry.capture_warning("car doesn't match any fingerprints: %s" % fingerprints)
      sentry.capture_warning("car doesn't match any fw: %s" % fw)
      break
@@ -5,7 +5,8 @@ from openpilot.common.params import Params, put_bool_nonblocking
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits
 from openpilot.selfdrive.car.chrysler import chryslercan
-from openpilot.selfdrive.car.chrysler.values import RAM_CARS, RAM_DT, CarControllerParams, ChryslerFlags
+from openpilot.selfdrive.car.chrysler.values import RAM_CARS, RAM_DT, CarControllerParams, ChryslerFlags, ChryslerFlagsSP
+from openpilot.selfdrive.car.interfaces import FORWARD_GEARS
 from openpilot.selfdrive.controls.lib.drive_helpers import FCA_V_CRUISE_MIN

 BUTTONS_STATES = ["accelCruise", "decelCruise", "cancel", "resumeCruise"]
@@ -93,7 +94,7 @@ class CarController:
    lkas_active = CC.latActive and CS.madsEnabled

    if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS:
-      can_sends.append(chryslercan.create_lkas_heartbit(self.packer, CS.madsEnabled, CS.lkas_heartbit))
+      can_sends.append(chryslercan.create_lkas_heartbit(self.packer, CS.lkas_disabled, CS.lkas_heartbit))

    ram_cars = self.CP.carFingerprint in RAM_CARS

@@ -151,6 +152,8 @@ class CarController:
          lkas_control_bit = True
        if (self.CP.minEnableSpeed >= 14.5) and (CS.out.gearShifter != 2):
          lkas_control_bit = False
+      elif self.CP.spFlags & ChryslerFlagsSP.SP_WP_S20:
+        lkas_control_bit = CC.latActive and CS.out.gearShifter in FORWARD_GEARS
      elif CS.out.vEgo > self.CP.minSteerSpeed:
        lkas_control_bit = True
      elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
@@ -24,6 +24,7 @@ class CarState(CarStateBase):
    self.lkas_enabled = False
    self.prev_lkas_enabled = False
    self.lkas_heartbit = None
+    self.lkas_disabled = False

    self.buttonStates = BUTTON_STATES.copy()
    self.buttonStatesPrev = BUTTON_STATES.copy()
@@ -99,6 +100,8 @@ class CarState(CarStateBase):
    else:
      self.lkas_enabled = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1
      self.lkas_heartbit = cp_cam.vl["LKAS_HEARTBIT"]
+      if not self.control_initialized:
+        self.lkas_disabled = bool(cp_cam.vl["LKAS_HEARTBIT"]["LKAS_DISABLED"])
      ret.steerFaultTemporary = cp.vl["EPS_2"]["LKAS_TEMPORARY_FAULT"] == 1
      ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4

@@ -81,8 +81,8 @@ def create_cruise_buttons(packer, frame, bus, CP, cruise_buttons_msg=None, butto
  return packer.make_can_msg("CRUISE_BUTTONS", bus, values)


-def create_lkas_heartbit(packer, mads_enabled, lkas_heartbit):
+def create_lkas_heartbit(packer, lkas_disabled, lkas_heartbit):
  # LKAS_HEARTBIT (697) LKAS heartbeat
  values = lkas_heartbit.copy()  # forward what we parsed
-  values["LKAS_DISABLED"] = 0 if mads_enabled else 1
+  values["LKAS_DISABLED"] = 1 if lkas_disabled else 0
  return packer.make_can_msg("LKAS_HEARTBIT", 0, values)
@@ -94,6 +94,10 @@ class CarInterface(CarInterfaceBase):
    ret.centerToFront = ret.wheelbase * 0.44
    ret.enableBsm = 720 in fingerprint[0]

+    if 0x4FF in fingerprint[0]:
+      ret.spFlags |= ChryslerFlagsSP.SP_WP_S20.value
+      ret.minSteerSpeed = 0.0
+
    return ret

  def _update(self, c):
@@ -124,9 +128,11 @@ class CarInterface(CarInterfaceBase):
          self.CS.madsEnabled = True
        if self.CS.prev_lkas_enabled != 1 and self.CS.lkas_enabled == 1:
          self.CS.madsEnabled = not self.CS.madsEnabled
+          self.CS.lkas_disabled = not self.CS.lkas_disabled
        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
    else:
      self.CS.madsEnabled = False
+    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)

    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
      if any(b.type == ButtonType.cancel for b in buttonEvents):
@@ -18,6 +18,7 @@ class ChryslerFlags(IntFlag):

 class ChryslerFlagsSP(IntFlag):
  SP_RAM_HD_PARAMSD_IGNORE = 1
+  SP_WP_S20 = 2


 class CAR(StrEnum):
@@ -238,7 +238,7 @@ class CarState(CarStateBase):
      ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active

    ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
-    ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
+    ret.cruiseState.enabled = self.pcm_cruise_enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
    ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"])

    # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
@@ -113,8 +113,12 @@ class CarInterface(CarInterfaceBase):
      ret.wheelbase = 2.70
      ret.centerToFront = ret.wheelbase * 0.4
      ret.steerRatio = 15.38  # 10.93 is end-to-end spec
-      ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
+      if eps_modified:
+        ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]]
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]  # 2.5x Modded EPS
+      else:
+        ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]

    elif candidate in (CAR.ACCORD, CAR.ACCORDH):
      ret.mass = 3279. * CV.LB_TO_KG
@@ -376,7 +380,7 @@ class CarInterface(CarInterfaceBase):
    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
      if ret.gasPressed and not ret.cruiseState.enabled:
        self.CS.accEnabled = False
-      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
+      self.CS.accEnabled = self.CS.pcm_cruise_enabled

    ret, self.CS = self.get_sp_common_state(ret, self.CS,
                                            min_enable_speed_pcm=(self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed),
@@ -638,6 +638,16 @@ FW_VERSIONS = {
      b'39990-TGG-J510\x00\x00',
      b'39990-TGL-E130\x00\x00',
      b'39990-TGN-E120\x00\x00',
+      # modded EPS bosch civic
+      b'39990-TBA,C020\x00\x00',
+      b'39990-TBA,C120\x00\x00',
+      b'39990-TEA,T820\x00\x00',
+      b'39990-TEZ,T020\x00\x00',
+      b'39990-TGG,A020\x00\x00',
+      b'39990-TGG,A120\x00\x00',
+      b'39990-TGG,J510\x00\x00',
+      b'39990-TGL,E130\x00\x00',
+      b'39990-TGN,E120\x00\x00',
    ],
    (Ecu.srs, 0x18da53f1, None): [
      b'77959-TBA-A060\x00\x00',
@@ -8,7 +8,7 @@ from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
 from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
 from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
-from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR
+from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR, LEGACY_SAFETY_MODE_CAR
 from openpilot.selfdrive.controls.lib.drive_helpers import HYUNDAI_V_CRUISE_MIN

 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -261,7 +261,17 @@ class CarController:
        if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
          self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
          if self.cruise_button is not None:
-            if self.frame % 2 == 0:
+            if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
+              send_freq = 1
+              if not (self.v_tsc_state != 0 or self.m_tsc_state > 1) and abs(self.target_speed - self.v_set_dis) <= 2:
+                send_freq = 5
+              # send resume at a max freq of 10Hz
+              if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
+                # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
+                can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
+                if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
+                  self.last_button_frame = self.frame
+            elif self.frame % 2 == 0:
              can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)

      # Parse lead distance from radarState and display the corresponding distance in the car's cluster
@@ -40,7 +40,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
                         CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022,
                         CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED,
                         CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN, CAR.ELANTRA_2022_NON_SCC,
-                         CAR.GENESIS_G70_2021_NON_SCC, CAR.KIA_SELTOS_2023_NON_SCC):
+                         CAR.GENESIS_G70_2021_NON_SCC, CAR.KIA_SELTOS_2023_NON_SCC, CAR.BAYON_1ST_GEN_NON_SCC):
    values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
    values["CF_Lkas_LdwsOpt_USM"] = 2

@@ -47,6 +47,7 @@ class CarInterface(CarInterfaceBase):
        # non-HDA2
        if 0x1cf not in fingerprint[CAN.ECAN]:
          ret.flags |= HyundaiFlags.CANFD_ALT_BUTTONS.value
+          ret.customStockLongAvailable = False
        # ICE cars do not have 0x130; GEARS message on 0x40 or 0x70 instead
        if 0x130 not in fingerprint[CAN.ECAN]:
          if 0x40 not in fingerprint[CAN.ECAN]:
@@ -103,6 +104,10 @@ class CarInterface(CarInterfaceBase):
      ret.wheelbase = 2.90
      ret.steerRatio = 15.6 * 1.15
      ret.tireStiffnessFactor = 0.63
+    elif candidate == CAR.BAYON_1ST_GEN_NON_SCC:
+      ret.mass = 1150.
+      ret.wheelbase = 2.58
+      ret.steerRatio = 13.27 * 1.15   # Variable steering ratio? https://www.hyundai.news/newsroom/dam/eu/uk/20210302_bayon_technical_data/hyundai-reveals-all-new_bayon-technical-data.pdf
    elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30):
      ret.mass = 1275.
      ret.wheelbase = 2.7
@@ -379,7 +384,7 @@ class CarInterface(CarInterfaceBase):
    # for enabling radar tracks on startup
    # some CAN platforms are able to enable radar tracks config at the radar ECU,
    # but the config is reset after ignition cycle
-    if CP.openpilotLongitudinalControl and (CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS):
+    if CP.spFlags & HyundaiFlagsSP.SP_RADAR_TRACKS:
      enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, config_data_id=b'\x01\x42')

  def _update(self, c):
@@ -80,6 +80,7 @@ class CAR(StrEnum):
  # Hyundai
  AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN"
  AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN"
+  BAYON_1ST_GEN_NON_SCC = "HYUNDAI BAYON 1ST GEN NON-SCC"
  ELANTRA = "HYUNDAI ELANTRA 2017"
  ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
  ELANTRA_2021 = "HYUNDAI ELANTRA 2021"
@@ -310,6 +311,7 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = {
  CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m])),

  # Non-SCC Cars
+  CAR.BAYON_1ST_GEN_NON_SCC: HyundaiCarInfo("Hyundai Bayon Non-SCC 2021", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_n])),
  CAR.ELANTRA_2022_NON_SCC: HyundaiCarInfo("Hyundai Elantra Non-SCC 2022", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_k])),
  CAR.KONA_NON_SCC: HyundaiCarInfo("Hyundai Kona Non-SCC 2019", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_b])),
  CAR.KIA_FORTE_2019_NON_SCC: HyundaiCarInfo("Kia Forte Non-SCC 2019", "No Smart Cruise Control (SCC)", car_parts=CarParts.common([CarHarness.hyundai_g])),
@@ -1813,6 +1815,12 @@ FW_VERSIONS = {
      b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00',
    ],
  },
+  CAR.BAYON_1ST_GEN_NON_SCC: {
+    # TODO: Check working route for more FW
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00BC3 LKA  AT EUR LHD 1.00 1.01 99211-Q0100 261'
+    ],
+  },
  CAR.ELANTRA: {
    (Ecu.fwdCamera, 0x7c4, None): [
      b'\xf1\x00ADP LKAS AT USA LHD 1.00 1.03 99211-F2000 X31',
@@ -2216,7 +2224,8 @@ FW_VERSIONS = {
 CHECKSUM = {
  "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021,
           CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022,
-           CAR.KIA_K5_HEV_2020, CAR.CUSTIN_1ST_GEN, CAR.ELANTRA_2022_NON_SCC, CAR.GENESIS_G70_2021_NON_SCC, CAR.KIA_SELTOS_2023_NON_SCC],
+           CAR.KIA_K5_HEV_2020, CAR.CUSTIN_1ST_GEN, CAR.ELANTRA_2022_NON_SCC, CAR.GENESIS_G70_2021_NON_SCC,
+           CAR.KIA_SELTOS_2023_NON_SCC, CAR.BAYON_1ST_GEN_NON_SCC],
  "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
 }

@@ -2262,7 +2271,7 @@ UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.
                                                         CAR.KIA_OPTIMA_H_G4_FL}

 NON_SCC_CAR = {CAR.KIA_FORTE_2021_NON_SCC, CAR.ELANTRA_2022_NON_SCC, CAR.KIA_FORTE_2019_NON_SCC, CAR.GENESIS_G70_2021_NON_SCC,
-               CAR.KIA_SELTOS_2023_NON_SCC, CAR.KONA_NON_SCC}
+               CAR.KIA_SELTOS_2023_NON_SCC, CAR.KONA_NON_SCC, CAR.BAYON_1ST_GEN_NON_SCC}
 NON_SCC_NO_FCA_CAR = {CAR.KIA_FORTE_2019_NON_SCC, }
 NON_SCC_RADAR_FCA_CAR = {CAR.GENESIS_G70_2021_NON_SCC, }

@@ -2271,6 +2280,7 @@ NON_SCC_RADAR_FCA_CAR = {CAR.GENESIS_G70_2021_NON_SCC, }
 DBC = {
  CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
  CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None),
+  CAR.BAYON_1ST_GEN_NON_SCC: dbc_dict('hyundai_kia_generic', None),
  CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
  CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
  CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
@@ -40,6 +40,8 @@ TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/lat_mode
 ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'}

 GAC_DICT = {1: 1, 2: 2, 3: 3}
+FORWARD_GEARS = [GearShifter.drive, GearShifter.low, GearShifter.eco,
+                 GearShifter.sport, GearShifter.manumatic, GearShifter.brake]


 def similarity(s1: str, s2: str) -> float:
@@ -160,10 +162,10 @@ def get_nn_model_path(_car, eps_firmware) -> Tuple[Optional[str], float]:
  else:
    check_model = _car
  model_path, max_similarity = check_nn_path(check_model)
-  if 0.0 <= max_similarity < 0.9:
+  if _car not in model_path or 0.0 <= max_similarity < 0.9:
    check_model = _car
    model_path, max_similarity = check_nn_path(check_model)
-    if 0.0 <= max_similarity < 0.9:
+    if _car not in model_path or 0.0 <= max_similarity < 0.9:
      model_path = None
  return model_path, max_similarity

@@ -268,8 +270,15 @@ class CarInterfaceBase(ABC):
        eps_firmware = str(next((fw.fwVersion for fw in car_fw if fw.ecu == "eps"), ""))
        model, similarity_score = get_nn_model_path(candidate, eps_firmware)
        if model is not None:
-          ret.lateralTuning.torque.nnModelName = os.path.splitext(os.path.basename(model))[0]
+          ret.lateralTuning.torque.nnModelName = nn_model_name = os.path.splitext(os.path.basename(model))[0]
          ret.lateralTuning.torque.nnModelFuzzyMatch = (similarity_score < 0.99)
+          if 'b\'' in nn_model_name:
+            nn_model, _ = nn_model_name.split('b\'')
+          else:
+            nn_model = nn_model_name
+          params.put("NNFFCarModel", nn_model)
+        else:
+          ret.lateralTuning.torque.nnModelName = "mock"

    # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
    if not ret.notCar:
@@ -552,19 +561,25 @@ class CarInterfaceBase(ABC):

    return mads_enabled

-  def get_sp_started_mads(self, mads_enabled, cruiseState_available):
+  def get_sp_started_mads(self, cs_out, CS):
+    if not cs_out.cruiseState.available and CS.out.cruiseState.available:
+      self.madsEnabledInit = False
+      self.madsEnabledInitPrev = False
+      return False
    if not self.mads_main_toggle or self.prev_acc_mads_combo:
-      return mads_enabled
-    if not self.madsEnabledInit and mads_enabled:
+      return CS.mads_enabled
+    if not self.madsEnabledInit and CS.mads_enabled:
      self.madsEnabledInit = True
      self.last_mads_init = time.monotonic()
-    if self.madsEnabledInit and not self.madsEnabledInitPrev:
+    if cs_out.gearShifter not in FORWARD_GEARS:
+      self.last_mads_init = time.monotonic()
+    if self.madsEnabledInit and (not self.madsEnabledInitPrev or cs_out.gearShifter not in FORWARD_GEARS):
      if time.monotonic() < self.last_mads_init + 1.:
        return False
      self.madsEnabledInitPrev = True
-      return cruiseState_available
+      return cs_out.cruiseState.available
    else:
-      return mads_enabled
+      return CS.mads_enabled

  def get_sp_common_state(self, cs_out, CS, min_enable_speed_pcm=False, gear_allowed=True, gap_button=False):
    cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed or min_enable_speed_pcm else \
@@ -757,6 +772,7 @@ class CarStateBase(ABC):
    self.mads_enabled = False
    self.prev_mads_enabled = False
    self.control_initialized = False
+    self.pcm_cruise_enabled = False
    self.gap_dist_button = 0
    self.gac_tr = int(self.param_s.get("LongitudinalPersonality"))
    self.gac_tr_cluster = clip(int(self.param_s.get("LongitudinalPersonality")), 1, 3)
@@ -68,6 +68,7 @@
  "Honda Ridgeline 2017-23": "HONDA RIDGELINE 2017",
  "Hyundai Azera 2022": "HYUNDAI AZERA 6TH GEN",
  "Hyundai Azera Hybrid 2020": "HYUNDAI AZERA HYBRID 6TH GEN",
+  "Hyundai Bayon Non-SCC 2021": "HYUNDAI BAYON 1ST GEN NON-SCC",
  "Hyundai Custin 2023": "HYUNDAI CUSTIN 1ST GEN",
  "Hyundai Elantra 2017-18": "HYUNDAI ELANTRA 2017",
  "Hyundai Elantra 2019": "HYUNDAI ELANTRA 2017",
@@ -202,7 +203,6 @@
  "Škoda Octavia RS 2016": "SKODA OCTAVIA 3RD GEN",
  "Škoda Scala 2020-23": "SKODA SCALA 1ST GEN",
  "Škoda Superb 2015-22": "SKODA SUPERB 3RD GEN",
-  "Škoda Superb 2015-22 CC Only": "SKODA SUPERB 3RD GEN CC ONLY",
  "Toyota Alphard 2019-20": "TOYOTA ALPHARD 2020",
  "Toyota Alphard Hybrid 2021": "TOYOTA ALPHARD 2020",
  "Toyota Avalon 2016": "TOYOTA AVALON 2016",
@@ -71,3 +71,6 @@ mock: [10.0, 10, 0.0]
 # Manually checked
 HONDA CIVIC 2022: [2.5, 1.2, 0.15]
 HONDA HR-V 2023: [2.5, 1.2, 0.2]
+
+# HKG Non-SCC
+HYUNDAI BAYON 1ST GEN NON-SCC: [2.5, 2.5, 0.1]
@@ -50,7 +50,7 @@ class CarController:
    actuators = CC.actuators
    hud_control = CC.hudControl
    pcm_cancel_cmd = CC.cruiseControl.cancel
-    lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
+    lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE and abs(CS.out.steeringAngleDeg) < MAX_USER_TORQUE

    # *** control msgs ***
    can_sends = []
@@ -88,7 +88,7 @@ class CarController:
    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
    # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
    # on consecutive messages
-    can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req))
+    can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req and lat_active))
    if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
      lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle
      full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and
@@ -173,7 +173,7 @@ class CarController:
      if self.frame % 20 == 0 or send_ui:
        can_sends.append(toyotacan.create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
                                                     hud_control.rightLaneVisible, hud_control.leftLaneDepart,
-                                                     hud_control.rightLaneDepart, CC.latActive, CS.lkas_hud, CS.madsEnabled))
+                                                     hud_control.rightLaneDepart, CC.latActive and lat_active, CS.lkas_hud, CS.madsEnabled))

      if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
        can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert))
@@ -65,6 +65,7 @@ class CarState(CarStateBase):
      self.zss_compute = False
      self.zss_cruise_active_last = False
      self.zss_angle_offset = 0.
+      self.zss_threshold_count = 0

  def update(self, cp, cp_cam):
    ret = car.CarState.new_message()
@@ -134,18 +135,27 @@ class CarState(CarStateBase):

    # ZSS support thanks to dragonpilot and ErichMoraga
    if self.CP.spFlags & ToyotaFlagsSP.SP_ZSS:
-      zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
-      # Only compute ZSS offset when acc is active
-      if bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) and not self.zss_cruise_active_last:
-        self.zss_compute = True # Cruise was just activated, so allow offset to be recomputed
-      self.zss_cruise_active_last = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
+      if self.zss_threshold_count <= 10:
+        zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
+        # Only compute ZSS offset when control is active
+        control_active = self.madsEnabled or bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
+        if control_active and not self.zss_cruise_active_last:
+          self.zss_threshold_count = 0
+          self.zss_compute = True  # Control was just activated, so allow offset to be recomputed
+        self.zss_cruise_active_last = control_active

-      # Compute ZSS offset
-      if self.zss_compute:
-        if abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3:
-          self.zss_angle_offset = zorro_steer - ret.steeringAngleDeg
-      # Apply offset
-      ret.steeringAngleDeg = zorro_steer - self.zss_angle_offset
+        # Compute ZSS offset
+        if self.zss_compute:
+          if abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3:
+            self.zss_compute = False
+            self.zss_angle_offset = zorro_steer - ret.steeringAngleDeg
+
+        # Safety checks
+        steering_angle_deg = zorro_steer - self.zss_angle_offset
+        if abs(ret.steeringAngleDeg - steering_angle_deg) > 4:
+          self.zss_threshold_count += 1
+        else:
+          ret.steeringAngleDeg = steering_angle_deg

    can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
    ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
@@ -263,9 +263,9 @@ class CarInterface(CarInterfaceBase):
          self.CS.madsEnabled = True
        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
      ret, self.CS = self.toggle_gac(ret, self.CS, bool(self.CS.gap_dist_button), 1, 3, 3, "-")
-      self.CS.madsEnabled = self.get_sp_started_mads(self.CS.madsEnabled, ret.cruiseState.available)
    else:
      self.CS.madsEnabled = False
+    self.CS.madsEnabled = self.get_sp_started_mads(ret, self.CS)

    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
      if any(b.type == ButtonType.cancel for b in buttonEvents):
@@ -52,5 +52,9 @@
  "Offroad_Recalibration": {
    "text": "openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
    "severity": 0
+  },
+  "Offroad_OSMUpdateRequired": {
+    "text": "OpenStreetMap database is out of date. New maps must be downloaded if you wish to continue using OpenStreetMap data for Enhanced Speed Control and road name display.\n\n%1",
+    "severity": 0
  }
 }
@@ -1,123 +1,274 @@
-from openpilot.common.numpy_fast import interp
-from openpilot.common.params import Params
+#!/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 = 2023-12-13
+from common.numpy_fast import interp
 from openpilot.selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE

-_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 = 2.0

-_DP_E2E_BLINKER_COUNT = 5
+HIGHWAY_CRUISE_KPH = 75
+
+STOP_AND_GO_FRAME = 60
+
+SET_MODE_TIMEOUT = 10
+
+MPC_FCW_WINDOW_SIZE = 5
+MPC_FCW_PROB = 0.6
+
+
+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


 class DynamicExperimentalController:
  def __init__(self):
-    self._params = Params()
    self._is_enabled = False
    self._mode = 'acc'
+    self._mode_prev = 'acc'
+    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_blinkers = False
+
+    self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._has_slowness = False
+
+    self._has_nav_enabled = False
+
+    self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._has_dangerous_ttc = False
+
+    self._v_ego_kph = 0.
+    self._v_cruise_kph = 0.
+
+    self._has_lead = False
+
+    self._has_standstill = False
+    self._has_standstill_prev = False
+
+    self._sng_transit_frame = 0
+    self._sng_state = SNG_State.off
+
+    self._mpc_fcw_gmac = GenericMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE)
+    self._has_mpc_fcw = False
+    self._mpc_fcw_crash_cnt = 0
+
+    self._set_mode_timeout = 0
    pass

-  def _set_dp_e2e_mode(self, mode, force=False):
-    if force:
-      self.dp_e2e_swap_count = 0
-      self._mode = mode
-      return
+  def _update(self, car_state, lead_one, md, controls_state):
+    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

-    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
+    # fcw detection
+    self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
+    self._has_mpc_fcw = self._mpc_fcw_gmac.get_moving_average() >= MPC_FCW_PROB

-      if self.dp_e2e_swap_count >= _DP_E2E_SWAP_COUNT:
-        self._mode = mode
+    # nav enable detection
+    self._has_nav_enabled = md.navEnabled

-  def _process_conditional_e2e(self, radar_unavailable, car_state, lead_one, md):
-    v_ego_kph = car_state.vEgo * 3.6
-
-    # 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
-
-    if self.dp_e2e_blinker_count > _DP_E2E_BLINKER_COUNT:
-      return self._set_dp_e2e_mode('blended', True)
-
-    # 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
-
-    # 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')
-
-    if self.dp_e2e_standstill_last and not car_state.standstill:
-      self.dp_e2e_sng = True
-
-    # 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 we see a lead
-    # voacc cars only
-    if radar_unavailable and self.dp_e2e_has_lead:
-      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)
+    # lead detection
+    self._lead_gmac.add_data(lead_one.status)
+    self._has_lead_filtered = self._lead_gmac.get_moving_average() >= LEAD_PROB

    # 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
+    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:
-      self.dp_e2e_stop_count = 0
+      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

-    if self.dp_e2e_stop_count >= _DP_E2E_STOP_COUNT:
-      return self._set_dp_e2e_mode('blended', True)
+    # slowness detection
+    if not self._has_standstill:
+      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

-    return self._set_dp_e2e_mode('acc')
+    # 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

-  def get_mpc_mode(self, mode, radar_unavailable, car_state, lead_one, md):
-    self._mode = mode
+    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_lead_filtered_prev = self._has_lead_filtered
+    self._frame += 1
+
+  def _blended_priority_mode(self):
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
+    if self._has_mpc_fcw:
+      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 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._set_mode('blended')
+      return
+
+    # 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._set_mode('blended')
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._set_mode('blended')
+      return
+
+    # when detecting lead slow down: blended
+    # use blended for higher braking capability
+    if self._has_dangerous_ttc:
+      self._set_mode('blended')
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
+      return
+
+    self._set_mode('blended')
+
+  def _acc_priority_mode(self):
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
+    if self._has_mpc_fcw:
+      self._set_mode('blended')
+      return
+
+    # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
+    #if self._has_lead_filtered and not self._has_standstill:
+    #  self._set_mode('acc')
+    #  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 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._set_mode('blended')
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._set_mode('blended')
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
+      return
+
+    self._set_mode('acc')
+
+  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)
+      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):
@@ -125,3 +276,15 @@ class DynamicExperimentalController:

  def is_enabled(self):
    return self._is_enabled
+
+  def set_mpc_fcw_crash_cnt(self, crash_cnt):
+    self._mpc_fcw_crash_cnt = crash_cnt
+
+  def _set_mode(self, mode):
+    if self._set_mode_timeout == 0:
+      self._mode = mode
+      if mode == "blended":
+        self._set_mode_timeout = SET_MODE_TIMEOUT
+
+    if self._set_mode_timeout > 0:
+      self._set_mode_timeout -= 1
@@ -261,7 +261,7 @@ def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, m

 def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
  model_name = CP.lateralTuning.torque.nnModelName
-  if model_name == "":
+  if model_name in ("", "mock"):
    return Alert(
      "NN Lateral Controller Not Loaded",
      '⚙️ -> "sunnypilot" for more details',
@@ -1,7 +1,7 @@
 {
-    "acados_include_path": "/data/openpilot-special/third_party/acados/include",
-    "acados_lib_path": "/data/openpilot-special/third_party/acados/lib",
-    "code_export_directory": "/data/openpilot-special/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
+    "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",
    "constraints": {
        "C": [],
        "C_e": [],
@@ -261,7 +261,7 @@
        "ny_e": 3,
        "nz": 0
    },
-    "json_file": "/data/openpilot-special/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
+    "json_file": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
    "model": {
        "con_h_expr": null,
        "con_h_expr_e": null,
@@ -1,264 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-#define S_FUNCTION_NAME acados_solver_sfunction_lat
-#define S_FUNCTION_LEVEL 2
-
-#define MDL_START
-
-// acados
-// #include "acados/utils/print.h"
-#include "acados_c/sim_interface.h"
-#include "acados_c/external_function_interface.h"
-
-// example specific
-#include "lat_model/lat_model.h"
-#include "acados_solver_lat.h"
-
-#include "simstruc.h"
-
-#define SAMPLINGTIME 0.009765625
-
-static void mdlInitializeSizes (SimStruct *S)
-{
-    // specify the number of continuous and discrete states
-    ssSetNumContStates(S, 0);
-    ssSetNumDiscStates(S, 0);// specify the number of input ports
-    if ( !ssSetNumInputPorts(S, 8) )
-        return;
-
-    // specify the number of output ports
-    if ( !ssSetNumOutputPorts(S, 6) )
-        return;
-
-    // specify dimension information for the input ports
-    // lbx_0
-    ssSetInputPortVectorDimension(S, 0, 4);
-    // ubx_0
-    ssSetInputPortVectorDimension(S, 1, 4);
-    // parameters
-    ssSetInputPortVectorDimension(S, 2, (32+1) * 2);
-    // y_ref_0
-    ssSetInputPortVectorDimension(S, 3, 5);
-    // y_ref
-    ssSetInputPortVectorDimension(S, 4, 155);
-    // y_ref_e
-    ssSetInputPortVectorDimension(S, 5, 3);
-    // lbx
-    ssSetInputPortVectorDimension(S, 6, 62);
-    // ubx
-    ssSetInputPortVectorDimension(S, 7, 62);/* specify dimension information for the OUTPUT ports */
-    ssSetOutputPortVectorDimension(S, 0, 1 );
-    ssSetOutputPortVectorDimension(S, 1, 1 );
-    ssSetOutputPortVectorDimension(S, 2, 1 );
-    ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
-    ssSetOutputPortVectorDimension(S, 4, 1);
-    ssSetOutputPortVectorDimension(S, 5, 1 );
-
-    // specify the direct feedthrough status
-    // should be set to 1 for all inputs used in mdlOutputs
-    ssSetInputPortDirectFeedThrough(S, 0, 1);
-    ssSetInputPortDirectFeedThrough(S, 1, 1);
-    ssSetInputPortDirectFeedThrough(S, 2, 1);
-    ssSetInputPortDirectFeedThrough(S, 3, 1);
-    ssSetInputPortDirectFeedThrough(S, 4, 1);
-    ssSetInputPortDirectFeedThrough(S, 5, 1);
-    ssSetInputPortDirectFeedThrough(S, 6, 1);
-    ssSetInputPortDirectFeedThrough(S, 7, 1);
-
-    // one sample time
-    ssSetNumSampleTimes(S, 1);
-}
-
-
-#if defined(MATLAB_MEX_FILE)
-
-#define MDL_SET_INPUT_PORT_DIMENSION_INFO
-#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
-
-static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-    if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
-         return;
-}
-
-static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-    if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
-         return;
-}
-
-#endif /* MATLAB_MEX_FILE */
-
-
-static void mdlInitializeSampleTimes(SimStruct *S)
-{
-    ssSetSampleTime(S, 0, SAMPLINGTIME);
-    ssSetOffsetTime(S, 0, 0.0);
-}
-
-
-static void mdlStart(SimStruct *S)
-{
-    lat_solver_capsule *capsule = lat_acados_create_capsule();
-    lat_acados_create(capsule);
-
-    ssSetUserData(S, (void*)capsule);
-}
-
-
-static void mdlOutputs(SimStruct *S, int_T tid)
-{
-    lat_solver_capsule *capsule = ssGetUserData(S);
-    ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
-    ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
-    ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
-    ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
-
-    InputRealPtrsType in_sign;      
-
-    // local buffer
-    real_t buffer[5];
-
-    /* go through inputs */
-    // lbx_0
-    in_sign = ssGetInputPortRealSignalPtrs(S, 0);
-    for (int i = 0; i < 4; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
-    // ubx_0
-    in_sign = ssGetInputPortRealSignalPtrs(S, 1);
-    for (int i = 0; i < 4; i++)
-        buffer[i] = (double)(*in_sign[i]);
-    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
-    // parameters - stage-variant !!!
-    in_sign = ssGetInputPortRealSignalPtrs(S, 2);
-
-    // update value of parameters
-    for (int ii = 0; ii <= 32; ii++)
-    {
-        for (int jj = 0; jj < 2; jj++)
-            buffer[jj] = (double)(*in_sign[ii*2+jj]);
-        lat_acados_update_params(capsule, ii, buffer, 2);
-    }
-
-  
-    // y_ref_0
-    in_sign = ssGetInputPortRealSignalPtrs(S, 3);
-
-    for (int i = 0; i < 5; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
-
-  
-    // y_ref - for stages 1 to N-1
-    in_sign = ssGetInputPortRealSignalPtrs(S, 4);
-
-    for (int ii = 1; ii < 32; ii++)
-    {
-        for (int jj = 0; jj < 5; jj++)
-            buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]);
-        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
-    }
-
-  
-    // y_ref_e
-    in_sign = ssGetInputPortRealSignalPtrs(S, 5);
-
-    for (int i = 0; i < 3; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 32, "yref", (void *) buffer);
-    // lbx
-    in_sign = ssGetInputPortRealSignalPtrs(S, 6);
-    for (int ii = 1; ii < 32; ii++)
-    {
-        for (int jj = 0; jj < 2; jj++)
-            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
-    }
-    // ubx
-    in_sign = ssGetInputPortRealSignalPtrs(S, 7);
-    for (int ii = 1; ii < 32; ii++)
-    {
-        for (int jj = 0; jj < 2; jj++)
-            buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
-    }
-
-    /* call solver */
-    int rti_phase = 0;
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
-    int acados_status = lat_acados_solve(capsule);
-
-
-    /* set outputs */
-    // assign pointers to output signals
-    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
-    int tmp_int;
-    out_u0 = ssGetOutputPortRealSignal(S, 0);
-    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
-
-  
-    out_status = ssGetOutputPortRealSignal(S, 1);
-    *out_status = (real_t) acados_status;
-    out_KKT_res = ssGetOutputPortRealSignal(S, 2);
-    *out_KKT_res = (real_t) nlp_out->inf_norm_res;
-    out_x1 = ssGetOutputPortRealSignal(S, 3);
-    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
-    out_cpu_time = ssGetOutputPortRealSignal(S, 4);
-    // get solution time
-    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
-    out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
-    // get sqp iter
-    ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
-    *out_sqp_iter = (real_t) tmp_int;
-
-}
-
-static void mdlTerminate(SimStruct *S)
-{
-    lat_solver_capsule *capsule = ssGetUserData(S);
-
-    lat_acados_free(capsule);
-    lat_acados_free_capsule(capsule);
-}
-
-
-#ifdef  MATLAB_MEX_FILE
-#include "simulink.c"
-#else
-#include "cg_sfun.h"
-#endif
@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-
-#ifndef lat_Y_0_COST
-#define lat_Y_0_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-int lat_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_0_fun_work(int *, int *, int *, int *);
-const int *lat_cost_y_0_fun_sparsity_in(int);
-const int *lat_cost_y_0_fun_sparsity_out(int);
-int lat_cost_y_0_fun_n_in(void);
-int lat_cost_y_0_fun_n_out(void);
-
-int lat_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
-const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
-int lat_cost_y_0_fun_jac_ut_xt_n_in(void);
-int lat_cost_y_0_fun_jac_ut_xt_n_out(void);
-
-int lat_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_0_hess_work(int *, int *, int *, int *);
-const int *lat_cost_y_0_hess_sparsity_in(int);
-const int *lat_cost_y_0_hess_sparsity_out(int);
-int lat_cost_y_0_hess_n_in(void);
-int lat_cost_y_0_hess_n_out(void);
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // lat_Y_0_COST
@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-
-#ifndef lat_Y_E_COST
-#define lat_Y_E_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-int lat_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_e_fun_work(int *, int *, int *, int *);
-const int *lat_cost_y_e_fun_sparsity_in(int);
-const int *lat_cost_y_e_fun_sparsity_out(int);
-int lat_cost_y_e_fun_n_in(void);
-int lat_cost_y_e_fun_n_out(void);
-
-int lat_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
-const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
-int lat_cost_y_e_fun_jac_ut_xt_n_in(void);
-int lat_cost_y_e_fun_jac_ut_xt_n_out(void);
-
-int lat_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_e_hess_work(int *, int *, int *, int *);
-const int *lat_cost_y_e_hess_sparsity_in(int);
-const int *lat_cost_y_e_hess_sparsity_out(int);
-int lat_cost_y_e_hess_n_in(void);
-int lat_cost_y_e_hess_n_out(void);
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // lat_Y_E_COST
@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-
-#ifndef lat_Y_COST
-#define lat_Y_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-int lat_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_fun_work(int *, int *, int *, int *);
-const int *lat_cost_y_fun_sparsity_in(int);
-const int *lat_cost_y_fun_sparsity_out(int);
-int lat_cost_y_fun_n_in(void);
-int lat_cost_y_fun_n_out(void);
-
-int lat_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *lat_cost_y_fun_jac_ut_xt_sparsity_in(int);
-const int *lat_cost_y_fun_jac_ut_xt_sparsity_out(int);
-int lat_cost_y_fun_jac_ut_xt_n_in(void);
-int lat_cost_y_fun_jac_ut_xt_n_out(void);
-
-int lat_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int lat_cost_y_hess_work(int *, int *, int *, int *);
-const int *lat_cost_y_hess_sparsity_in(int);
-const int *lat_cost_y_hess_sparsity_out(int);
-int lat_cost_y_hess_n_in(void);
-int lat_cost_y_hess_n_out(void);
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // lat_Y_COST
@@ -1,125 +0,0 @@
-%
-% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
-% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
-% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
-% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
-%
-% This file is part of acados.
-%
-% The 2-Clause BSD License
-%
-% Redistribution and use in source and binary forms, with or without
-% modification, are permitted provided that the following conditions are met:
-%
-% 1. Redistributions of source code must retain the above copyright notice,
-% this list of conditions and the following disclaimer.
-%
-% 2. Redistributions in binary form must reproduce the above copyright notice,
-% this list of conditions and the following disclaimer in the documentation
-% and/or other materials provided with the distribution.
-%
-% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-% POSSIBILITY OF SUCH DAMAGE.;
-%
-
-SOURCES = { ...
-            'lat_model/lat_expl_ode_fun.c', ...
-            'lat_model/lat_expl_vde_forw.c',...
-            'lat_cost/lat_cost_y_0_fun.c',...
-            'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
-            'lat_cost/lat_cost_y_0_hess.c',...
-            'lat_cost/lat_cost_y_fun.c',...
-            'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
-            'lat_cost/lat_cost_y_hess.c',...
-            'lat_cost/lat_cost_y_e_fun.c',...
-            'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
-            'lat_cost/lat_cost_y_e_hess.c',...
-            'acados_solver_sfunction_lat.c', ...
-            'acados_solver_lat.c'
-          };
-
-INC_PATH = '/data/openpilot-special/third_party/acados/include';
-
-INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
-        ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
-        ['-I', fullfile(INC_PATH, 'acados')], ...
-        ['-I', fullfile(INC_PATH)]};
-
-
-
-CFLAGS = 'CFLAGS=$CFLAGS';
-LDFLAGS = 'LDFLAGS=$LDFLAGS';
-COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
-COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
-
-
-
-LIB_PATH = ['-L', fullfile('/data/openpilot-special/third_party/acados/lib')];
-
-LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
-
-% acados linking libraries and flags
-    
-
-mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
-    LIB_PATH, LIBS{:}, SOURCES{:}, ...
-    '-output', 'acados_solver_sfunction_lat' );
-
-fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
-    eval('mexext')] );
-
-
-%% print note on usage of s-function
-fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
-input_note = 'Inputs are:\n';
-i_in = 1;
-input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
-                    ' size [4]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
-                    ' size [4]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
-                    ' size [66]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
-                    ' size [155]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [62]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [62]\n ');
-i_in = i_in + 1;
-
-fprintf(input_note)
-
-disp(' ')
-
-output_note = 'Outputs are:\n';
-i_out = 0;
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
-
-fprintf(output_note)
@@ -1,264 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-#define S_FUNCTION_NAME acados_solver_sfunction_long
-#define S_FUNCTION_LEVEL 2
-
-#define MDL_START
-
-// acados
-// #include "acados/utils/print.h"
-#include "acados_c/sim_interface.h"
-#include "acados_c/external_function_interface.h"
-
-// example specific
-#include "long_model/long_model.h"
-#include "acados_solver_long.h"
-
-#include "simstruc.h"
-
-#define SAMPLINGTIME 0.06944444444444445
-
-static void mdlInitializeSizes (SimStruct *S)
-{
-    // specify the number of continuous and discrete states
-    ssSetNumContStates(S, 0);
-    ssSetNumDiscStates(S, 0);// specify the number of input ports
-    if ( !ssSetNumInputPorts(S, 8) )
-        return;
-
-    // specify the number of output ports
-    if ( !ssSetNumOutputPorts(S, 6) )
-        return;
-
-    // specify dimension information for the input ports
-    // lbx_0
-    ssSetInputPortVectorDimension(S, 0, 3);
-    // ubx_0
-    ssSetInputPortVectorDimension(S, 1, 3);
-    // parameters
-    ssSetInputPortVectorDimension(S, 2, (12+1) * 6);
-    // y_ref_0
-    ssSetInputPortVectorDimension(S, 3, 6);
-    // y_ref
-    ssSetInputPortVectorDimension(S, 4, 66);
-    // y_ref_e
-    ssSetInputPortVectorDimension(S, 5, 5);
-    // lh
-    ssSetInputPortVectorDimension(S, 6, 4);
-    // uh
-    ssSetInputPortVectorDimension(S, 7, 4);/* specify dimension information for the OUTPUT ports */
-    ssSetOutputPortVectorDimension(S, 0, 1 );
-    ssSetOutputPortVectorDimension(S, 1, 1 );
-    ssSetOutputPortVectorDimension(S, 2, 1 );
-    ssSetOutputPortVectorDimension(S, 3, 3 ); // state at shooting node 1
-    ssSetOutputPortVectorDimension(S, 4, 1);
-    ssSetOutputPortVectorDimension(S, 5, 1 );
-
-    // specify the direct feedthrough status
-    // should be set to 1 for all inputs used in mdlOutputs
-    ssSetInputPortDirectFeedThrough(S, 0, 1);
-    ssSetInputPortDirectFeedThrough(S, 1, 1);
-    ssSetInputPortDirectFeedThrough(S, 2, 1);
-    ssSetInputPortDirectFeedThrough(S, 3, 1);
-    ssSetInputPortDirectFeedThrough(S, 4, 1);
-    ssSetInputPortDirectFeedThrough(S, 5, 1);
-    ssSetInputPortDirectFeedThrough(S, 6, 1);
-    ssSetInputPortDirectFeedThrough(S, 7, 1);
-
-    // one sample time
-    ssSetNumSampleTimes(S, 1);
-}
-
-
-#if defined(MATLAB_MEX_FILE)
-
-#define MDL_SET_INPUT_PORT_DIMENSION_INFO
-#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
-
-static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-    if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
-         return;
-}
-
-static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
-{
-    if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
-         return;
-}
-
-#endif /* MATLAB_MEX_FILE */
-
-
-static void mdlInitializeSampleTimes(SimStruct *S)
-{
-    ssSetSampleTime(S, 0, SAMPLINGTIME);
-    ssSetOffsetTime(S, 0, 0.0);
-}
-
-
-static void mdlStart(SimStruct *S)
-{
-    long_solver_capsule *capsule = long_acados_create_capsule();
-    long_acados_create(capsule);
-
-    ssSetUserData(S, (void*)capsule);
-}
-
-
-static void mdlOutputs(SimStruct *S, int_T tid)
-{
-    long_solver_capsule *capsule = ssGetUserData(S);
-    ocp_nlp_config *nlp_config = long_acados_get_nlp_config(capsule);
-    ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(capsule);
-    ocp_nlp_in *nlp_in = long_acados_get_nlp_in(capsule);
-    ocp_nlp_out *nlp_out = long_acados_get_nlp_out(capsule);
-
-    InputRealPtrsType in_sign;      
-
-    // local buffer
-    real_t buffer[6];
-
-    /* go through inputs */
-    // lbx_0
-    in_sign = ssGetInputPortRealSignalPtrs(S, 0);
-    for (int i = 0; i < 3; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
-    // ubx_0
-    in_sign = ssGetInputPortRealSignalPtrs(S, 1);
-    for (int i = 0; i < 3; i++)
-        buffer[i] = (double)(*in_sign[i]);
-    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
-    // parameters - stage-variant !!!
-    in_sign = ssGetInputPortRealSignalPtrs(S, 2);
-
-    // update value of parameters
-    for (int ii = 0; ii <= 12; ii++)
-    {
-        for (int jj = 0; jj < 6; jj++)
-            buffer[jj] = (double)(*in_sign[ii*6+jj]);
-        long_acados_update_params(capsule, ii, buffer, 6);
-    }
-
-  
-    // y_ref_0
-    in_sign = ssGetInputPortRealSignalPtrs(S, 3);
-
-    for (int i = 0; i < 6; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
-
-  
-    // y_ref - for stages 1 to N-1
-    in_sign = ssGetInputPortRealSignalPtrs(S, 4);
-
-    for (int ii = 1; ii < 12; ii++)
-    {
-        for (int jj = 0; jj < 6; jj++)
-            buffer[jj] = (double)(*in_sign[(ii-1)*6+jj]);
-        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
-    }
-
-  
-    // y_ref_e
-    in_sign = ssGetInputPortRealSignalPtrs(S, 5);
-
-    for (int i = 0; i < 5; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 12, "yref", (void *) buffer);
-    // lh
-    in_sign = ssGetInputPortRealSignalPtrs(S, 6);
-
-    for (int i = 0; i < 4; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    for (int ii = 0; ii < 12; ii++)
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
-    // uh
-    in_sign = ssGetInputPortRealSignalPtrs(S, 7);
-
-    for (int i = 0; i < 4; i++)
-        buffer[i] = (double)(*in_sign[i]);
-
-    for (int ii = 0; ii < 12; ii++)
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
-
-    /* call solver */
-    int rti_phase = 0;
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
-    int acados_status = long_acados_solve(capsule);
-
-
-    /* set outputs */
-    // assign pointers to output signals
-    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
-    int tmp_int;
-    out_u0 = ssGetOutputPortRealSignal(S, 0);
-    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
-
-  
-    out_status = ssGetOutputPortRealSignal(S, 1);
-    *out_status = (real_t) acados_status;
-    out_KKT_res = ssGetOutputPortRealSignal(S, 2);
-    *out_KKT_res = (real_t) nlp_out->inf_norm_res;
-    out_x1 = ssGetOutputPortRealSignal(S, 3);
-    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
-    out_cpu_time = ssGetOutputPortRealSignal(S, 4);
-    // get solution time
-    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
-    out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
-    // get sqp iter
-    ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
-    *out_sqp_iter = (real_t) tmp_int;
-
-}
-
-static void mdlTerminate(SimStruct *S)
-{
-    long_solver_capsule *capsule = ssGetUserData(S);
-
-    long_acados_free(capsule);
-    long_acados_free_capsule(capsule);
-}
-
-
-#ifdef  MATLAB_MEX_FILE
-#include "simulink.c"
-#else
-#include "cg_sfun.h"
-#endif
@@ -1,63 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-#ifndef long_H_CONSTRAINT
-#define long_H_CONSTRAINT
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-int long_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
-const int *long_constr_h_fun_jac_uxt_zt_sparsity_in(int);
-const int *long_constr_h_fun_jac_uxt_zt_sparsity_out(int);
-int long_constr_h_fun_jac_uxt_zt_n_in(void);
-int long_constr_h_fun_jac_uxt_zt_n_out(void);
-
-int long_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_constr_h_fun_work(int *, int *, int *, int *);
-const int *long_constr_h_fun_sparsity_in(int);
-const int *long_constr_h_fun_sparsity_out(int);
-int long_constr_h_fun_n_in(void);
-int long_constr_h_fun_n_out(void);
-
-
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // long_H_CONSTRAINT
@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-
-#ifndef long_Y_0_COST
-#define long_Y_0_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-int long_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_0_fun_work(int *, int *, int *, int *);
-const int *long_cost_y_0_fun_sparsity_in(int);
-const int *long_cost_y_0_fun_sparsity_out(int);
-int long_cost_y_0_fun_n_in(void);
-int long_cost_y_0_fun_n_out(void);
-
-int long_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *long_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
-const int *long_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
-int long_cost_y_0_fun_jac_ut_xt_n_in(void);
-int long_cost_y_0_fun_jac_ut_xt_n_out(void);
-
-int long_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_0_hess_work(int *, int *, int *, int *);
-const int *long_cost_y_0_hess_sparsity_in(int);
-const int *long_cost_y_0_hess_sparsity_out(int);
-int long_cost_y_0_hess_n_in(void);
-int long_cost_y_0_hess_n_out(void);
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // long_Y_0_COST
@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-
-#ifndef long_Y_E_COST
-#define long_Y_E_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-int long_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_e_fun_work(int *, int *, int *, int *);
-const int *long_cost_y_e_fun_sparsity_in(int);
-const int *long_cost_y_e_fun_sparsity_out(int);
-int long_cost_y_e_fun_n_in(void);
-int long_cost_y_e_fun_n_out(void);
-
-int long_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *long_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
-const int *long_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
-int long_cost_y_e_fun_jac_ut_xt_n_in(void);
-int long_cost_y_e_fun_jac_ut_xt_n_out(void);
-
-int long_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_e_hess_work(int *, int *, int *, int *);
-const int *long_cost_y_e_hess_sparsity_in(int);
-const int *long_cost_y_e_hess_sparsity_out(int);
-int long_cost_y_e_hess_n_in(void);
-int long_cost_y_e_hess_n_out(void);
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // long_Y_E_COST
@@ -1,69 +0,0 @@
-/*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
- * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
- * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
- * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
- *
- * This file is part of acados.
- *
- * The 2-Clause BSD License
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice,
- * this list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- * this list of conditions and the following disclaimer in the documentation
- * and/or other materials provided with the distribution.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.;
- */
-
-
-#ifndef long_Y_COST
-#define long_Y_COST
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_fun_work(int *, int *, int *, int *);
-const int *long_cost_y_fun_sparsity_in(int);
-const int *long_cost_y_fun_sparsity_out(int);
-int long_cost_y_fun_n_in(void);
-int long_cost_y_fun_n_out(void);
-
-int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
-const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
-const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
-int long_cost_y_fun_jac_ut_xt_n_in(void);
-int long_cost_y_fun_jac_ut_xt_n_out(void);
-
-int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
-int long_cost_y_hess_work(int *, int *, int *, int *);
-const int *long_cost_y_hess_sparsity_in(int);
-const int *long_cost_y_hess_sparsity_out(int);
-int long_cost_y_hess_n_in(void);
-int long_cost_y_hess_n_out(void);
-
-
-#ifdef __cplusplus
-} /* extern "C" */
-#endif
-
-#endif  // long_Y_COST
@@ -1,128 +0,0 @@
-%
-% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
-% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
-% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
-% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
-%
-% This file is part of acados.
-%
-% The 2-Clause BSD License
-%
-% Redistribution and use in source and binary forms, with or without
-% modification, are permitted provided that the following conditions are met:
-%
-% 1. Redistributions of source code must retain the above copyright notice,
-% this list of conditions and the following disclaimer.
-%
-% 2. Redistributions in binary form must reproduce the above copyright notice,
-% this list of conditions and the following disclaimer in the documentation
-% and/or other materials provided with the distribution.
-%
-% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-% POSSIBILITY OF SUCH DAMAGE.;
-%
-
-SOURCES = { ...
-            'long_model/long_expl_ode_fun.c', ...
-            'long_model/long_expl_vde_forw.c',...
-            'long_cost/long_cost_y_0_fun.c',...
-            'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
-            'long_cost/long_cost_y_0_hess.c',...
-            'long_cost/long_cost_y_fun.c',...
-            'long_cost/long_cost_y_fun_jac_ut_xt.c',...
-            'long_cost/long_cost_y_hess.c',...
-            'long_cost/long_cost_y_e_fun.c',...
-            'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
-            'long_cost/long_cost_y_e_hess.c',...
-            'long_constraints/long_constr_h_fun.c', ...
-            'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
-            'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
-            'acados_solver_sfunction_long.c', ...
-            'acados_solver_long.c'
-          };
-
-INC_PATH = '/data/openpilot-special/third_party/acados/include';
-
-INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
-        ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
-        ['-I', fullfile(INC_PATH, 'acados')], ...
-        ['-I', fullfile(INC_PATH)]};
-
-
-
-CFLAGS = 'CFLAGS=$CFLAGS';
-LDFLAGS = 'LDFLAGS=$LDFLAGS';
-COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
-COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
-
-
-
-LIB_PATH = ['-L', fullfile('/data/openpilot-special/third_party/acados/lib')];
-
-LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
-
-% acados linking libraries and flags
-    
-
-mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
-    LIB_PATH, LIBS{:}, SOURCES{:}, ...
-    '-output', 'acados_solver_sfunction_long' );
-
-fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
-    eval('mexext')] );
-
-
-%% print note on usage of s-function
-fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
-input_note = 'Inputs are:\n';
-i_in = 1;
-input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
-                    ' size [3]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
-                    ' size [3]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
-                    ' size [78]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
-                    ' size [66]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
-i_in = i_in + 1;
-input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\n ');
-i_in = i_in + 1;
-
-fprintf(input_note)
-
-disp(' ')
-
-output_note = 'Outputs are:\n';
-i_out = 0;
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
-i_out = i_out + 1;
-output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
-
-fprintf(output_note)
@@ -78,7 +78,7 @@ class LongitudinalPlanner:

    self.cruise_source = 'cruise'
    self.vision_turn_controller = VisionTurnController(CP)
-    self.speed_limit_controller = SpeedLimitController()
+    self.speed_limit_controller = SpeedLimitController(CP)
    self.events = Events()
    self.turn_speed_controller = TurnSpeedController()
    self.dynamic_experimental_controller = DynamicExperimentalController()
@@ -114,7 +114,7 @@ class LongitudinalPlanner:
      self.read_param()
    self.param_read_counter += 1
    if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
-      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode(self.mpc.mode, self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'])
+      self.mpc.mode = self.dynamic_experimental_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'

@@ -151,13 +151,13 @@ class LongitudinalPlanner:
    if force_slow_decel:
      v_cruise = 0.0

-    # Get acceleration and active solutions for custom long mpc.
-    self.cruise_source, a_min_sol, v_cruise_sol = self.cruise_solutions(
-      not reset_state and (self.CP.openpilotLongitudinalControl or not self.CP.pcmCruiseSpeed), self.v_desired_filter.x,
-      self.a_desired, v_cruise, sm)
+    # Get active solutions for custom long mpc.
+    self.cruise_source, v_cruise_sol = self.cruise_solutions(
+      prev_accel_constraint and (self.CP.openpilotLongitudinalControl or not self.CP.pcmCruiseSpeed),
+      self.v_desired_filter.x, self.a_desired, v_cruise, sm)

    # clip limits, cannot init MPC outside of bounds
-    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol)
+    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)

    self.mpc.set_weights(prev_accel_constraint, personality=self.personality)
@@ -217,7 +217,7 @@ class LongitudinalPlanner:
    longitudinalPlanSP.e2eX = self.mpc.e2e_x.tolist()

    longitudinalPlanSP.visionTurnControllerState = self.vision_turn_controller.state
-    longitudinalPlanSP.visionTurnSpeed = float(self.vision_turn_controller.v_turn)
+    longitudinalPlanSP.visionTurnSpeed = float(self.vision_turn_controller.v_target)
    longitudinalPlanSP.visionCurrentLatAcc = float(self.vision_turn_controller.current_lat_acc)
    longitudinalPlanSP.visionMaxPredLatAcc = float(self.vision_turn_controller.max_pred_lat_acc)

@@ -229,9 +229,7 @@ class LongitudinalPlanner:
    longitudinalPlanSP.events = self.events.to_msg()

    longitudinalPlanSP.turnSpeedControlState = self.turn_speed_controller.state
-    longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.speed_limit)
-    longitudinalPlanSP.distToTurn = float(self.turn_speed_controller.distance)
-    longitudinalPlanSP.turnSign = int(self.turn_speed_controller.turn_sign)
+    longitudinalPlanSP.turnSpeed = float(self.turn_speed_controller.v_target)

    longitudinalPlanSP.personality = self.personality

@@ -241,30 +239,26 @@ class LongitudinalPlanner:

  def cruise_solutions(self, enabled, v_ego, a_ego, v_cruise, sm):
    # Update controllers
-    self.vision_turn_controller.update(enabled, v_ego, a_ego, v_cruise, sm)
+    self.vision_turn_controller.update(enabled, v_ego, v_cruise, sm)
    self.events = Events()
-    self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.CP, self.events)
-    self.turn_speed_controller.update(enabled, v_ego, a_ego, sm)
+    self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.events)
+    self.turn_speed_controller.update(enabled, v_ego, sm, v_cruise)

    # Pick solution with the lowest velocity target.
-    a_solutions = {'cruise': float("inf")}
    v_solutions = {'cruise': v_cruise}

    if self.vision_turn_controller.is_active:
-      a_solutions['turn'] = self.vision_turn_controller.a_target
-      v_solutions['turn'] = self.vision_turn_controller.v_turn
+      v_solutions['turn'] = self.vision_turn_controller.v_target

    if self.speed_limit_controller.is_active:
-      a_solutions['limit'] = self.speed_limit_controller.a_target
      v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted

    if self.turn_speed_controller.is_active:
-      a_solutions['turnlimit'] = self.turn_speed_controller.a_target
-      v_solutions['turnlimit'] = self.turn_speed_controller.speed_limit
+      v_solutions['turnlimit'] = self.turn_speed_controller.v_target

    source = min(v_solutions, key=v_solutions.get)

-    return source, a_solutions[source], v_solutions[source]
+    return source, v_solutions[source]

  def e2e_events(self, sm):
    e2e_long_status = sm['e2eLongStateSP'].status
@@ -16,3 +16,14 @@ class Policy(IntEnum):
  map_data_priority = 4
  car_state_priority = 5
  combined = 6
+
+
+class Engage(IntEnum):
+  auto = 0
+  user_confirm = 1
+
+
+class OffsetType(IntEnum):
+  default = 0
+  fixed = 1
+  percentage = 2
@@ -1,14 +1,12 @@
-import numpy as np
 import time
-from common.numpy_fast import clip, interp
-from common.conversions import Conversions as CV
-from common.params import Params
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import clip, interp
+from openpilot.common.params import Params
 from openpilot.selfdrive.controls.lib.sunnypilot import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \
  PARAMS_UPDATE_PERIOD, TEMP_INACTIVE_GUARD_PERIOD, EventName, SpeedLimitControlState
-from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_MIN_ACC, LIMIT_MAX_ACC, LIMIT_SPEED_OFFSET_TH, \
-  CONTROL_N
+from openpilot.selfdrive.controls.lib.drive_helpers import LIMIT_SPEED_OFFSET_TH, CONTROL_N
 from openpilot.selfdrive.controls.lib.events import Events, ET
-from openpilot.selfdrive.controls.lib.sunnypilot.common import Source, Policy
+from openpilot.selfdrive.controls.lib.sunnypilot.common import Source, Policy, Engage, OffsetType
 from openpilot.selfdrive.controls.lib.sunnypilot.helpers import description_for_state, debug
 from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_resolver import SpeedLimitResolver
 from openpilot.selfdrive.modeld.constants import ModelConstants
@@ -17,8 +15,9 @@ ACTIVE_STATES = (SpeedLimitControlState.active, SpeedLimitControlState.adapting)


 class SpeedLimitController:
-  def __init__(self):
+  def __init__(self, CP):
    self._params = Params()
+    self._CP = CP
    self._policy = Policy(int(self._params.get("SpeedLimitControlPolicy", encoding='utf8')))
    self._resolver = SpeedLimitResolver(self._policy)
    self._last_params_update = 0.0
@@ -42,17 +41,21 @@ class SpeedLimitController:
    self._state = SpeedLimitControlState.inactive
    self._state_prev = SpeedLimitControlState.inactive
    self._gas_pressed = False
-    self._a_target = 0.
+    self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise

    self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
    self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
-    self._engage_type = int(self._params.get("SpeedLimitEngageType", encoding='utf8'))
+    self._warning_type_type = int(self._params.get("SpeedLimitWarningType", encoding='utf8'))
+    self._warning_offset_type = int(self._params.get("SpeedLimitWarningOffsetType", encoding='utf8'))
+    self._warning_offset_value = float(self._params.get("SpeedLimitWarningValueOffset", encoding='utf8'))
+    self._engage_type = clip(int(self._params.get("SpeedLimitEngageType", encoding='utf8')), Engage.auto, Engage.user_confirm)
    self._brake_pressed = False
    self._brake_pressed_prev = False
    self._current_time = 0.
    self._v_cruise_rounded = 0.
    self._v_cruise_prev_rounded = 0.
    self._speed_limit_offsetted_rounded = 0.
+    self._speed_limit_warning_offsetted_rounded = 0.
    self._ms_to_local = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH

    # Mapping functions to state transitions
@@ -75,10 +78,6 @@ class SpeedLimitController:
      SpeedLimitControlState.preActive: self.get_current_acceleration_as_target,
    }

-  @property
-  def a_target(self):
-    return self._a_target if self.is_active else self._a_ego
-
  @property
  def state(self):
    return self._state
@@ -105,14 +104,24 @@ class SpeedLimitController:

  @property
  def speed_limit_offset(self):
-    if self._offset_type == 0:
+    if self._offset_type == OffsetType.default:
      return interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit
-    elif self._offset_type == 1:
+    elif self._offset_type == OffsetType.fixed:
      return self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
-    elif self._offset_type == 2:
+    elif self._offset_type == OffsetType.percentage:
      return self._offset_value * 0.01 * self._speed_limit
    return 0.

+  @property
+  def speed_limit_warning_offset(self):
+    if self._warning_offset_type == OffsetType.default:
+      return interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit
+    elif self._warning_offset_type == OffsetType.fixed:
+      return self._warning_offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)
+    elif self._warning_offset_type == OffsetType.percentage:
+      return self._warning_offset_value * 0.01 * self._speed_limit
+    return 0.
+
  @property
  def speed_limit(self):
    return self._speed_limit
@@ -128,18 +137,19 @@ class SpeedLimitController:
  def _update_v_cruise_setpoint_prev(self):
    self._v_cruise_setpoint_prev = self._v_cruise_setpoint

-  def _update_params(self, CP):
+  def _update_params(self):
    if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD:
      self._is_enabled = self._params.get_bool("EnableSlc")
      self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
      self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
+      self._warning_type_type = int(self._params.get("SpeedLimitWarningType", encoding='utf8'))
+      self._warning_offset_type = int(self._params.get("SpeedLimitWarningOffsetType", encoding='utf8'))
+      self._warning_offset_value = float(self._params.get("SpeedLimitWarningValueOffset", encoding='utf8'))
      self._policy = Policy(int(self._params.get("SpeedLimitControlPolicy", encoding='utf8')))
      self._is_metric = self._params.get_bool("IsMetric")
      self._ms_to_local = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH
      self._resolver.change_policy(self._policy)
-      pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise
-      self._engage_type = clip(int(self._params.get("SpeedLimitEngageType", encoding='utf8')), 0, 1) if pcm_cruise_op_long else \
-          int(self._params.get("SpeedLimitEngageType", encoding='utf8'))
+      self._engage_type = Engage.auto if self._pcm_cruise_op_long else clip(int(self._params.get("SpeedLimitEngageType", encoding='utf8')), Engage.auto, Engage.user_confirm)
      debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_type}')
      self._last_params_update = self._current_time

@@ -158,24 +168,25 @@ class SpeedLimitController:
    self._speed_limit_changed = self._speed_limit != self._speed_limit_prev
    self._v_cruise_setpoint_changed = self._v_cruise_setpoint != self._v_cruise_setpoint_prev
    self._speed_limit_prev = self._speed_limit
-    if self._engage_type != 2:
+    if self._engage_type != Engage.user_confirm:
      self._update_v_cruise_setpoint_prev()
    self._op_enabled_prev = self._op_enabled
    self._brake_pressed_prev = self._brake_pressed

    self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._ms_to_local))
    self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._ms_to_local))
-    self._speed_limit_offsetted_rounded = int(round((self._speed_limit + self.speed_limit_offset) * self._ms_to_local))
+    self._speed_limit_offsetted_rounded = int(0) if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_offset) * self._ms_to_local))
+    self._speed_limit_warning_offsetted_rounded = int(0) if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_warning_offset) * self._ms_to_local))

  def transition_state_from_inactive(self):
    """ Make state transition from inactive state """
-    if self._engage_type == 2:
+    if self._engage_type == Engage.user_confirm:
      if (((self._last_op_enabled_time + 7.) >= self._current_time >= (self._last_op_enabled_time + 2.)) or
              self._speed_limit_changed):
        if self._speed_limit_changed:
          self._last_op_enabled_time = self._current_time - 2.  # immediately prompt confirmation
        self.state = SpeedLimitControlState.preActive
-    elif self._engage_type == 1:
+    elif self._engage_type == Engage.auto:
      if self._v_offset < LIMIT_SPEED_OFFSET_TH:
        self.state = SpeedLimitControlState.adapting
      else:
@@ -184,10 +195,10 @@ class SpeedLimitController:
  def transition_state_from_temp_inactive(self):
    """ Make state transition from temporary inactive state """
    if self._speed_limit_changed:
-      if self._engage_type == 2:
+      if self._engage_type == Engage.user_confirm:
        self._last_op_enabled_time = self._current_time - 2.  # immediately prompt confirmation
        self.state = SpeedLimitControlState.preActive
-      elif self._engage_type == 1:
+      elif self._engage_type == Engage.auto:
        self.state = SpeedLimitControlState.inactive

  def transition_state_from_pre_active(self):
@@ -213,14 +224,14 @@ class SpeedLimitController:

  def transition_state_from_active(self):
    """ Make state transition from active state """
-    if self._engage_type == 2:
+    if self._engage_type == Engage.user_confirm:
      if self._state_prev == SpeedLimitControlState.active:
        if self._v_cruise_setpoint_changed and self._v_cruise_rounded != self._speed_limit_offsetted_rounded:
          self.state = SpeedLimitControlState.tempInactive
        elif self._speed_limit_changed:
          self._last_op_enabled_time = self._current_time - 2.  # immediately prompt confirmation
          self.state = SpeedLimitControlState.preActive
-    elif self._engage_type == 1:
+    elif self._engage_type == Engage.auto:
      if self._v_offset < LIMIT_SPEED_OFFSET_TH:
        self.state = SpeedLimitControlState.adapting

@@ -229,22 +240,22 @@ class SpeedLimitController:

    # In any case, if op is disabled, or speed limit control is disabled
    # or the reported speed limit is 0 or gas is pressed, deactivate.
-    if (not self._op_enabled or not self._is_enabled or self._speed_limit == 0 or
-            (self._gas_pressed and self._disengage_on_accelerator)) or self._engage_type == 0:
+    if not self._op_enabled or not self._is_enabled or self._speed_limit == 0 or \
+            (self._gas_pressed and self._disengage_on_accelerator):
      self.state = SpeedLimitControlState.inactive
      return

    # In any case, we deactivate the speed limit controller temporarily if the user changes the cruise speed.
    # Ignore if a minimum amount 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._engage_type == 1 and self._v_cruise_setpoint_changed and \
+    if self._engage_type == Engage.auto and self._v_cruise_setpoint_changed and \
            self._current_time > (self._last_op_enabled_time + TEMP_INACTIVE_GUARD_PERIOD):
      self.state = SpeedLimitControlState.tempInactive
      return

    self.state_transition_strategy[self.state]()

-    if self._engage_type == 2:
+    if self._engage_type == Engage.user_confirm:
      self._update_v_cruise_setpoint_prev()

  def get_current_acceleration_as_target(self):
@@ -262,29 +273,27 @@ class SpeedLimitController:
    """ In active state, aim to keep speed constant around control time horizon """
    return self._v_offset / ModelConstants.T_IDXS[CONTROL_N]

-  def _update_solution(self):
-    a_target = self.acceleration_solutions[self.state]()
-
-    # Keep solution limited.
-    self._a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC)
-
  def _update_events(self, events):
+    if self._speed_limit > 0 and self._warning_type_type == 2 and \
+      self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._ms_to_local)):
+      events.add(EventName.speedLimitPreActive)
+
    if not self.is_active:
      if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \
        self._v_cruise_rounded != self._speed_limit_offsetted_rounded:
          events.add(EventName.speedLimitPreActive)
    else:
-      if self._engage_type == 2:
+      if self._engage_type == Engage.user_confirm:
        if self._state_prev == SpeedLimitControlState.preActive:
          events.add(EventName.speedLimitConfirmed)
          events.add(EventName.speedLimitActive)
-      elif self._engage_type == 1:
+      elif self._engage_type == Engage.auto:
        if self._state_prev not in ACTIVE_STATES:
          events.add(EventName.speedLimitActive)
        elif self._speed_limit_changed != 0:
          events.add(EventName.speedLimitValueChange)

-  def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, CP, events=Events()):
+  def update(self, enabled, v_ego, a_ego, sm, v_cruise_setpoint, events=Events()):
    _car_state = sm['carState']
    self._op_enabled = sm['controlsState'].enabled and _car_state.cruiseState.enabled and \
                       not (_car_state.brakePressed and (not self._brake_pressed_prev or not _car_state.standstill)) and \
@@ -298,8 +307,7 @@ class SpeedLimitController:

    self._speed_limit, self._distance, self._source = self._resolver.resolve(v_ego, self.speed_limit, sm)

-    self._update_params(CP)
+    self._update_params()
    self._update_calculations()
    self._state_transition()
-    self._update_solution()
    self._update_events(events)
@@ -21,20 +21,20 @@ class SpeedLimitResolver:
      Policy.combined: [Source.car_state, Source.nav, Source.map_data],
      Policy.nav_only: [Source.nav]
    }
-    self._reset_limit_sources()
+    for source in Source:
+      self._reset_limit_sources(source)

  def change_policy(self, policy: Policy):
    self._policy = policy

-  def _reset_limit_sources(self):
-    self._limit_solutions.clear()
-    self._distance_solutions.clear()
-    for source in Source:
-      self._limit_solutions[source] = 0.
-      self._distance_solutions[source] = 0.
+  def _reset_limit_sources(self, source):
+    self._limit_solutions[source] = 0.
+    self._distance_solutions[source] = 0.
+
+  def _is_sock_updated(self, sock):
+    return self._sm.alive[sock] and self._sm.updated[sock]

  def resolve(self, v_ego, current_speed_limit, sm):
-    self._reset_limit_sources()
    self._v_ego = v_ego
    self._current_speed_limit = current_speed_limit
    self._sm = sm
@@ -49,36 +49,44 @@ class SpeedLimitResolver:
    self._get_from_map_data()

  def _get_from_car_state(self):
+    if not self._is_sock_updated('carState'):
+      debug('SL: No carState instruction for speed limit')
+      return
+
+    self._reset_limit_sources(Source.car_state)
    self._limit_solutions[Source.car_state] = self._sm['carState'].cruiseState.speedLimit
    self._distance_solutions[Source.car_state] = 0.

  def _get_from_nav(self):
-    if not self._sm.alive['navInstruction']:
+    if not self._is_sock_updated('navInstruction'):
      debug('SL: No nav instruction for speed limit')
      return

    # Load limits from nav instruction
+    self._reset_limit_sources(Source.nav)
    self._limit_solutions[Source.nav] = self._sm['navInstruction'].speedLimit
    self._distance_solutions[Source.nav] = 0.

  def _get_from_map_data(self):
    sock = 'liveMapDataSP'
-    if self._sm.logMonoTime[sock] is None:
+
+    if not self._is_sock_updated(sock):
      debug('SL: No map data for speed limit')
      return

    # Load limits from map_data
+    self._reset_limit_sources(Source.map_data)
    self._process_map_data(self._sm[sock])

  def _process_map_data(self, map_data):
-    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
-    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
-
    gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3
    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
      debug(f'SL: Ignoring map data as is too old. Age: {gps_fix_age}')
      return

+    speed_limit = map_data.speedLimit if map_data.speedLimitValid else 0.
+    next_speed_limit = map_data.speedLimitAhead if map_data.speedLimitAheadValid else 0.
+
    self._calculate_map_data_limits(speed_limit, next_speed_limit, map_data)

  def _calculate_map_data_limits(self, speed_limit, next_speed_limit, map_data):
@@ -1,62 +1,63 @@
-import numpy as np
+import json
+import math
 import time
-from common.params import Params
 from cereal import custom
-from 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 openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.common.params import Params
+from openpilot.system.version import is_release_sp_branch
+
+R = 6373000.0  # approximate radius of earth in meters
+TO_RADIANS = math.pi / 180
+TO_DEGREES = 180 / math.pi
+TARGET_JERK = -0.6  # m/s^3 There's some jounce limits that are not consistent so we're fudging this some
+TARGET_ACCEL = -1.2  # m/s^2 should match up with the long planner limit
+TARGET_OFFSET = 1.0  # seconds - This controls how soon before the curve you reach the target velocity. It also helps
+                     # reach the target velocity when innacuracies in the distance modeling logic would cause overshoot.
+                     # The value is multiplied against the target velocity to determine the additional distance. This is
+                     # done to keep the distance calculations consistent but results in the offset actually being less
+                     # time than specified depending on how much of a speed diffrential there is between v_ego and the
+                     # target velocity.


-_ACTIVE_LIMIT_MIN_ACC = -0.5  # m/s^2 Maximum deceleration allowed while active.
-_ACTIVE_LIMIT_MAX_ACC = 0.5   # m/s^2 Maximum acelration allowed while active.
+def calculate_accel(t, target_jerk, a_ego):
+  return a_ego + target_jerk * t


-_DEBUG = False
+def calculate_velocity(t, target_jerk, a_ego, v_ego):
+  return v_ego + a_ego * t + target_jerk/2 * (t ** 2)
+
+
+def calculate_distance(t, target_jerk, a_ego, v_ego):
+  return t * v_ego + a_ego/2 * (t ** 2) + target_jerk/6 * (t ** 3)
+
+
+PARAMS_UPDATE_PERIOD = 5.

 TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState


-def _debug(msg):
-  if not _DEBUG:
-    return
-  print(msg)
+# points should be in radians
+# output is meters
+def distance_to_point(ax, ay, bx, by):
+  a = math.sin((bx-ax)/2)*math.sin((bx-ax)/2) + math.cos(ax) * math.cos(bx)*math.sin((by-ay)/2)*math.sin((by-ay)/2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
+
+  return R * c  # in meters


-def _description_for_state(turn_speed_control_state):
-  if turn_speed_control_state == TurnSpeedControlState.inactive:
-    return 'INACTIVE'
-  if turn_speed_control_state == TurnSpeedControlState.tempInactive:
-    return 'TEMP INACTIVE'
-  if turn_speed_control_state == TurnSpeedControlState.adapting:
-    return 'ADAPTING'
-  if turn_speed_control_state == TurnSpeedControlState.active:
-    return 'ACTIVE'
-
-
-class TurnSpeedController():
+class TurnSpeedController:
  def __init__(self):
-    self._params = Params()
-    self._last_params_update = 0.
-    self._is_enabled = self._params.get_bool("TurnSpeedControl")
+    self.params = Params()
+    self.mem_params = Params("/dev/shm/params")
+    self.enabled = self.params.get_bool("TurnSpeedControl") and not is_release_sp_branch()
+    self.last_params_update = 0
    self._op_enabled = False
-    self._v_ego = 0.
-    self._a_ego = 0.
-    self._v_cruise_setpoint = 0.
-
-    self._v_offset = 0.
-    self._speed_limit = 0.
-    self._speed_limit_temp_inactive = 0.
-    self._distance = 0.
-    self._turn_sign = 0
+    self._gas_pressed = False
    self._state = TurnSpeedControlState.inactive
-
-    self._next_speed_limit_prev = 0.
-
-    self._a_target = 0.
-
-  @property
-  def a_target(self):
-    return self._a_target if self.is_active else self._a_ego
+    self._v_cruise = 0
+    self._min_v = 0
+    self.target_lat = 0.0
+    self.target_lon = 0.0
+    self.target_v = 0.0

  @property
  def state(self):
@@ -64,18 +65,6 @@ class TurnSpeedController():

  @state.setter
  def state(self, value):
-    if value != self._state:
-      _debug(f'Turn Speed Controller state: {_description_for_state(value)}')
-
-      if value == TurnSpeedControlState.adapting:
-        _debug('TSC: Enteriing Adapting as speed offset is below threshold')
-        _debug(f'_v_offset: {self._v_offset * 3.6}\nspeed_limit: {self.speed_limit * 3.6}')
-        _debug(f'_v_ego: {self._v_ego * 3.6}\ndistance: {self.distance}')
-
-      if value == TurnSpeedControlState.tempInactive:
-        # Track the speed limit value when controller was set to temp inactive.
-        self._speed_limit_temp_inactive = self._speed_limit
-
    self._state = value

  @property
@@ -83,161 +72,156 @@ class TurnSpeedController():
    return self.state > TurnSpeedControlState.tempInactive

  @property
-  def speed_limit(self):
-    return max(self._speed_limit, LIMIT_MIN_SPEED) if self._speed_limit > 0. else 0.
+  def v_target(self):
+    return self._min_v

-  @property
-  def distance(self):
-    return max(self._distance, 0.)
-
-  @property
-  def turn_sign(self):
-    return self._turn_sign
-
-  def _get_limit_from_map_data(self, sm):
-    """Provides the speed limit, distance and turn sign to it for turns based on map data.
-    """
-    # Ignore if no live map data
-    sock = 'liveMapDataSP'
-    if sm.logMonoTime[sock] is None:
-      _debug('TS: No map data for turn speed limit')
-      return 0., 0., 0
-
-    # Load map_data and initialize
-    map_data = sm[sock]
-    speed_limit = 0.
-
-    # Calculate the age of the gps fix. Ignore if too old.
-    gps_fix_age = time.time() - map_data.lastGpsTimestamp * 1e-3
-    if gps_fix_age > LIMIT_MAX_MAP_DATA_AGE:
-      _debug(f'TS: Ignoring map data as is too old. Age: {gps_fix_age}')
-      return 0., 0., 0
-
-    # Load turn ahead sections info from map_data with distances corrected by gps_fix_age
-    distance_since_fix = self._v_ego * gps_fix_age
-    distances_to_sections_ahead = np.maximum(0., np.array(map_data.turnSpeedLimitsAheadDistances) - distance_since_fix)
-    speed_limit_in_sections_ahead = map_data.turnSpeedLimitsAhead
-    turn_signs_in_sections_ahead = map_data.turnSpeedLimitsAheadSigns
-
-    # Ensure current speed limit is considered only if we are inside the section.
-    if map_data.turnSpeedLimitValid and self._v_ego > 0.:
-      speed_limit_end_time = (map_data.turnSpeedLimitEndDistance / self._v_ego) - gps_fix_age
-      if speed_limit_end_time > 0.:
-        speed_limit = map_data.turnSpeedLimit
-
-    # When we have no ahead speed limit to consider or all are greater than current speed limit
-    # or car has stopped, then provide current value and reset tracking.
-    turn_sign = map_data.turnSpeedLimitSign if map_data.turnSpeedLimitValid else 0
-    if len(speed_limit_in_sections_ahead) == 0 or self._v_ego <= 0. or \
-       (speed_limit > 0 and np.amin(speed_limit_in_sections_ahead) > speed_limit):
-      self._next_speed_limit_prev = 0.
-      return speed_limit, 0., turn_sign
-
-    # Calculated the time needed to adapt to the limits ahead and the corresponding distances.
-    adapt_times = (np.maximum(speed_limit_in_sections_ahead, LIMIT_MIN_SPEED) - self._v_ego) / LIMIT_ADAPT_ACC
-    adapt_distances = self._v_ego * adapt_times + 0.5 * LIMIT_ADAPT_ACC * adapt_times**2
-    distance_gaps = distances_to_sections_ahead - adapt_distances
-
-    # We select as next speed limit, the one that have the lowest distance gap.
-    next_idx = np.argmin(distance_gaps)
-    next_speed_limit = speed_limit_in_sections_ahead[next_idx]
-    distance_to_section_ahead = distances_to_sections_ahead[next_idx]
-    next_turn_sign = turn_signs_in_sections_ahead[next_idx]
-    distance_gap = distance_gaps[next_idx]
-
-    # When we have a next_speed_limit value that has not changed from a provided next speed limit value
-    # in previous resolutions, we keep providing it along with the updated distance to it.
-    if next_speed_limit == self._next_speed_limit_prev:
-      return next_speed_limit, distance_to_section_ahead, next_turn_sign
-
-    # Reset tracking
-    self._next_speed_limit_prev = 0.
-
-    # When we detect we are close enough, we provide the next limit value and track it.
-    if distance_gap <= 0.:
-      self._next_speed_limit_prev = next_speed_limit
-      return next_speed_limit, distance_to_section_ahead, next_turn_sign
-
-    # Otherwise we just provide the calculated speed_limit
-    return speed_limit, 0., turn_sign
-
-  def _update_params(self):
+  def update_params(self):
    t = time.monotonic()
-    if t > self._last_params_update + 5.0:
-      self._is_enabled = self._params.get_bool("TurnSpeedControl")
-      self._last_params_update = t
+    if t > self.last_params_update + PARAMS_UPDATE_PERIOD:
+      self.enabled = self.params.get_bool("TurnSpeedControl") and not is_release_sp_branch()
+      self.last_params_update = t

-  def _update_calculations(self):
-    # Update current velocity offset (error)
-    self._v_offset = self.speed_limit - self._v_ego
+  def target_speed(self, v_ego, a_ego) -> float:
+    if not self.enabled:
+      return 0.0

-  def _state_transition(self, sm):
-    # In any case, if op is disabled, or turn speed limit control is disabled
-    # or the reported speed limit is 0, deactivate.
-    if not self._op_enabled or not self._is_enabled or self.speed_limit == 0.:
+    lat = 0.0
+    lon = 0.0
+    try:
+      position = json.loads(self.mem_params.get("LastGPSPosition"))
+      lat = position["latitude"]
+      lon = position["longitude"]
+    except: return 0.0
+
+    try:
+      target_velocities = json.loads(self.mem_params.get("MapTargetVelocities"))
+    except: return 0.0
+
+    min_dist = 1000
+    min_idx = 0
+    distances = []
+
+    # find our location in the path
+    for i in range(len(target_velocities)):
+      target_velocity = target_velocities[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, tlat * TO_RADIANS, tlon * TO_RADIANS)
+      distances.append(d)
+      if d < min_dist:
+        min_dist = d
+        min_idx = i
+
+    # only look at values from our current position forward
+    forward_points = target_velocities[min_idx:]
+    forward_distances = distances[min_idx:]
+
+    # find velocities that we are within the distance we need to adjust for
+    valid_velocities = []
+    for i in range(len(forward_points)):
+      target_velocity = forward_points[i]
+      tlat = target_velocity["latitude"]
+      tlon = target_velocity["longitude"]
+      tv = target_velocity["velocity"]
+      if tv > v_ego:
+        continue
+
+      d = forward_distances[i]
+
+      a_diff = (a_ego - TARGET_ACCEL)
+      accel_t = abs(a_diff / TARGET_JERK)
+      min_accel_v = calculate_velocity(accel_t, TARGET_JERK, a_ego, v_ego)
+
+      max_d = 0
+      if tv > min_accel_v:
+        # calculate time needed based on target jerk
+        a = 0.5 * TARGET_JERK
+        b = a_ego
+        c = v_ego - tv
+        t_a = -1 * ((b**2 - 4 * a * c) ** 0.5 + b) / 2 * a
+        t_b = ((b**2 - 4 * a * c) ** 0.5 - b) / 2 * a
+        if not isinstance(t_a, complex) and t_a > 0:
+          t = t_a
+        else:
+          t = t_b
+        if isinstance(t, complex):
+          continue
+
+        max_d = max_d + calculate_distance(t, TARGET_JERK, a_ego, v_ego)
+      else:
+        t = accel_t
+        max_d = calculate_distance(t, TARGET_JERK, a_ego, v_ego)
+
+        # calculate additional time needed based on target accel
+        t = abs((min_accel_v - tv) / TARGET_ACCEL)
+        max_d += calculate_distance(t, 0, TARGET_ACCEL, min_accel_v)
+
+      if d < max_d + tv * TARGET_OFFSET:
+        valid_velocities.append((float(tv), tlat, tlon))
+
+    # Find the smallest velocity we need to adjust for
+    min_v = 100.0
+    target_lat = 0.0
+    target_lon = 0.0
+    for tv, lat, lon in valid_velocities:
+      if tv < min_v:
+        min_v = tv
+        target_lat = lat
+        target_lon = lon
+
+    if self.target_v < min_v and not (self.target_lat == 0 and self.target_lon == 0):
+      for i in range(len(forward_points)):
+        target_velocity = forward_points[i]
+        tlat = target_velocity["latitude"]
+        tlon = target_velocity["longitude"]
+        tv = target_velocity["velocity"]
+        if tv > v_ego:
+          continue
+
+        if tlat == self.target_lat and tlon == self.target_lon and tv == self.target_v:
+          return float(self.target_v)
+      # not found so lets reset
+      self.target_v = 0.0
+      self.target_lat = 0.0
+      self.target_lon = 0.0
+
+    self.target_v = min_v
+    self.target_lat = target_lat
+    self.target_lon = target_lon
+
+    return min_v
+
+  def _state_transition(self):
+    if not self._op_enabled or not self.enabled:
      self.state = TurnSpeedControlState.inactive
      return

-    # In any case, we deactivate the speed limit controller temporarily
-    # if gas is pressed (to support gas override implementations).
-    if sm['carState'].gasPressed:
+    if self._gas_pressed:
      self.state = TurnSpeedControlState.tempInactive
      return

-    # inactive
+    # INACTIVE
    if self.state == TurnSpeedControlState.inactive:
-      # If the limit speed offset is negative (i.e. reduce speed) and lower than threshold and distanct to turn limit
-      # is positive (not in turn yet) we go to adapting state to reduce speed, otherwise we go directly to active
-      if self._v_offset < LIMIT_SPEED_OFFSET_TH and self.distance > 0.:
-        self.state = TurnSpeedControlState.adapting
-      else:
+      if self._v_cruise > self._min_v != 0:
        self.state = TurnSpeedControlState.active
-    # tempInactive
+
+    # TEMP INACTIVE
    elif self.state == TurnSpeedControlState.tempInactive:
-      # if the speed limit recorded when going to temp Inactive changes
-      # then set to inactive, activation will happen on next cycle
-      if self._speed_limit != self._speed_limit_temp_inactive:
-        self.state = TurnSpeedControlState.inactive
-    # adapting
-    elif self.state == TurnSpeedControlState.adapting:
-      # Go to active once the speed offset is over threshold or the distance to turn is now 0.
-      if self._v_offset >= LIMIT_SPEED_OFFSET_TH or self.distance == 0.:
+      if self._v_cruise > self._min_v != 0:
        self.state = TurnSpeedControlState.active
-    # active
+      else:
+        self.state = TurnSpeedControlState.inactive
+
+    # ACTIVE
    elif self.state == TurnSpeedControlState.active:
-      # Go to adapting if the speed offset goes below threshold as long as the distance to turn is still positive.
-      if self._v_offset < LIMIT_SPEED_OFFSET_TH and self.distance > 0.:
-        self.state = TurnSpeedControlState.adapting
+      if not (self._v_cruise > self._min_v != 0):
+        self.state = TurnSpeedControlState.inactive

-  def _update_solution(self):
-    # inactive or tempInactive state
-    if self.state <= TurnSpeedControlState.tempInactive:
-      # Preserve current values
-      a_target = self._a_ego
-    # adapting
-    elif self.state == TurnSpeedControlState.adapting:
-      # When adapting we target to achieve the speed limit on the distance.
-      a_target = (self.speed_limit**2 - self._v_ego**2) / (2. * self.distance)
-      a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC)
-    # active
-    elif self.state == TurnSpeedControlState.active:
-      # When active we are trying to keep the speed constant around the control time horizon.
-      # but under constrained acceleration limits since we are in a turn.
-      a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
-      a_target = np.clip(a_target, _ACTIVE_LIMIT_MIN_ACC, _ACTIVE_LIMIT_MAX_ACC)
+  def update(self, op_enabled, v_ego, sm, v_cruise):
+    self.update_params()
+    self._op_enabled = op_enabled
+    self._gas_pressed = sm['carState'].gasPressed
+    self._v_cruise = v_cruise
+    self._min_v = self.target_speed(v_ego, sm['carState'].aEgo)

-    # update solution values.
-    self._a_target = a_target
-
-  def update(self, enabled, v_ego, a_ego, sm):
-    self._op_enabled = enabled
-    self._v_ego = v_ego
-    self._a_ego = a_ego
-
-    # Get the speed limit from Map Data
-    self._speed_limit, self._distance, self._turn_sign = self._get_limit_from_map_data(sm)
-
-    self._update_params()
-    self._update_calculations()
-    self._state_transition(sm)
-    self._update_solution()
+    self._state_transition()
@@ -1,85 +1,26 @@
+# PFEIFER - VTSC
+
+# Acknowledgements:
+# Past versions of this code were based on the move-fast teams vtsc
+# implementation. (https://github.com/move-fast/openpilot) Huge thanks to them
+# for their initial implementation. I also used sunnypilot as a reference.
+# (https://github.com/sunnyhaibin/sunnypilot) Big thanks for sunny's amazing work
+
 import numpy as np
-import math
 import time
 from cereal import custom
-from common.numpy_fast import interp
-from common.params import Params
-from common.conversions import Conversions as CV
-from selfdrive.controls.lib.lateral_planner import TRAJECTORY_SIZE
-from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.sunnypilot.helpers import debug

+TARGET_LAT_A = 1.9  # m/s^2
+MIN_TARGET_V = 5  # m/s

-_MIN_V = 5.6  # Do not operate under 20km/h
-
-_ENTERING_PRED_LAT_ACC_TH = 1.3  # Predicted Lat Acc threshold to trigger entering turn state.
-_ABORT_ENTERING_PRED_LAT_ACC_TH = 1.1  # Predicted Lat Acc threshold to abort entering state if speed drops.
-
-_TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning turn state.
-
-_LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
-_FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger end of turn cycle.
-
-_EVAL_STEP = 5.  # mts. Resolution of the curvature evaluation.
-_EVAL_START = 20.  # mts. Distance ahead where to start evaluating vision curvature.
-_EVAL_LENGHT = 150.  # mts. Distance ahead where to stop evaluating vision curvature.
-_EVAL_RANGE = np.arange(_EVAL_START, _EVAL_LENGHT, _EVAL_STEP)
-
-_A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
-
-_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
-
-# Lookup table for the minimum smooth deceleration during the ENTERING state
-# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
-_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
-_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
-
-# Lookup table for the acceleration for the TURNING state
-# depending on the current lateral acceleration of the vehicle.
-_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
-_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
-
-_LEAVING_ACC = 0.5  # Confortble acceleration to regain speed while leaving a turn.
-
-_MIN_LANE_PROB = 0.6  # Minimum lanes probability to allow curvature prediction based on lanes.
-
-_DEBUG = False
-
-
-def _debug(msg):
-  if not _DEBUG:
-    return
-  print(msg)
-
+PARAMS_UPDATE_PERIOD = 5.

 VisionTurnControllerState = custom.LongitudinalPlanSP.VisionTurnControllerState


-def eval_curvature(poly, x_vals):
-  """
-  This function returns a vector with the curvature based on path defined by `poly`
-  evaluated on distance vector `x_vals`
-  """
-  # https://en.wikipedia.org/wiki/Curvature#  Local_expressions
-  def curvature(x):
-    a = abs(2 * poly[1] + 6 * poly[0] * x) / (1 + (3 * poly[0] * x**2 + 2 * poly[1] * x + poly[2])**2)**(1.5)
-    return a
-
-  return np.vectorize(curvature)(x_vals)
-
-
-def eval_lat_acc(v_ego, x_curv):
-  """
-  This function returns a vector with the lateral acceleration based
-  for the provided speed `v_ego` evaluated over curvature vector `x_curv`
-  """
-
-  def lat_acc(curv):
-    a = v_ego**2 * curv
-    return a
-
-  return np.vectorize(lat_acc)(x_curv)
-
-
 def _description_for_state(turn_controller_state):
  if turn_controller_state == VisionTurnControllerState.disabled:
    return 'DISABLED'
@@ -91,20 +32,19 @@ def _description_for_state(turn_controller_state):
    return 'LEAVING'


-class VisionTurnController():
+class VisionTurnController:
  def __init__(self, CP):
    self._params = Params()
    self._CP = CP
    self._op_enabled = False
    self._gas_pressed = False
    self._is_enabled = self._params.get_bool("TurnVisionControl")
-    self._disengage_on_accelerator = self._params.get_bool("DisengageOnAccelerator")
    self._last_params_update = 0.
-    self._v_cruise_setpoint = 0.
    self._v_ego = 0.
-    self._a_ego = 0.
-    self._a_target = 0.
-    self._v_overshoot = 0.
+    self._v_target = MIN_TARGET_V
+    self._current_lat_acc = 0.
+    self._max_pred_lat_acc = 0.
+    self._v_cruise = 0.
    self._state = VisionTurnControllerState.disabled

    self._reset()
@@ -116,21 +56,14 @@ class VisionTurnController():
  @state.setter
  def state(self, value):
    if value != self._state:
-      _debug(f'TVC: TurnVisionController state: {_description_for_state(value)}')
+      debug(f"V-TSC: VisionTurnControllerState state: {_description_for_state(value)}")
      if value == VisionTurnControllerState.disabled:
        self._reset()
    self._state = value

  @property
-  def a_target(self):
-    return self._a_target if self.is_active else self._a_ego
-
-  @property
-  def v_turn(self):
-    if not self.is_active:
-      return self._v_cruise_setpoint
-    return self._v_overshoot if self._lat_acc_overshoot_ahead \
-      else self._v_ego + self._a_target * _NO_OVERSHOOT_TIME_HORIZON
+  def v_target(self):
+    return self._v_target

  @property
  def is_active(self):
@@ -146,156 +79,54 @@ class VisionTurnController():

  def _reset(self):
    self._current_lat_acc = 0.
-    self._max_v_for_current_curvature = 0.
    self._max_pred_lat_acc = 0.
-    self._v_overshoot_distance = 200.
-    self._lat_acc_overshoot_ahead = False

  def _update_params(self):
    t = time.monotonic()
-    if t > self._last_params_update + 5.0:
+    if t > self._last_params_update + PARAMS_UPDATE_PERIOD:
      self._is_enabled = self._params.get_bool("TurnVisionControl")
      self._last_params_update = t

  def _update_calculations(self, sm):
-    # Get path polynomial approximation for curvature estimation from model data.
-    path_poly = None
-    model_data = sm['modelV2'] if sm.valid.get('modelV2', False) else None
-    lat_planner_data = sm['lateralPlanSP'] if sm.valid.get('lateralPlanSP', False) else None
-
-    # 1. When the probability of lanes is good enough, compute polynomial from lanes as they are way more stable
-    # on current mode than drving path.
-    if model_data is not None and len(model_data.laneLines) == 4 and len(model_data.laneLines[0].t) == TRAJECTORY_SIZE:
-      ll_x = model_data.laneLines[1].x  # left and right ll x is the same
-      lll_y = np.array(model_data.laneLines[1].y)
-      rll_y = np.array(model_data.laneLines[2].y)
-      l_prob = model_data.laneLineProbs[1]
-      r_prob = model_data.laneLineProbs[2]
-      lll_std = model_data.laneLineStds[1]
-      rll_std = model_data.laneLineStds[2]
-
-      # Reduce reliance on lanelines that are too far apart or will be in a few seconds
-      width_pts = rll_y - lll_y
-      prob_mods = []
-      for t_check in [0.0, 1.5, 3.0]:
-        width_at_t = interp(t_check * (self._v_ego + 7), ll_x, width_pts)
-        prob_mods.append(interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
-      mod = min(prob_mods)
-      l_prob *= mod
-      r_prob *= mod
-
-      # Reduce reliance on uncertain lanelines
-      l_std_mod = interp(lll_std, [.15, .3], [1.0, 0.0])
-      r_std_mod = interp(rll_std, [.15, .3], [1.0, 0.0])
-      l_prob *= l_std_mod
-      r_prob *= r_std_mod
-
-      # Find path from lanes as the average center lane only if min probability on both lanes is above threshold.
-      if l_prob > _MIN_LANE_PROB and r_prob > _MIN_LANE_PROB:
-        c_y = width_pts / 2 + lll_y
-        path_poly = np.polyfit(ll_x, c_y, 3)
-
-    # 2. If not polynomial derived from lanes, then derive it from compensated driving path with lanes as
-    # provided by `lateralPlanner`.
-    if path_poly is None and lat_planner_data is not None and len(lat_planner_data.dPathWLinesX) > 0 \
-       and lat_planner_data.dPathWLinesX[0] > 0:
-      path_poly = np.polyfit(lat_planner_data.dPathWLinesX, lat_planner_data.dPathWLinesY, 3)
-
-    # 3. If no polynomial derived from lanes or driving path, then provide a straight line poly.
-    if path_poly is None:
-      path_poly = np.array([0., 0., 0., 0.])
+    rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
+    vel_plan = np.array(sm['modelV2'].velocity.x)

    current_curvature = abs(
      sm['carState'].steeringAngleDeg * CV.DEG_TO_RAD / (self._CP.steerRatio * self._CP.wheelbase))
    self._current_lat_acc = current_curvature * self._v_ego**2
-    self._max_v_for_current_curvature = math.sqrt(_A_LAT_REG_MAX / current_curvature) if current_curvature > 0 \
-      else V_CRUISE_MAX * CV.KPH_TO_MS

-    pred_curvatures = eval_curvature(path_poly, _EVAL_RANGE)
-    max_pred_curvature = np.amax(pred_curvatures)
-    self._max_pred_lat_acc = self._v_ego**2 * max_pred_curvature
+    # get the maximum lat accel from the model
+    predicted_lat_accels = rate_plan * vel_plan
+    self._max_pred_lat_acc = np.amax(predicted_lat_accels)

-    max_curvature_for_vego = _A_LAT_REG_MAX / max(self._v_ego, 0.1)**2
-    lat_acc_overshoot_idxs = np.nonzero(pred_curvatures >= max_curvature_for_vego)[0]
-    self._lat_acc_overshoot_ahead = len(lat_acc_overshoot_idxs) > 0
+    # get the maximum curve based on the current velocity
+    v_ego = max(self._v_ego, 0.1)  # ensure a value greater than 0 for calculations
+    max_curve = self.max_pred_lat_acc / (v_ego**2)

-    if self._lat_acc_overshoot_ahead:
-      self._v_overshoot = min(math.sqrt(_A_LAT_REG_MAX / max_pred_curvature), self._v_cruise_setpoint)
-      self._v_overshoot_distance = max(lat_acc_overshoot_idxs[0] * _EVAL_STEP + _EVAL_START, _EVAL_STEP)
-      _debug(f'TVC: High LatAcc. Dist: {self._v_overshoot_distance:.2f}, v: {self._v_overshoot * CV.MS_TO_KPH:.2f}')
+    # Get the target velocity for the maximum curve
+    self._v_target = (TARGET_LAT_A / max_curve) ** 0.5
+    self._v_target = max(self.v_target, MIN_TARGET_V)

  def _state_transition(self):
-    # In any case, if system is disabled or the feature is disabeld or gas is pressed, disable.
-    if not self._op_enabled or not self._is_enabled or (self._gas_pressed and self._disengage_on_accelerator):
+    if not self._op_enabled or not self._is_enabled or self._gas_pressed or self._v_ego < MIN_TARGET_V:
      self.state = VisionTurnControllerState.disabled
      return

    # DISABLED
    if self.state == VisionTurnControllerState.disabled:
-      # Do not enter a turn control cycle if speed is low.
-      if self._v_ego <= _MIN_V:
-        pass
-      # If substantial lateral acceleration is predicted ahead, then move to Entering turn state.
-      elif self._max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.entering
-    # ENTERING
-    elif self.state == VisionTurnControllerState.entering:
-      # Transition to Turning if current lateral acceleration is over the threshold.
-      if self._current_lat_acc >= _TURNING_LAT_ACC_TH:
+      if self._v_cruise > self.v_target:
        self.state = VisionTurnControllerState.turning
-      # Abort if the predicted lateral acceleration drops
-      elif self._max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.disabled
    # TURNING
    elif self.state == VisionTurnControllerState.turning:
-      # Transition to Leaving if current lateral acceleration drops drops below threshold.
-      if self._current_lat_acc <= _LEAVING_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.leaving
-    # LEAVING
-    elif self.state == VisionTurnControllerState.leaving:
-      # Transition back to Turning if current lateral acceleration goes back over the threshold.
-      if self._current_lat_acc >= _TURNING_LAT_ACC_TH:
-        self.state = VisionTurnControllerState.turning
-      # Finish if current lateral acceleration goes below threshold.
-      elif self._current_lat_acc < _FINISH_LAT_ACC_TH:
+      if not (self._v_cruise > self.v_target):
        self.state = VisionTurnControllerState.disabled

-  def _update_solution(self):
-    # DISABLED
-    if self.state == VisionTurnControllerState.disabled:
-      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
-        # the smooth deceleration.
-      a_target = self._a_ego
-    # ENTERING
-    elif self.state == VisionTurnControllerState.entering:
-      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
-      a_target = interp(self._max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
-      if self._lat_acc_overshoot_ahead:
-        # when overshooting, target the acceleration needed to achieve the overshoot speed at
-        # the required distance
-        a_target = min((self._v_overshoot**2 - self._v_ego**2) / (2 * self._v_overshoot_distance), a_target)
-      _debug(f'TVC Entering: Overshooting: {self._lat_acc_overshoot_ahead}')
-      _debug(f'    Decel: {a_target:.2f}, target v: {self.v_turn * CV.MS_TO_KPH}')
-    # TURNING
-    elif self.state == VisionTurnControllerState.turning:
-      # When turning we provide a target acceleration that is comfortable for the lateral accelearation felt.
-      a_target = interp(self._current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
-    # LEAVING
-    elif self.state == VisionTurnControllerState.leaving:
-      # When leaving we provide a comfortable acceleration to regain speed.
-      a_target = _LEAVING_ACC
-
-    # update solution values.
-    self._a_target = a_target
-
-  def update(self, enabled, v_ego, a_ego, v_cruise_setpoint, sm):
+  def update(self, enabled, v_ego, v_cruise, sm):
    self._op_enabled = enabled
    self._gas_pressed = sm['carState'].gasPressed
    self._v_ego = v_ego
-    self._a_ego = a_ego
-    self._v_cruise_setpoint = v_cruise_setpoint
+    self._v_cruise = v_cruise

    self._update_params()
    self._update_calculations(sm)
    self._state_transition()
-    self._update_solution()
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
 *                                                                            *
 *                         This file is part of 'ekf'                         *
 ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_7470397526871273149) {
-   out_7470397526871273149[0] = delta_x[0] + nom_x[0];
-   out_7470397526871273149[1] = delta_x[1] + nom_x[1];
-   out_7470397526871273149[2] = delta_x[2] + nom_x[2];
-   out_7470397526871273149[3] = delta_x[3] + nom_x[3];
-   out_7470397526871273149[4] = delta_x[4] + nom_x[4];
-   out_7470397526871273149[5] = delta_x[5] + nom_x[5];
-   out_7470397526871273149[6] = delta_x[6] + nom_x[6];
-   out_7470397526871273149[7] = delta_x[7] + nom_x[7];
-   out_7470397526871273149[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_1027636603068461502) {
+   out_1027636603068461502[0] = delta_x[0] + nom_x[0];
+   out_1027636603068461502[1] = delta_x[1] + nom_x[1];
+   out_1027636603068461502[2] = delta_x[2] + nom_x[2];
+   out_1027636603068461502[3] = delta_x[3] + nom_x[3];
+   out_1027636603068461502[4] = delta_x[4] + nom_x[4];
+   out_1027636603068461502[5] = delta_x[5] + nom_x[5];
+   out_1027636603068461502[6] = delta_x[6] + nom_x[6];
+   out_1027636603068461502[7] = delta_x[7] + nom_x[7];
+   out_1027636603068461502[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_6628002157851279603) {
-   out_6628002157851279603[0] = -nom_x[0] + true_x[0];
-   out_6628002157851279603[1] = -nom_x[1] + true_x[1];
-   out_6628002157851279603[2] = -nom_x[2] + true_x[2];
-   out_6628002157851279603[3] = -nom_x[3] + true_x[3];
-   out_6628002157851279603[4] = -nom_x[4] + true_x[4];
-   out_6628002157851279603[5] = -nom_x[5] + true_x[5];
-   out_6628002157851279603[6] = -nom_x[6] + true_x[6];
-   out_6628002157851279603[7] = -nom_x[7] + true_x[7];
-   out_6628002157851279603[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_2498107869525935257) {
+   out_2498107869525935257[0] = -nom_x[0] + true_x[0];
+   out_2498107869525935257[1] = -nom_x[1] + true_x[1];
+   out_2498107869525935257[2] = -nom_x[2] + true_x[2];
+   out_2498107869525935257[3] = -nom_x[3] + true_x[3];
+   out_2498107869525935257[4] = -nom_x[4] + true_x[4];
+   out_2498107869525935257[5] = -nom_x[5] + true_x[5];
+   out_2498107869525935257[6] = -nom_x[6] + true_x[6];
+   out_2498107869525935257[7] = -nom_x[7] + true_x[7];
+   out_2498107869525935257[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_338615810674868519) {
-   out_338615810674868519[0] = 1.0;
-   out_338615810674868519[1] = 0;
-   out_338615810674868519[2] = 0;
-   out_338615810674868519[3] = 0;
-   out_338615810674868519[4] = 0;
-   out_338615810674868519[5] = 0;
-   out_338615810674868519[6] = 0;
-   out_338615810674868519[7] = 0;
-   out_338615810674868519[8] = 0;
-   out_338615810674868519[9] = 0;
-   out_338615810674868519[10] = 1.0;
-   out_338615810674868519[11] = 0;
-   out_338615810674868519[12] = 0;
-   out_338615810674868519[13] = 0;
-   out_338615810674868519[14] = 0;
-   out_338615810674868519[15] = 0;
-   out_338615810674868519[16] = 0;
-   out_338615810674868519[17] = 0;
-   out_338615810674868519[18] = 0;
-   out_338615810674868519[19] = 0;
-   out_338615810674868519[20] = 1.0;
-   out_338615810674868519[21] = 0;
-   out_338615810674868519[22] = 0;
-   out_338615810674868519[23] = 0;
-   out_338615810674868519[24] = 0;
-   out_338615810674868519[25] = 0;
-   out_338615810674868519[26] = 0;
-   out_338615810674868519[27] = 0;
-   out_338615810674868519[28] = 0;
-   out_338615810674868519[29] = 0;
-   out_338615810674868519[30] = 1.0;
-   out_338615810674868519[31] = 0;
-   out_338615810674868519[32] = 0;
-   out_338615810674868519[33] = 0;
-   out_338615810674868519[34] = 0;
-   out_338615810674868519[35] = 0;
-   out_338615810674868519[36] = 0;
-   out_338615810674868519[37] = 0;
-   out_338615810674868519[38] = 0;
-   out_338615810674868519[39] = 0;
-   out_338615810674868519[40] = 1.0;
-   out_338615810674868519[41] = 0;
-   out_338615810674868519[42] = 0;
-   out_338615810674868519[43] = 0;
-   out_338615810674868519[44] = 0;
-   out_338615810674868519[45] = 0;
-   out_338615810674868519[46] = 0;
-   out_338615810674868519[47] = 0;
-   out_338615810674868519[48] = 0;
-   out_338615810674868519[49] = 0;
-   out_338615810674868519[50] = 1.0;
-   out_338615810674868519[51] = 0;
-   out_338615810674868519[52] = 0;
-   out_338615810674868519[53] = 0;
-   out_338615810674868519[54] = 0;
-   out_338615810674868519[55] = 0;
-   out_338615810674868519[56] = 0;
-   out_338615810674868519[57] = 0;
-   out_338615810674868519[58] = 0;
-   out_338615810674868519[59] = 0;
-   out_338615810674868519[60] = 1.0;
-   out_338615810674868519[61] = 0;
-   out_338615810674868519[62] = 0;
-   out_338615810674868519[63] = 0;
-   out_338615810674868519[64] = 0;
-   out_338615810674868519[65] = 0;
-   out_338615810674868519[66] = 0;
-   out_338615810674868519[67] = 0;
-   out_338615810674868519[68] = 0;
-   out_338615810674868519[69] = 0;
-   out_338615810674868519[70] = 1.0;
-   out_338615810674868519[71] = 0;
-   out_338615810674868519[72] = 0;
-   out_338615810674868519[73] = 0;
-   out_338615810674868519[74] = 0;
-   out_338615810674868519[75] = 0;
-   out_338615810674868519[76] = 0;
-   out_338615810674868519[77] = 0;
-   out_338615810674868519[78] = 0;
-   out_338615810674868519[79] = 0;
-   out_338615810674868519[80] = 1.0;
+void H_mod_fun(double *state, double *out_2880487860269583385) {
+   out_2880487860269583385[0] = 1.0;
+   out_2880487860269583385[1] = 0;
+   out_2880487860269583385[2] = 0;
+   out_2880487860269583385[3] = 0;
+   out_2880487860269583385[4] = 0;
+   out_2880487860269583385[5] = 0;
+   out_2880487860269583385[6] = 0;
+   out_2880487860269583385[7] = 0;
+   out_2880487860269583385[8] = 0;
+   out_2880487860269583385[9] = 0;
+   out_2880487860269583385[10] = 1.0;
+   out_2880487860269583385[11] = 0;
+   out_2880487860269583385[12] = 0;
+   out_2880487860269583385[13] = 0;
+   out_2880487860269583385[14] = 0;
+   out_2880487860269583385[15] = 0;
+   out_2880487860269583385[16] = 0;
+   out_2880487860269583385[17] = 0;
+   out_2880487860269583385[18] = 0;
+   out_2880487860269583385[19] = 0;
+   out_2880487860269583385[20] = 1.0;
+   out_2880487860269583385[21] = 0;
+   out_2880487860269583385[22] = 0;
+   out_2880487860269583385[23] = 0;
+   out_2880487860269583385[24] = 0;
+   out_2880487860269583385[25] = 0;
+   out_2880487860269583385[26] = 0;
+   out_2880487860269583385[27] = 0;
+   out_2880487860269583385[28] = 0;
+   out_2880487860269583385[29] = 0;
+   out_2880487860269583385[30] = 1.0;
+   out_2880487860269583385[31] = 0;
+   out_2880487860269583385[32] = 0;
+   out_2880487860269583385[33] = 0;
+   out_2880487860269583385[34] = 0;
+   out_2880487860269583385[35] = 0;
+   out_2880487860269583385[36] = 0;
+   out_2880487860269583385[37] = 0;
+   out_2880487860269583385[38] = 0;
+   out_2880487860269583385[39] = 0;
+   out_2880487860269583385[40] = 1.0;
+   out_2880487860269583385[41] = 0;
+   out_2880487860269583385[42] = 0;
+   out_2880487860269583385[43] = 0;
+   out_2880487860269583385[44] = 0;
+   out_2880487860269583385[45] = 0;
+   out_2880487860269583385[46] = 0;
+   out_2880487860269583385[47] = 0;
+   out_2880487860269583385[48] = 0;
+   out_2880487860269583385[49] = 0;
+   out_2880487860269583385[50] = 1.0;
+   out_2880487860269583385[51] = 0;
+   out_2880487860269583385[52] = 0;
+   out_2880487860269583385[53] = 0;
+   out_2880487860269583385[54] = 0;
+   out_2880487860269583385[55] = 0;
+   out_2880487860269583385[56] = 0;
+   out_2880487860269583385[57] = 0;
+   out_2880487860269583385[58] = 0;
+   out_2880487860269583385[59] = 0;
+   out_2880487860269583385[60] = 1.0;
+   out_2880487860269583385[61] = 0;
+   out_2880487860269583385[62] = 0;
+   out_2880487860269583385[63] = 0;
+   out_2880487860269583385[64] = 0;
+   out_2880487860269583385[65] = 0;
+   out_2880487860269583385[66] = 0;
+   out_2880487860269583385[67] = 0;
+   out_2880487860269583385[68] = 0;
+   out_2880487860269583385[69] = 0;
+   out_2880487860269583385[70] = 1.0;
+   out_2880487860269583385[71] = 0;
+   out_2880487860269583385[72] = 0;
+   out_2880487860269583385[73] = 0;
+   out_2880487860269583385[74] = 0;
+   out_2880487860269583385[75] = 0;
+   out_2880487860269583385[76] = 0;
+   out_2880487860269583385[77] = 0;
+   out_2880487860269583385[78] = 0;
+   out_2880487860269583385[79] = 0;
+   out_2880487860269583385[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_3565074677649486182) {
-   out_3565074677649486182[0] = state[0];
-   out_3565074677649486182[1] = state[1];
-   out_3565074677649486182[2] = state[2];
-   out_3565074677649486182[3] = state[3];
-   out_3565074677649486182[4] = state[4];
-   out_3565074677649486182[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_3565074677649486182[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_3565074677649486182[7] = state[7];
-   out_3565074677649486182[8] = state[8];
+void f_fun(double *state, double dt, double *out_8985245024092687483) {
+   out_8985245024092687483[0] = state[0];
+   out_8985245024092687483[1] = state[1];
+   out_8985245024092687483[2] = state[2];
+   out_8985245024092687483[3] = state[3];
+   out_8985245024092687483[4] = state[4];
+   out_8985245024092687483[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_8985245024092687483[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_8985245024092687483[7] = state[7];
+   out_8985245024092687483[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_1768894250710066862) {
-   out_1768894250710066862[0] = 1;
-   out_1768894250710066862[1] = 0;
-   out_1768894250710066862[2] = 0;
-   out_1768894250710066862[3] = 0;
-   out_1768894250710066862[4] = 0;
-   out_1768894250710066862[5] = 0;
-   out_1768894250710066862[6] = 0;
-   out_1768894250710066862[7] = 0;
-   out_1768894250710066862[8] = 0;
-   out_1768894250710066862[9] = 0;
-   out_1768894250710066862[10] = 1;
-   out_1768894250710066862[11] = 0;
-   out_1768894250710066862[12] = 0;
-   out_1768894250710066862[13] = 0;
-   out_1768894250710066862[14] = 0;
-   out_1768894250710066862[15] = 0;
-   out_1768894250710066862[16] = 0;
-   out_1768894250710066862[17] = 0;
-   out_1768894250710066862[18] = 0;
-   out_1768894250710066862[19] = 0;
-   out_1768894250710066862[20] = 1;
-   out_1768894250710066862[21] = 0;
-   out_1768894250710066862[22] = 0;
-   out_1768894250710066862[23] = 0;
-   out_1768894250710066862[24] = 0;
-   out_1768894250710066862[25] = 0;
-   out_1768894250710066862[26] = 0;
-   out_1768894250710066862[27] = 0;
-   out_1768894250710066862[28] = 0;
-   out_1768894250710066862[29] = 0;
-   out_1768894250710066862[30] = 1;
-   out_1768894250710066862[31] = 0;
-   out_1768894250710066862[32] = 0;
-   out_1768894250710066862[33] = 0;
-   out_1768894250710066862[34] = 0;
-   out_1768894250710066862[35] = 0;
-   out_1768894250710066862[36] = 0;
-   out_1768894250710066862[37] = 0;
-   out_1768894250710066862[38] = 0;
-   out_1768894250710066862[39] = 0;
-   out_1768894250710066862[40] = 1;
-   out_1768894250710066862[41] = 0;
-   out_1768894250710066862[42] = 0;
-   out_1768894250710066862[43] = 0;
-   out_1768894250710066862[44] = 0;
-   out_1768894250710066862[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_1768894250710066862[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_1768894250710066862[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1768894250710066862[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1768894250710066862[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_1768894250710066862[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_1768894250710066862[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_1768894250710066862[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_1768894250710066862[53] = -9.8000000000000007*dt;
-   out_1768894250710066862[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_1768894250710066862[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_1768894250710066862[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1768894250710066862[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1768894250710066862[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_1768894250710066862[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_1768894250710066862[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_1768894250710066862[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_1768894250710066862[62] = 0;
-   out_1768894250710066862[63] = 0;
-   out_1768894250710066862[64] = 0;
-   out_1768894250710066862[65] = 0;
-   out_1768894250710066862[66] = 0;
-   out_1768894250710066862[67] = 0;
-   out_1768894250710066862[68] = 0;
-   out_1768894250710066862[69] = 0;
-   out_1768894250710066862[70] = 1;
-   out_1768894250710066862[71] = 0;
-   out_1768894250710066862[72] = 0;
-   out_1768894250710066862[73] = 0;
-   out_1768894250710066862[74] = 0;
-   out_1768894250710066862[75] = 0;
-   out_1768894250710066862[76] = 0;
-   out_1768894250710066862[77] = 0;
-   out_1768894250710066862[78] = 0;
-   out_1768894250710066862[79] = 0;
-   out_1768894250710066862[80] = 1;
+void F_fun(double *state, double dt, double *out_135664326016666632) {
+   out_135664326016666632[0] = 1;
+   out_135664326016666632[1] = 0;
+   out_135664326016666632[2] = 0;
+   out_135664326016666632[3] = 0;
+   out_135664326016666632[4] = 0;
+   out_135664326016666632[5] = 0;
+   out_135664326016666632[6] = 0;
+   out_135664326016666632[7] = 0;
+   out_135664326016666632[8] = 0;
+   out_135664326016666632[9] = 0;
+   out_135664326016666632[10] = 1;
+   out_135664326016666632[11] = 0;
+   out_135664326016666632[12] = 0;
+   out_135664326016666632[13] = 0;
+   out_135664326016666632[14] = 0;
+   out_135664326016666632[15] = 0;
+   out_135664326016666632[16] = 0;
+   out_135664326016666632[17] = 0;
+   out_135664326016666632[18] = 0;
+   out_135664326016666632[19] = 0;
+   out_135664326016666632[20] = 1;
+   out_135664326016666632[21] = 0;
+   out_135664326016666632[22] = 0;
+   out_135664326016666632[23] = 0;
+   out_135664326016666632[24] = 0;
+   out_135664326016666632[25] = 0;
+   out_135664326016666632[26] = 0;
+   out_135664326016666632[27] = 0;
+   out_135664326016666632[28] = 0;
+   out_135664326016666632[29] = 0;
+   out_135664326016666632[30] = 1;
+   out_135664326016666632[31] = 0;
+   out_135664326016666632[32] = 0;
+   out_135664326016666632[33] = 0;
+   out_135664326016666632[34] = 0;
+   out_135664326016666632[35] = 0;
+   out_135664326016666632[36] = 0;
+   out_135664326016666632[37] = 0;
+   out_135664326016666632[38] = 0;
+   out_135664326016666632[39] = 0;
+   out_135664326016666632[40] = 1;
+   out_135664326016666632[41] = 0;
+   out_135664326016666632[42] = 0;
+   out_135664326016666632[43] = 0;
+   out_135664326016666632[44] = 0;
+   out_135664326016666632[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_135664326016666632[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_135664326016666632[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_135664326016666632[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_135664326016666632[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_135664326016666632[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_135664326016666632[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_135664326016666632[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_135664326016666632[53] = -9.8000000000000007*dt;
+   out_135664326016666632[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_135664326016666632[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_135664326016666632[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_135664326016666632[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_135664326016666632[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_135664326016666632[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_135664326016666632[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_135664326016666632[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_135664326016666632[62] = 0;
+   out_135664326016666632[63] = 0;
+   out_135664326016666632[64] = 0;
+   out_135664326016666632[65] = 0;
+   out_135664326016666632[66] = 0;
+   out_135664326016666632[67] = 0;
+   out_135664326016666632[68] = 0;
+   out_135664326016666632[69] = 0;
+   out_135664326016666632[70] = 1;
+   out_135664326016666632[71] = 0;
+   out_135664326016666632[72] = 0;
+   out_135664326016666632[73] = 0;
+   out_135664326016666632[74] = 0;
+   out_135664326016666632[75] = 0;
+   out_135664326016666632[76] = 0;
+   out_135664326016666632[77] = 0;
+   out_135664326016666632[78] = 0;
+   out_135664326016666632[79] = 0;
+   out_135664326016666632[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_5309084791442776367) {
-   out_5309084791442776367[0] = state[6];
+void h_25(double *state, double *unused, double *out_1339936627130384923) {
+   out_1339936627130384923[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_2366865651751345480) {
-   out_2366865651751345480[0] = 0;
-   out_2366865651751345480[1] = 0;
-   out_2366865651751345480[2] = 0;
-   out_2366865651751345480[3] = 0;
-   out_2366865651751345480[4] = 0;
-   out_2366865651751345480[5] = 0;
-   out_2366865651751345480[6] = 1;
-   out_2366865651751345480[7] = 0;
-   out_2366865651751345480[8] = 0;
+void H_25(double *state, double *unused, double *out_8541205388556229655) {
+   out_8541205388556229655[0] = 0;
+   out_8541205388556229655[1] = 0;
+   out_8541205388556229655[2] = 0;
+   out_8541205388556229655[3] = 0;
+   out_8541205388556229655[4] = 0;
+   out_8541205388556229655[5] = 0;
+   out_8541205388556229655[6] = 1;
+   out_8541205388556229655[7] = 0;
+   out_8541205388556229655[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_4964929704142686901) {
-   out_4964929704142686901[0] = state[4];
-   out_4964929704142686901[1] = state[5];
+void h_24(double *state, double *unused, double *out_7843416543337357224) {
+   out_7843416543337357224[0] = state[4];
+   out_7843416543337357224[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_6861199534317849745) {
-   out_6861199534317849745[0] = 0;
-   out_6861199534317849745[1] = 0;
-   out_6861199534317849745[2] = 0;
-   out_6861199534317849745[3] = 0;
-   out_6861199534317849745[4] = 1;
-   out_6861199534317849745[5] = 0;
-   out_6861199534317849745[6] = 0;
-   out_6861199534317849745[7] = 0;
-   out_6861199534317849745[8] = 0;
-   out_6861199534317849745[9] = 0;
-   out_6861199534317849745[10] = 0;
-   out_6861199534317849745[11] = 0;
-   out_6861199534317849745[12] = 0;
-   out_6861199534317849745[13] = 0;
-   out_6861199534317849745[14] = 1;
-   out_6861199534317849745[15] = 0;
-   out_6861199534317849745[16] = 0;
-   out_6861199534317849745[17] = 0;
+void H_24(double *state, double *unused, double *out_4983279789753906519) {
+   out_4983279789753906519[0] = 0;
+   out_4983279789753906519[1] = 0;
+   out_4983279789753906519[2] = 0;
+   out_4983279789753906519[3] = 0;
+   out_4983279789753906519[4] = 1;
+   out_4983279789753906519[5] = 0;
+   out_4983279789753906519[6] = 0;
+   out_4983279789753906519[7] = 0;
+   out_4983279789753906519[8] = 0;
+   out_4983279789753906519[9] = 0;
+   out_4983279789753906519[10] = 0;
+   out_4983279789753906519[11] = 0;
+   out_4983279789753906519[12] = 0;
+   out_4983279789753906519[13] = 0;
+   out_4983279789753906519[14] = 1;
+   out_4983279789753906519[15] = 0;
+   out_4983279789753906519[16] = 0;
+   out_4983279789753906519[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_6759716184071659534) {
-   out_6759716184071659534[0] = state[4];
+void h_30(double *state, double *unused, double *out_7253669653801133296) {
+   out_7253669653801133296[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_6894561981878953678) {
-   out_6894561981878953678[0] = 0;
-   out_6894561981878953678[1] = 0;
-   out_6894561981878953678[2] = 0;
-   out_6894561981878953678[3] = 0;
-   out_6894561981878953678[4] = 1;
-   out_6894561981878953678[5] = 0;
-   out_6894561981878953678[6] = 0;
-   out_6894561981878953678[7] = 0;
-   out_6894561981878953678[8] = 0;
+void H_30(double *state, double *unused, double *out_6022872430048981028) {
+   out_6022872430048981028[0] = 0;
+   out_6022872430048981028[1] = 0;
+   out_6022872430048981028[2] = 0;
+   out_6022872430048981028[3] = 0;
+   out_6022872430048981028[4] = 1;
+   out_6022872430048981028[5] = 0;
+   out_6022872430048981028[6] = 0;
+   out_6022872430048981028[7] = 0;
+   out_6022872430048981028[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_6461120673802567135) {
-   out_6461120673802567135[0] = state[7];
+void h_26(double *state, double *unused, double *out_7856985538477610813) {
+   out_7856985538477610813[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_6108368970625401704) {
-   out_6108368970625401704[0] = 0;
-   out_6108368970625401704[1] = 0;
-   out_6108368970625401704[2] = 0;
-   out_6108368970625401704[3] = 0;
-   out_6108368970625401704[4] = 0;
-   out_6108368970625401704[5] = 0;
-   out_6108368970625401704[6] = 0;
-   out_6108368970625401704[7] = 1;
-   out_6108368970625401704[8] = 0;
+void H_26(double *state, double *unused, double *out_5236679418795429054) {
+   out_5236679418795429054[0] = 0;
+   out_5236679418795429054[1] = 0;
+   out_5236679418795429054[2] = 0;
+   out_5236679418795429054[3] = 0;
+   out_5236679418795429054[4] = 0;
+   out_5236679418795429054[5] = 0;
+   out_5236679418795429054[6] = 0;
+   out_5236679418795429054[7] = 1;
+   out_5236679418795429054[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_1903562740631468860) {
-   out_1903562740631468860[0] = state[3];
+void h_27(double *state, double *unused, double *out_3661736640530460722) {
+   out_3661736640530460722[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_4670967910695010461) {
-   out_4670967910695010461[0] = 0;
-   out_4670967910695010461[1] = 0;
-   out_4670967910695010461[2] = 0;
-   out_4670967910695010461[3] = 1;
-   out_4670967910695010461[4] = 0;
-   out_4670967910695010461[5] = 0;
-   out_4670967910695010461[6] = 0;
-   out_4670967910695010461[7] = 0;
-   out_4670967910695010461[8] = 0;
+void H_27(double *state, double *unused, double *out_3799278358865037811) {
+   out_3799278358865037811[0] = 0;
+   out_3799278358865037811[1] = 0;
+   out_3799278358865037811[2] = 0;
+   out_3799278358865037811[3] = 1;
+   out_3799278358865037811[4] = 0;
+   out_3799278358865037811[5] = 0;
+   out_3799278358865037811[6] = 0;
+   out_3799278358865037811[7] = 0;
+   out_3799278358865037811[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_1758042916410813889) {
-   out_1758042916410813889[0] = state[1];
+void h_29(double *state, double *unused, double *out_2211105247901577555) {
+   out_2211105247901577555[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_6384330637564561494) {
-   out_6384330637564561494[0] = 0;
-   out_6384330637564561494[1] = 1;
-   out_6384330637564561494[2] = 0;
-   out_6384330637564561494[3] = 0;
-   out_6384330637564561494[4] = 0;
-   out_6384330637564561494[5] = 0;
-   out_6384330637564561494[6] = 0;
-   out_6384330637564561494[7] = 0;
-   out_6384330637564561494[8] = 0;
+void H_29(double *state, double *unused, double *out_5512641085734588844) {
+   out_5512641085734588844[0] = 0;
+   out_5512641085734588844[1] = 1;
+   out_5512641085734588844[2] = 0;
+   out_5512641085734588844[3] = 0;
+   out_5512641085734588844[4] = 0;
+   out_5512641085734588844[5] = 0;
+   out_5512641085734588844[6] = 0;
+   out_5512641085734588844[7] = 0;
+   out_5512641085734588844[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_2847072593533131295) {
-   out_2847072593533131295[0] = state[0];
+void h_28(double *state, double *unused, double *out_8663409908191306367) {
+   out_8663409908191306367[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_6980014419075459548) {
-   out_6980014419075459548[0] = 1;
-   out_6980014419075459548[1] = 0;
-   out_6980014419075459548[2] = 0;
-   out_6980014419075459548[3] = 0;
-   out_6980014419075459548[4] = 0;
-   out_6980014419075459548[5] = 0;
-   out_6980014419075459548[6] = 0;
-   out_6980014419075459548[7] = 0;
-   out_6980014419075459548[8] = 0;
+void H_28(double *state, double *unused, double *out_7851703970905432198) {
+   out_7851703970905432198[0] = 1;
+   out_7851703970905432198[1] = 0;
+   out_7851703970905432198[2] = 0;
+   out_7851703970905432198[3] = 0;
+   out_7851703970905432198[4] = 0;
+   out_7851703970905432198[5] = 0;
+   out_7851703970905432198[6] = 0;
+   out_7851703970905432198[7] = 0;
+   out_7851703970905432198[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_858029248528357936) {
-   out_858029248528357936[0] = state[8];
+void h_31(double *state, double *unused, double *out_5201620103558886575) {
+   out_5201620103558886575[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_6734577072858753180) {
-   out_6734577072858753180[0] = 0;
-   out_6734577072858753180[1] = 0;
-   out_6734577072858753180[2] = 0;
-   out_6734577072858753180[3] = 0;
-   out_6734577072858753180[4] = 0;
-   out_6734577072858753180[5] = 0;
-   out_6734577072858753180[6] = 0;
-   out_6734577072858753180[7] = 0;
-   out_6734577072858753180[8] = 1;
+void H_31(double *state, double *unused, double *out_1464530138044412402) {
+   out_1464530138044412402[0] = 0;
+   out_1464530138044412402[1] = 0;
+   out_1464530138044412402[2] = 0;
+   out_1464530138044412402[3] = 0;
+   out_1464530138044412402[4] = 0;
+   out_1464530138044412402[5] = 0;
+   out_1464530138044412402[6] = 0;
+   out_1464530138044412402[7] = 0;
+   out_1464530138044412402[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
  update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_7470397526871273149) {
-  err_fun(nom_x, delta_x, out_7470397526871273149);
+void car_err_fun(double *nom_x, double *delta_x, double *out_1027636603068461502) {
+  err_fun(nom_x, delta_x, out_1027636603068461502);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6628002157851279603) {
-  inv_err_fun(nom_x, true_x, out_6628002157851279603);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_2498107869525935257) {
+  inv_err_fun(nom_x, true_x, out_2498107869525935257);
 }
-void car_H_mod_fun(double *state, double *out_338615810674868519) {
-  H_mod_fun(state, out_338615810674868519);
+void car_H_mod_fun(double *state, double *out_2880487860269583385) {
+  H_mod_fun(state, out_2880487860269583385);
 }
-void car_f_fun(double *state, double dt, double *out_3565074677649486182) {
-  f_fun(state,  dt, out_3565074677649486182);
+void car_f_fun(double *state, double dt, double *out_8985245024092687483) {
+  f_fun(state,  dt, out_8985245024092687483);
 }
-void car_F_fun(double *state, double dt, double *out_1768894250710066862) {
-  F_fun(state,  dt, out_1768894250710066862);
+void car_F_fun(double *state, double dt, double *out_135664326016666632) {
+  F_fun(state,  dt, out_135664326016666632);
 }
-void car_h_25(double *state, double *unused, double *out_5309084791442776367) {
-  h_25(state, unused, out_5309084791442776367);
+void car_h_25(double *state, double *unused, double *out_1339936627130384923) {
+  h_25(state, unused, out_1339936627130384923);
 }
-void car_H_25(double *state, double *unused, double *out_2366865651751345480) {
-  H_25(state, unused, out_2366865651751345480);
+void car_H_25(double *state, double *unused, double *out_8541205388556229655) {
+  H_25(state, unused, out_8541205388556229655);
 }
-void car_h_24(double *state, double *unused, double *out_4964929704142686901) {
-  h_24(state, unused, out_4964929704142686901);
+void car_h_24(double *state, double *unused, double *out_7843416543337357224) {
+  h_24(state, unused, out_7843416543337357224);
 }
-void car_H_24(double *state, double *unused, double *out_6861199534317849745) {
-  H_24(state, unused, out_6861199534317849745);
+void car_H_24(double *state, double *unused, double *out_4983279789753906519) {
+  H_24(state, unused, out_4983279789753906519);
 }
-void car_h_30(double *state, double *unused, double *out_6759716184071659534) {
-  h_30(state, unused, out_6759716184071659534);
+void car_h_30(double *state, double *unused, double *out_7253669653801133296) {
+  h_30(state, unused, out_7253669653801133296);
 }
-void car_H_30(double *state, double *unused, double *out_6894561981878953678) {
-  H_30(state, unused, out_6894561981878953678);
+void car_H_30(double *state, double *unused, double *out_6022872430048981028) {
+  H_30(state, unused, out_6022872430048981028);
 }
-void car_h_26(double *state, double *unused, double *out_6461120673802567135) {
-  h_26(state, unused, out_6461120673802567135);
+void car_h_26(double *state, double *unused, double *out_7856985538477610813) {
+  h_26(state, unused, out_7856985538477610813);
 }
-void car_H_26(double *state, double *unused, double *out_6108368970625401704) {
-  H_26(state, unused, out_6108368970625401704);
+void car_H_26(double *state, double *unused, double *out_5236679418795429054) {
+  H_26(state, unused, out_5236679418795429054);
 }
-void car_h_27(double *state, double *unused, double *out_1903562740631468860) {
-  h_27(state, unused, out_1903562740631468860);
+void car_h_27(double *state, double *unused, double *out_3661736640530460722) {
+  h_27(state, unused, out_3661736640530460722);
 }
-void car_H_27(double *state, double *unused, double *out_4670967910695010461) {
-  H_27(state, unused, out_4670967910695010461);
+void car_H_27(double *state, double *unused, double *out_3799278358865037811) {
+  H_27(state, unused, out_3799278358865037811);
 }
-void car_h_29(double *state, double *unused, double *out_1758042916410813889) {
-  h_29(state, unused, out_1758042916410813889);
+void car_h_29(double *state, double *unused, double *out_2211105247901577555) {
+  h_29(state, unused, out_2211105247901577555);
 }
-void car_H_29(double *state, double *unused, double *out_6384330637564561494) {
-  H_29(state, unused, out_6384330637564561494);
+void car_H_29(double *state, double *unused, double *out_5512641085734588844) {
+  H_29(state, unused, out_5512641085734588844);
 }
-void car_h_28(double *state, double *unused, double *out_2847072593533131295) {
-  h_28(state, unused, out_2847072593533131295);
+void car_h_28(double *state, double *unused, double *out_8663409908191306367) {
+  h_28(state, unused, out_8663409908191306367);
 }
-void car_H_28(double *state, double *unused, double *out_6980014419075459548) {
-  H_28(state, unused, out_6980014419075459548);
+void car_H_28(double *state, double *unused, double *out_7851703970905432198) {
+  H_28(state, unused, out_7851703970905432198);
 }
-void car_h_31(double *state, double *unused, double *out_858029248528357936) {
-  h_31(state, unused, out_858029248528357936);
+void car_h_31(double *state, double *unused, double *out_5201620103558886575) {
+  h_31(state, unused, out_5201620103558886575);
 }
-void car_H_31(double *state, double *unused, double *out_6734577072858753180) {
-  H_31(state, unused, out_6734577072858753180);
+void car_H_31(double *state, double *unused, double *out_1464530138044412402) {
+  H_31(state, unused, out_1464530138044412402);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
  predict(in_x, in_P, in_Q, dt);
@@ -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_7470397526871273149);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6628002157851279603);
-void car_H_mod_fun(double *state, double *out_338615810674868519);
-void car_f_fun(double *state, double dt, double *out_3565074677649486182);
-void car_F_fun(double *state, double dt, double *out_1768894250710066862);
-void car_h_25(double *state, double *unused, double *out_5309084791442776367);
-void car_H_25(double *state, double *unused, double *out_2366865651751345480);
-void car_h_24(double *state, double *unused, double *out_4964929704142686901);
-void car_H_24(double *state, double *unused, double *out_6861199534317849745);
-void car_h_30(double *state, double *unused, double *out_6759716184071659534);
-void car_H_30(double *state, double *unused, double *out_6894561981878953678);
-void car_h_26(double *state, double *unused, double *out_6461120673802567135);
-void car_H_26(double *state, double *unused, double *out_6108368970625401704);
-void car_h_27(double *state, double *unused, double *out_1903562740631468860);
-void car_H_27(double *state, double *unused, double *out_4670967910695010461);
-void car_h_29(double *state, double *unused, double *out_1758042916410813889);
-void car_H_29(double *state, double *unused, double *out_6384330637564561494);
-void car_h_28(double *state, double *unused, double *out_2847072593533131295);
-void car_H_28(double *state, double *unused, double *out_6980014419075459548);
-void car_h_31(double *state, double *unused, double *out_858029248528357936);
-void car_H_31(double *state, double *unused, double *out_6734577072858753180);
+void car_err_fun(double *nom_x, double *delta_x, double *out_1027636603068461502);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_2498107869525935257);
+void car_H_mod_fun(double *state, double *out_2880487860269583385);
+void car_f_fun(double *state, double dt, double *out_8985245024092687483);
+void car_F_fun(double *state, double dt, double *out_135664326016666632);
+void car_h_25(double *state, double *unused, double *out_1339936627130384923);
+void car_H_25(double *state, double *unused, double *out_8541205388556229655);
+void car_h_24(double *state, double *unused, double *out_7843416543337357224);
+void car_H_24(double *state, double *unused, double *out_4983279789753906519);
+void car_h_30(double *state, double *unused, double *out_7253669653801133296);
+void car_H_30(double *state, double *unused, double *out_6022872430048981028);
+void car_h_26(double *state, double *unused, double *out_7856985538477610813);
+void car_H_26(double *state, double *unused, double *out_5236679418795429054);
+void car_h_27(double *state, double *unused, double *out_3661736640530460722);
+void car_H_27(double *state, double *unused, double *out_3799278358865037811);
+void car_h_29(double *state, double *unused, double *out_2211105247901577555);
+void car_H_29(double *state, double *unused, double *out_5512641085734588844);
+void car_h_28(double *state, double *unused, double *out_8663409908191306367);
+void car_H_28(double *state, double *unused, double *out_7851703970905432198);
+void car_h_31(double *state, double *unused, double *out_5201620103558886575);
+void car_H_31(double *state, double *unused, double *out_1464530138044412402);
 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);
@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
 *                                                                            *
 *                         This file is part of 'ekf'                         *
 ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_2891005972447064978) {
-   out_2891005972447064978[0] = delta_x[0] + nom_x[0];
-   out_2891005972447064978[1] = delta_x[1] + nom_x[1];
-   out_2891005972447064978[2] = delta_x[2] + nom_x[2];
-   out_2891005972447064978[3] = delta_x[3] + nom_x[3];
-   out_2891005972447064978[4] = delta_x[4] + nom_x[4];
-   out_2891005972447064978[5] = delta_x[5] + nom_x[5];
-   out_2891005972447064978[6] = delta_x[6] + nom_x[6];
-   out_2891005972447064978[7] = delta_x[7] + nom_x[7];
-   out_2891005972447064978[8] = delta_x[8] + nom_x[8];
-   out_2891005972447064978[9] = delta_x[9] + nom_x[9];
-   out_2891005972447064978[10] = delta_x[10] + nom_x[10];
+void err_fun(double *nom_x, double *delta_x, double *out_7759149317582249240) {
+   out_7759149317582249240[0] = delta_x[0] + nom_x[0];
+   out_7759149317582249240[1] = delta_x[1] + nom_x[1];
+   out_7759149317582249240[2] = delta_x[2] + nom_x[2];
+   out_7759149317582249240[3] = delta_x[3] + nom_x[3];
+   out_7759149317582249240[4] = delta_x[4] + nom_x[4];
+   out_7759149317582249240[5] = delta_x[5] + nom_x[5];
+   out_7759149317582249240[6] = delta_x[6] + nom_x[6];
+   out_7759149317582249240[7] = delta_x[7] + nom_x[7];
+   out_7759149317582249240[8] = delta_x[8] + nom_x[8];
+   out_7759149317582249240[9] = delta_x[9] + nom_x[9];
+   out_7759149317582249240[10] = delta_x[10] + nom_x[10];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_3707348186065527957) {
-   out_3707348186065527957[0] = -nom_x[0] + true_x[0];
-   out_3707348186065527957[1] = -nom_x[1] + true_x[1];
-   out_3707348186065527957[2] = -nom_x[2] + true_x[2];
-   out_3707348186065527957[3] = -nom_x[3] + true_x[3];
-   out_3707348186065527957[4] = -nom_x[4] + true_x[4];
-   out_3707348186065527957[5] = -nom_x[5] + true_x[5];
-   out_3707348186065527957[6] = -nom_x[6] + true_x[6];
-   out_3707348186065527957[7] = -nom_x[7] + true_x[7];
-   out_3707348186065527957[8] = -nom_x[8] + true_x[8];
-   out_3707348186065527957[9] = -nom_x[9] + true_x[9];
-   out_3707348186065527957[10] = -nom_x[10] + true_x[10];
+void inv_err_fun(double *nom_x, double *true_x, double *out_9118422902961757933) {
+   out_9118422902961757933[0] = -nom_x[0] + true_x[0];
+   out_9118422902961757933[1] = -nom_x[1] + true_x[1];
+   out_9118422902961757933[2] = -nom_x[2] + true_x[2];
+   out_9118422902961757933[3] = -nom_x[3] + true_x[3];
+   out_9118422902961757933[4] = -nom_x[4] + true_x[4];
+   out_9118422902961757933[5] = -nom_x[5] + true_x[5];
+   out_9118422902961757933[6] = -nom_x[6] + true_x[6];
+   out_9118422902961757933[7] = -nom_x[7] + true_x[7];
+   out_9118422902961757933[8] = -nom_x[8] + true_x[8];
+   out_9118422902961757933[9] = -nom_x[9] + true_x[9];
+   out_9118422902961757933[10] = -nom_x[10] + true_x[10];
 }
-void H_mod_fun(double *state, double *out_3968679355288269749) {
-   out_3968679355288269749[0] = 1.0;
-   out_3968679355288269749[1] = 0;
-   out_3968679355288269749[2] = 0;
-   out_3968679355288269749[3] = 0;
-   out_3968679355288269749[4] = 0;
-   out_3968679355288269749[5] = 0;
-   out_3968679355288269749[6] = 0;
-   out_3968679355288269749[7] = 0;
-   out_3968679355288269749[8] = 0;
-   out_3968679355288269749[9] = 0;
-   out_3968679355288269749[10] = 0;
-   out_3968679355288269749[11] = 0;
-   out_3968679355288269749[12] = 1.0;
-   out_3968679355288269749[13] = 0;
-   out_3968679355288269749[14] = 0;
-   out_3968679355288269749[15] = 0;
-   out_3968679355288269749[16] = 0;
-   out_3968679355288269749[17] = 0;
-   out_3968679355288269749[18] = 0;
-   out_3968679355288269749[19] = 0;
-   out_3968679355288269749[20] = 0;
-   out_3968679355288269749[21] = 0;
-   out_3968679355288269749[22] = 0;
-   out_3968679355288269749[23] = 0;
-   out_3968679355288269749[24] = 1.0;
-   out_3968679355288269749[25] = 0;
-   out_3968679355288269749[26] = 0;
-   out_3968679355288269749[27] = 0;
-   out_3968679355288269749[28] = 0;
-   out_3968679355288269749[29] = 0;
-   out_3968679355288269749[30] = 0;
-   out_3968679355288269749[31] = 0;
-   out_3968679355288269749[32] = 0;
-   out_3968679355288269749[33] = 0;
-   out_3968679355288269749[34] = 0;
-   out_3968679355288269749[35] = 0;
-   out_3968679355288269749[36] = 1.0;
-   out_3968679355288269749[37] = 0;
-   out_3968679355288269749[38] = 0;
-   out_3968679355288269749[39] = 0;
-   out_3968679355288269749[40] = 0;
-   out_3968679355288269749[41] = 0;
-   out_3968679355288269749[42] = 0;
-   out_3968679355288269749[43] = 0;
-   out_3968679355288269749[44] = 0;
-   out_3968679355288269749[45] = 0;
-   out_3968679355288269749[46] = 0;
-   out_3968679355288269749[47] = 0;
-   out_3968679355288269749[48] = 1.0;
-   out_3968679355288269749[49] = 0;
-   out_3968679355288269749[50] = 0;
-   out_3968679355288269749[51] = 0;
-   out_3968679355288269749[52] = 0;
-   out_3968679355288269749[53] = 0;
-   out_3968679355288269749[54] = 0;
-   out_3968679355288269749[55] = 0;
-   out_3968679355288269749[56] = 0;
-   out_3968679355288269749[57] = 0;
-   out_3968679355288269749[58] = 0;
-   out_3968679355288269749[59] = 0;
-   out_3968679355288269749[60] = 1.0;
-   out_3968679355288269749[61] = 0;
-   out_3968679355288269749[62] = 0;
-   out_3968679355288269749[63] = 0;
-   out_3968679355288269749[64] = 0;
-   out_3968679355288269749[65] = 0;
-   out_3968679355288269749[66] = 0;
-   out_3968679355288269749[67] = 0;
-   out_3968679355288269749[68] = 0;
-   out_3968679355288269749[69] = 0;
-   out_3968679355288269749[70] = 0;
-   out_3968679355288269749[71] = 0;
-   out_3968679355288269749[72] = 1.0;
-   out_3968679355288269749[73] = 0;
-   out_3968679355288269749[74] = 0;
-   out_3968679355288269749[75] = 0;
-   out_3968679355288269749[76] = 0;
-   out_3968679355288269749[77] = 0;
-   out_3968679355288269749[78] = 0;
-   out_3968679355288269749[79] = 0;
-   out_3968679355288269749[80] = 0;
-   out_3968679355288269749[81] = 0;
-   out_3968679355288269749[82] = 0;
-   out_3968679355288269749[83] = 0;
-   out_3968679355288269749[84] = 1.0;
-   out_3968679355288269749[85] = 0;
-   out_3968679355288269749[86] = 0;
-   out_3968679355288269749[87] = 0;
-   out_3968679355288269749[88] = 0;
-   out_3968679355288269749[89] = 0;
-   out_3968679355288269749[90] = 0;
-   out_3968679355288269749[91] = 0;
-   out_3968679355288269749[92] = 0;
-   out_3968679355288269749[93] = 0;
-   out_3968679355288269749[94] = 0;
-   out_3968679355288269749[95] = 0;
-   out_3968679355288269749[96] = 1.0;
-   out_3968679355288269749[97] = 0;
-   out_3968679355288269749[98] = 0;
-   out_3968679355288269749[99] = 0;
-   out_3968679355288269749[100] = 0;
-   out_3968679355288269749[101] = 0;
-   out_3968679355288269749[102] = 0;
-   out_3968679355288269749[103] = 0;
-   out_3968679355288269749[104] = 0;
-   out_3968679355288269749[105] = 0;
-   out_3968679355288269749[106] = 0;
-   out_3968679355288269749[107] = 0;
-   out_3968679355288269749[108] = 1.0;
-   out_3968679355288269749[109] = 0;
-   out_3968679355288269749[110] = 0;
-   out_3968679355288269749[111] = 0;
-   out_3968679355288269749[112] = 0;
-   out_3968679355288269749[113] = 0;
-   out_3968679355288269749[114] = 0;
-   out_3968679355288269749[115] = 0;
-   out_3968679355288269749[116] = 0;
-   out_3968679355288269749[117] = 0;
-   out_3968679355288269749[118] = 0;
-   out_3968679355288269749[119] = 0;
-   out_3968679355288269749[120] = 1.0;
+void H_mod_fun(double *state, double *out_1633358768098501879) {
+   out_1633358768098501879[0] = 1.0;
+   out_1633358768098501879[1] = 0;
+   out_1633358768098501879[2] = 0;
+   out_1633358768098501879[3] = 0;
+   out_1633358768098501879[4] = 0;
+   out_1633358768098501879[5] = 0;
+   out_1633358768098501879[6] = 0;
+   out_1633358768098501879[7] = 0;
+   out_1633358768098501879[8] = 0;
+   out_1633358768098501879[9] = 0;
+   out_1633358768098501879[10] = 0;
+   out_1633358768098501879[11] = 0;
+   out_1633358768098501879[12] = 1.0;
+   out_1633358768098501879[13] = 0;
+   out_1633358768098501879[14] = 0;
+   out_1633358768098501879[15] = 0;
+   out_1633358768098501879[16] = 0;
+   out_1633358768098501879[17] = 0;
+   out_1633358768098501879[18] = 0;
+   out_1633358768098501879[19] = 0;
+   out_1633358768098501879[20] = 0;
+   out_1633358768098501879[21] = 0;
+   out_1633358768098501879[22] = 0;
+   out_1633358768098501879[23] = 0;
+   out_1633358768098501879[24] = 1.0;
+   out_1633358768098501879[25] = 0;
+   out_1633358768098501879[26] = 0;
+   out_1633358768098501879[27] = 0;
+   out_1633358768098501879[28] = 0;
+   out_1633358768098501879[29] = 0;
+   out_1633358768098501879[30] = 0;
+   out_1633358768098501879[31] = 0;
+   out_1633358768098501879[32] = 0;
+   out_1633358768098501879[33] = 0;
+   out_1633358768098501879[34] = 0;
+   out_1633358768098501879[35] = 0;
+   out_1633358768098501879[36] = 1.0;
+   out_1633358768098501879[37] = 0;
+   out_1633358768098501879[38] = 0;
+   out_1633358768098501879[39] = 0;
+   out_1633358768098501879[40] = 0;
+   out_1633358768098501879[41] = 0;
+   out_1633358768098501879[42] = 0;
+   out_1633358768098501879[43] = 0;
+   out_1633358768098501879[44] = 0;
+   out_1633358768098501879[45] = 0;
+   out_1633358768098501879[46] = 0;
+   out_1633358768098501879[47] = 0;
+   out_1633358768098501879[48] = 1.0;
+   out_1633358768098501879[49] = 0;
+   out_1633358768098501879[50] = 0;
+   out_1633358768098501879[51] = 0;
+   out_1633358768098501879[52] = 0;
+   out_1633358768098501879[53] = 0;
+   out_1633358768098501879[54] = 0;
+   out_1633358768098501879[55] = 0;
+   out_1633358768098501879[56] = 0;
+   out_1633358768098501879[57] = 0;
+   out_1633358768098501879[58] = 0;
+   out_1633358768098501879[59] = 0;
+   out_1633358768098501879[60] = 1.0;
+   out_1633358768098501879[61] = 0;
+   out_1633358768098501879[62] = 0;
+   out_1633358768098501879[63] = 0;
+   out_1633358768098501879[64] = 0;
+   out_1633358768098501879[65] = 0;
+   out_1633358768098501879[66] = 0;
+   out_1633358768098501879[67] = 0;
+   out_1633358768098501879[68] = 0;
+   out_1633358768098501879[69] = 0;
+   out_1633358768098501879[70] = 0;
+   out_1633358768098501879[71] = 0;
+   out_1633358768098501879[72] = 1.0;
+   out_1633358768098501879[73] = 0;
+   out_1633358768098501879[74] = 0;
+   out_1633358768098501879[75] = 0;
+   out_1633358768098501879[76] = 0;
+   out_1633358768098501879[77] = 0;
+   out_1633358768098501879[78] = 0;
+   out_1633358768098501879[79] = 0;
+   out_1633358768098501879[80] = 0;
+   out_1633358768098501879[81] = 0;
+   out_1633358768098501879[82] = 0;
+   out_1633358768098501879[83] = 0;
+   out_1633358768098501879[84] = 1.0;
+   out_1633358768098501879[85] = 0;
+   out_1633358768098501879[86] = 0;
+   out_1633358768098501879[87] = 0;
+   out_1633358768098501879[88] = 0;
+   out_1633358768098501879[89] = 0;
+   out_1633358768098501879[90] = 0;
+   out_1633358768098501879[91] = 0;
+   out_1633358768098501879[92] = 0;
+   out_1633358768098501879[93] = 0;
+   out_1633358768098501879[94] = 0;
+   out_1633358768098501879[95] = 0;
+   out_1633358768098501879[96] = 1.0;
+   out_1633358768098501879[97] = 0;
+   out_1633358768098501879[98] = 0;
+   out_1633358768098501879[99] = 0;
+   out_1633358768098501879[100] = 0;
+   out_1633358768098501879[101] = 0;
+   out_1633358768098501879[102] = 0;
+   out_1633358768098501879[103] = 0;
+   out_1633358768098501879[104] = 0;
+   out_1633358768098501879[105] = 0;
+   out_1633358768098501879[106] = 0;
+   out_1633358768098501879[107] = 0;
+   out_1633358768098501879[108] = 1.0;
+   out_1633358768098501879[109] = 0;
+   out_1633358768098501879[110] = 0;
+   out_1633358768098501879[111] = 0;
+   out_1633358768098501879[112] = 0;
+   out_1633358768098501879[113] = 0;
+   out_1633358768098501879[114] = 0;
+   out_1633358768098501879[115] = 0;
+   out_1633358768098501879[116] = 0;
+   out_1633358768098501879[117] = 0;
+   out_1633358768098501879[118] = 0;
+   out_1633358768098501879[119] = 0;
+   out_1633358768098501879[120] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_399920222174142974) {
-   out_399920222174142974[0] = dt*state[3] + state[0];
-   out_399920222174142974[1] = dt*state[4] + state[1];
-   out_399920222174142974[2] = dt*state[5] + state[2];
-   out_399920222174142974[3] = state[3];
-   out_399920222174142974[4] = state[4];
-   out_399920222174142974[5] = state[5];
-   out_399920222174142974[6] = dt*state[7] + state[6];
-   out_399920222174142974[7] = dt*state[8] + state[7];
-   out_399920222174142974[8] = state[8];
-   out_399920222174142974[9] = state[9];
-   out_399920222174142974[10] = state[10];
+void f_fun(double *state, double dt, double *out_436651591053660649) {
+   out_436651591053660649[0] = dt*state[3] + state[0];
+   out_436651591053660649[1] = dt*state[4] + state[1];
+   out_436651591053660649[2] = dt*state[5] + state[2];
+   out_436651591053660649[3] = state[3];
+   out_436651591053660649[4] = state[4];
+   out_436651591053660649[5] = state[5];
+   out_436651591053660649[6] = dt*state[7] + state[6];
+   out_436651591053660649[7] = dt*state[8] + state[7];
+   out_436651591053660649[8] = state[8];
+   out_436651591053660649[9] = state[9];
+   out_436651591053660649[10] = state[10];
 }
-void F_fun(double *state, double dt, double *out_4032596610344199094) {
-   out_4032596610344199094[0] = 1;
-   out_4032596610344199094[1] = 0;
-   out_4032596610344199094[2] = 0;
-   out_4032596610344199094[3] = dt;
-   out_4032596610344199094[4] = 0;
-   out_4032596610344199094[5] = 0;
-   out_4032596610344199094[6] = 0;
-   out_4032596610344199094[7] = 0;
-   out_4032596610344199094[8] = 0;
-   out_4032596610344199094[9] = 0;
-   out_4032596610344199094[10] = 0;
-   out_4032596610344199094[11] = 0;
-   out_4032596610344199094[12] = 1;
-   out_4032596610344199094[13] = 0;
-   out_4032596610344199094[14] = 0;
-   out_4032596610344199094[15] = dt;
-   out_4032596610344199094[16] = 0;
-   out_4032596610344199094[17] = 0;
-   out_4032596610344199094[18] = 0;
-   out_4032596610344199094[19] = 0;
-   out_4032596610344199094[20] = 0;
-   out_4032596610344199094[21] = 0;
-   out_4032596610344199094[22] = 0;
-   out_4032596610344199094[23] = 0;
-   out_4032596610344199094[24] = 1;
-   out_4032596610344199094[25] = 0;
-   out_4032596610344199094[26] = 0;
-   out_4032596610344199094[27] = dt;
-   out_4032596610344199094[28] = 0;
-   out_4032596610344199094[29] = 0;
-   out_4032596610344199094[30] = 0;
-   out_4032596610344199094[31] = 0;
-   out_4032596610344199094[32] = 0;
-   out_4032596610344199094[33] = 0;
-   out_4032596610344199094[34] = 0;
-   out_4032596610344199094[35] = 0;
-   out_4032596610344199094[36] = 1;
-   out_4032596610344199094[37] = 0;
-   out_4032596610344199094[38] = 0;
-   out_4032596610344199094[39] = 0;
-   out_4032596610344199094[40] = 0;
-   out_4032596610344199094[41] = 0;
-   out_4032596610344199094[42] = 0;
-   out_4032596610344199094[43] = 0;
-   out_4032596610344199094[44] = 0;
-   out_4032596610344199094[45] = 0;
-   out_4032596610344199094[46] = 0;
-   out_4032596610344199094[47] = 0;
-   out_4032596610344199094[48] = 1;
-   out_4032596610344199094[49] = 0;
-   out_4032596610344199094[50] = 0;
-   out_4032596610344199094[51] = 0;
-   out_4032596610344199094[52] = 0;
-   out_4032596610344199094[53] = 0;
-   out_4032596610344199094[54] = 0;
-   out_4032596610344199094[55] = 0;
-   out_4032596610344199094[56] = 0;
-   out_4032596610344199094[57] = 0;
-   out_4032596610344199094[58] = 0;
-   out_4032596610344199094[59] = 0;
-   out_4032596610344199094[60] = 1;
-   out_4032596610344199094[61] = 0;
-   out_4032596610344199094[62] = 0;
-   out_4032596610344199094[63] = 0;
-   out_4032596610344199094[64] = 0;
-   out_4032596610344199094[65] = 0;
-   out_4032596610344199094[66] = 0;
-   out_4032596610344199094[67] = 0;
-   out_4032596610344199094[68] = 0;
-   out_4032596610344199094[69] = 0;
-   out_4032596610344199094[70] = 0;
-   out_4032596610344199094[71] = 0;
-   out_4032596610344199094[72] = 1;
-   out_4032596610344199094[73] = dt;
-   out_4032596610344199094[74] = 0;
-   out_4032596610344199094[75] = 0;
-   out_4032596610344199094[76] = 0;
-   out_4032596610344199094[77] = 0;
-   out_4032596610344199094[78] = 0;
-   out_4032596610344199094[79] = 0;
-   out_4032596610344199094[80] = 0;
-   out_4032596610344199094[81] = 0;
-   out_4032596610344199094[82] = 0;
-   out_4032596610344199094[83] = 0;
-   out_4032596610344199094[84] = 1;
-   out_4032596610344199094[85] = dt;
-   out_4032596610344199094[86] = 0;
-   out_4032596610344199094[87] = 0;
-   out_4032596610344199094[88] = 0;
-   out_4032596610344199094[89] = 0;
-   out_4032596610344199094[90] = 0;
-   out_4032596610344199094[91] = 0;
-   out_4032596610344199094[92] = 0;
-   out_4032596610344199094[93] = 0;
-   out_4032596610344199094[94] = 0;
-   out_4032596610344199094[95] = 0;
-   out_4032596610344199094[96] = 1;
-   out_4032596610344199094[97] = 0;
-   out_4032596610344199094[98] = 0;
-   out_4032596610344199094[99] = 0;
-   out_4032596610344199094[100] = 0;
-   out_4032596610344199094[101] = 0;
-   out_4032596610344199094[102] = 0;
-   out_4032596610344199094[103] = 0;
-   out_4032596610344199094[104] = 0;
-   out_4032596610344199094[105] = 0;
-   out_4032596610344199094[106] = 0;
-   out_4032596610344199094[107] = 0;
-   out_4032596610344199094[108] = 1;
-   out_4032596610344199094[109] = 0;
-   out_4032596610344199094[110] = 0;
-   out_4032596610344199094[111] = 0;
-   out_4032596610344199094[112] = 0;
-   out_4032596610344199094[113] = 0;
-   out_4032596610344199094[114] = 0;
-   out_4032596610344199094[115] = 0;
-   out_4032596610344199094[116] = 0;
-   out_4032596610344199094[117] = 0;
-   out_4032596610344199094[118] = 0;
-   out_4032596610344199094[119] = 0;
-   out_4032596610344199094[120] = 1;
+void F_fun(double *state, double dt, double *out_7304280096771648861) {
+   out_7304280096771648861[0] = 1;
+   out_7304280096771648861[1] = 0;
+   out_7304280096771648861[2] = 0;
+   out_7304280096771648861[3] = dt;
+   out_7304280096771648861[4] = 0;
+   out_7304280096771648861[5] = 0;
+   out_7304280096771648861[6] = 0;
+   out_7304280096771648861[7] = 0;
+   out_7304280096771648861[8] = 0;
+   out_7304280096771648861[9] = 0;
+   out_7304280096771648861[10] = 0;
+   out_7304280096771648861[11] = 0;
+   out_7304280096771648861[12] = 1;
+   out_7304280096771648861[13] = 0;
+   out_7304280096771648861[14] = 0;
+   out_7304280096771648861[15] = dt;
+   out_7304280096771648861[16] = 0;
+   out_7304280096771648861[17] = 0;
+   out_7304280096771648861[18] = 0;
+   out_7304280096771648861[19] = 0;
+   out_7304280096771648861[20] = 0;
+   out_7304280096771648861[21] = 0;
+   out_7304280096771648861[22] = 0;
+   out_7304280096771648861[23] = 0;
+   out_7304280096771648861[24] = 1;
+   out_7304280096771648861[25] = 0;
+   out_7304280096771648861[26] = 0;
+   out_7304280096771648861[27] = dt;
+   out_7304280096771648861[28] = 0;
+   out_7304280096771648861[29] = 0;
+   out_7304280096771648861[30] = 0;
+   out_7304280096771648861[31] = 0;
+   out_7304280096771648861[32] = 0;
+   out_7304280096771648861[33] = 0;
+   out_7304280096771648861[34] = 0;
+   out_7304280096771648861[35] = 0;
+   out_7304280096771648861[36] = 1;
+   out_7304280096771648861[37] = 0;
+   out_7304280096771648861[38] = 0;
+   out_7304280096771648861[39] = 0;
+   out_7304280096771648861[40] = 0;
+   out_7304280096771648861[41] = 0;
+   out_7304280096771648861[42] = 0;
+   out_7304280096771648861[43] = 0;
+   out_7304280096771648861[44] = 0;
+   out_7304280096771648861[45] = 0;
+   out_7304280096771648861[46] = 0;
+   out_7304280096771648861[47] = 0;
+   out_7304280096771648861[48] = 1;
+   out_7304280096771648861[49] = 0;
+   out_7304280096771648861[50] = 0;
+   out_7304280096771648861[51] = 0;
+   out_7304280096771648861[52] = 0;
+   out_7304280096771648861[53] = 0;
+   out_7304280096771648861[54] = 0;
+   out_7304280096771648861[55] = 0;
+   out_7304280096771648861[56] = 0;
+   out_7304280096771648861[57] = 0;
+   out_7304280096771648861[58] = 0;
+   out_7304280096771648861[59] = 0;
+   out_7304280096771648861[60] = 1;
+   out_7304280096771648861[61] = 0;
+   out_7304280096771648861[62] = 0;
+   out_7304280096771648861[63] = 0;
+   out_7304280096771648861[64] = 0;
+   out_7304280096771648861[65] = 0;
+   out_7304280096771648861[66] = 0;
+   out_7304280096771648861[67] = 0;
+   out_7304280096771648861[68] = 0;
+   out_7304280096771648861[69] = 0;
+   out_7304280096771648861[70] = 0;
+   out_7304280096771648861[71] = 0;
+   out_7304280096771648861[72] = 1;
+   out_7304280096771648861[73] = dt;
+   out_7304280096771648861[74] = 0;
+   out_7304280096771648861[75] = 0;
+   out_7304280096771648861[76] = 0;
+   out_7304280096771648861[77] = 0;
+   out_7304280096771648861[78] = 0;
+   out_7304280096771648861[79] = 0;
+   out_7304280096771648861[80] = 0;
+   out_7304280096771648861[81] = 0;
+   out_7304280096771648861[82] = 0;
+   out_7304280096771648861[83] = 0;
+   out_7304280096771648861[84] = 1;
+   out_7304280096771648861[85] = dt;
+   out_7304280096771648861[86] = 0;
+   out_7304280096771648861[87] = 0;
+   out_7304280096771648861[88] = 0;
+   out_7304280096771648861[89] = 0;
+   out_7304280096771648861[90] = 0;
+   out_7304280096771648861[91] = 0;
+   out_7304280096771648861[92] = 0;
+   out_7304280096771648861[93] = 0;
+   out_7304280096771648861[94] = 0;
+   out_7304280096771648861[95] = 0;
+   out_7304280096771648861[96] = 1;
+   out_7304280096771648861[97] = 0;
+   out_7304280096771648861[98] = 0;
+   out_7304280096771648861[99] = 0;
+   out_7304280096771648861[100] = 0;
+   out_7304280096771648861[101] = 0;
+   out_7304280096771648861[102] = 0;
+   out_7304280096771648861[103] = 0;
+   out_7304280096771648861[104] = 0;
+   out_7304280096771648861[105] = 0;
+   out_7304280096771648861[106] = 0;
+   out_7304280096771648861[107] = 0;
+   out_7304280096771648861[108] = 1;
+   out_7304280096771648861[109] = 0;
+   out_7304280096771648861[110] = 0;
+   out_7304280096771648861[111] = 0;
+   out_7304280096771648861[112] = 0;
+   out_7304280096771648861[113] = 0;
+   out_7304280096771648861[114] = 0;
+   out_7304280096771648861[115] = 0;
+   out_7304280096771648861[116] = 0;
+   out_7304280096771648861[117] = 0;
+   out_7304280096771648861[118] = 0;
+   out_7304280096771648861[119] = 0;
+   out_7304280096771648861[120] = 1;
 }
-void h_6(double *state, double *sat_pos, double *out_7663281162910400321) {
-   out_7663281162910400321[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
+void h_6(double *state, double *sat_pos, double *out_4764504202269866901) {
+   out_4764504202269866901[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
 }
-void H_6(double *state, double *sat_pos, double *out_6735513936967990127) {
-   out_6735513936967990127[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_6735513936967990127[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_6735513936967990127[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_6735513936967990127[3] = 0;
-   out_6735513936967990127[4] = 0;
-   out_6735513936967990127[5] = 0;
-   out_6735513936967990127[6] = 1;
-   out_6735513936967990127[7] = 0;
-   out_6735513936967990127[8] = 0;
-   out_6735513936967990127[9] = 0;
-   out_6735513936967990127[10] = 0;
+void H_6(double *state, double *sat_pos, double *out_4085363850207817832) {
+   out_4085363850207817832[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_4085363850207817832[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_4085363850207817832[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_4085363850207817832[3] = 0;
+   out_4085363850207817832[4] = 0;
+   out_4085363850207817832[5] = 0;
+   out_4085363850207817832[6] = 1;
+   out_4085363850207817832[7] = 0;
+   out_4085363850207817832[8] = 0;
+   out_4085363850207817832[9] = 0;
+   out_4085363850207817832[10] = 0;
 }
-void h_20(double *state, double *sat_pos, double *out_113008328182865177) {
-   out_113008328182865177[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
+void h_20(double *state, double *sat_pos, double *out_6801986365944636663) {
+   out_6801986365944636663[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
 }
-void H_20(double *state, double *sat_pos, double *out_8868019487569726413) {
-   out_8868019487569726413[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_8868019487569726413[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_8868019487569726413[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
-   out_8868019487569726413[3] = 0;
-   out_8868019487569726413[4] = 0;
-   out_8868019487569726413[5] = 0;
-   out_8868019487569726413[6] = 1;
-   out_8868019487569726413[7] = 0;
-   out_8868019487569726413[8] = 0;
-   out_8868019487569726413[9] = 1;
-   out_8868019487569726413[10] = sat_pos[3];
+void H_20(double *state, double *sat_pos, double *out_7866410108663176377) {
+   out_7866410108663176377[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_7866410108663176377[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_7866410108663176377[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
+   out_7866410108663176377[3] = 0;
+   out_7866410108663176377[4] = 0;
+   out_7866410108663176377[5] = 0;
+   out_7866410108663176377[6] = 1;
+   out_7866410108663176377[7] = 0;
+   out_7866410108663176377[8] = 0;
+   out_7866410108663176377[9] = 1;
+   out_7866410108663176377[10] = sat_pos[3];
 }
-void h_7(double *state, double *sat_pos_vel, double *out_4555536823234833923) {
-   out_4555536823234833923[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
+void h_7(double *state, double *sat_pos_vel, double *out_4005211607259102508) {
+   out_4005211607259102508[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
 }
-void H_7(double *state, double *sat_pos_vel, double *out_4061718503456001340) {
-   out_4061718503456001340[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[6] = 0;
-   out_4061718503456001340[7] = 1;
-   out_4061718503456001340[8] = 0;
-   out_4061718503456001340[9] = 0;
-   out_4061718503456001340[10] = 0;
+void H_7(double *state, double *sat_pos_vel, double *out_2701243593729319356) {
+   out_2701243593729319356[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[6] = 0;
+   out_2701243593729319356[7] = 1;
+   out_2701243593729319356[8] = 0;
+   out_2701243593729319356[9] = 0;
+   out_2701243593729319356[10] = 0;
 }
-void h_21(double *state, double *sat_pos_vel, double *out_4555536823234833923) {
-   out_4555536823234833923[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
+void h_21(double *state, double *sat_pos_vel, double *out_4005211607259102508) {
+   out_4005211607259102508[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
 }
-void H_21(double *state, double *sat_pos_vel, double *out_4061718503456001340) {
-   out_4061718503456001340[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
-   out_4061718503456001340[6] = 0;
-   out_4061718503456001340[7] = 1;
-   out_4061718503456001340[8] = 0;
-   out_4061718503456001340[9] = 0;
-   out_4061718503456001340[10] = 0;
+void H_21(double *state, double *sat_pos_vel, double *out_2701243593729319356) {
+   out_2701243593729319356[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
+   out_2701243593729319356[6] = 0;
+   out_2701243593729319356[7] = 1;
+   out_2701243593729319356[8] = 0;
+   out_2701243593729319356[9] = 0;
+   out_2701243593729319356[10] = 0;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
  update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
 }
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_2891005972447064978) {
-  err_fun(nom_x, delta_x, out_2891005972447064978);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_7759149317582249240) {
+  err_fun(nom_x, delta_x, out_7759149317582249240);
 }
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_3707348186065527957) {
-  inv_err_fun(nom_x, true_x, out_3707348186065527957);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9118422902961757933) {
+  inv_err_fun(nom_x, true_x, out_9118422902961757933);
 }
-void gnss_H_mod_fun(double *state, double *out_3968679355288269749) {
-  H_mod_fun(state, out_3968679355288269749);
+void gnss_H_mod_fun(double *state, double *out_1633358768098501879) {
+  H_mod_fun(state, out_1633358768098501879);
 }
-void gnss_f_fun(double *state, double dt, double *out_399920222174142974) {
-  f_fun(state,  dt, out_399920222174142974);
+void gnss_f_fun(double *state, double dt, double *out_436651591053660649) {
+  f_fun(state,  dt, out_436651591053660649);
 }
-void gnss_F_fun(double *state, double dt, double *out_4032596610344199094) {
-  F_fun(state,  dt, out_4032596610344199094);
+void gnss_F_fun(double *state, double dt, double *out_7304280096771648861) {
+  F_fun(state,  dt, out_7304280096771648861);
 }
-void gnss_h_6(double *state, double *sat_pos, double *out_7663281162910400321) {
-  h_6(state, sat_pos, out_7663281162910400321);
+void gnss_h_6(double *state, double *sat_pos, double *out_4764504202269866901) {
+  h_6(state, sat_pos, out_4764504202269866901);
 }
-void gnss_H_6(double *state, double *sat_pos, double *out_6735513936967990127) {
-  H_6(state, sat_pos, out_6735513936967990127);
+void gnss_H_6(double *state, double *sat_pos, double *out_4085363850207817832) {
+  H_6(state, sat_pos, out_4085363850207817832);
 }
-void gnss_h_20(double *state, double *sat_pos, double *out_113008328182865177) {
-  h_20(state, sat_pos, out_113008328182865177);
+void gnss_h_20(double *state, double *sat_pos, double *out_6801986365944636663) {
+  h_20(state, sat_pos, out_6801986365944636663);
 }
-void gnss_H_20(double *state, double *sat_pos, double *out_8868019487569726413) {
-  H_20(state, sat_pos, out_8868019487569726413);
+void gnss_H_20(double *state, double *sat_pos, double *out_7866410108663176377) {
+  H_20(state, sat_pos, out_7866410108663176377);
 }
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_4555536823234833923) {
-  h_7(state, sat_pos_vel, out_4555536823234833923);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_4005211607259102508) {
+  h_7(state, sat_pos_vel, out_4005211607259102508);
 }
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_4061718503456001340) {
-  H_7(state, sat_pos_vel, out_4061718503456001340);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_2701243593729319356) {
+  H_7(state, sat_pos_vel, out_2701243593729319356);
 }
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_4555536823234833923) {
-  h_21(state, sat_pos_vel, out_4555536823234833923);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_4005211607259102508) {
+  h_21(state, sat_pos_vel, out_4005211607259102508);
 }
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_4061718503456001340) {
-  H_21(state, sat_pos_vel, out_4061718503456001340);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_2701243593729319356) {
+  H_21(state, sat_pos_vel, out_2701243593729319356);
 }
 void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
  predict(in_x, in_P, in_Q, dt);
@@ -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_2891005972447064978);
-void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_3707348186065527957);
-void gnss_H_mod_fun(double *state, double *out_3968679355288269749);
-void gnss_f_fun(double *state, double dt, double *out_399920222174142974);
-void gnss_F_fun(double *state, double dt, double *out_4032596610344199094);
-void gnss_h_6(double *state, double *sat_pos, double *out_7663281162910400321);
-void gnss_H_6(double *state, double *sat_pos, double *out_6735513936967990127);
-void gnss_h_20(double *state, double *sat_pos, double *out_113008328182865177);
-void gnss_H_20(double *state, double *sat_pos, double *out_8868019487569726413);
-void gnss_h_7(double *state, double *sat_pos_vel, double *out_4555536823234833923);
-void gnss_H_7(double *state, double *sat_pos_vel, double *out_4061718503456001340);
-void gnss_h_21(double *state, double *sat_pos_vel, double *out_4555536823234833923);
-void gnss_H_21(double *state, double *sat_pos_vel, double *out_4061718503456001340);
+void gnss_err_fun(double *nom_x, double *delta_x, double *out_7759149317582249240);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_9118422902961757933);
+void gnss_H_mod_fun(double *state, double *out_1633358768098501879);
+void gnss_f_fun(double *state, double dt, double *out_436651591053660649);
+void gnss_F_fun(double *state, double dt, double *out_7304280096771648861);
+void gnss_h_6(double *state, double *sat_pos, double *out_4764504202269866901);
+void gnss_H_6(double *state, double *sat_pos, double *out_4085363850207817832);
+void gnss_h_20(double *state, double *sat_pos, double *out_6801986365944636663);
+void gnss_H_20(double *state, double *sat_pos, double *out_7866410108663176377);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_4005211607259102508);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_2701243593729319356);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_4005211607259102508);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_2701243593729319356);
 void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -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_6108960324934765761);
-void live_err_fun(double *nom_x, double *delta_x, double *out_5138688319610964290);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_6257573199628817064);
-void live_H_mod_fun(double *state, double *out_8083599239921719366);
-void live_f_fun(double *state, double dt, double *out_7643793919602016258);
-void live_F_fun(double *state, double dt, double *out_4260521617827134546);
-void live_h_4(double *state, double *unused, double *out_2452644479399717823);
-void live_H_4(double *state, double *unused, double *out_1161040572896477752);
-void live_h_9(double *state, double *unused, double *out_4605071760982415093);
-void live_H_9(double *state, double *unused, double *out_5318208309251255235);
-void live_h_10(double *state, double *unused, double *out_7166915440040899603);
-void live_H_10(double *state, double *unused, double *out_2424141324741574485);
-void live_h_12(double *state, double *unused, double *out_7882416310371039195);
-void live_H_12(double *state, double *unused, double *out_539941547848884085);
-void live_h_35(double *state, double *unused, double *out_5434439103979294343);
-void live_H_35(double *state, double *unused, double *out_2205621484476129624);
-void live_h_32(double *state, double *unused, double *out_5159489016614781586);
-void live_H_32(double *state, double *unused, double *out_4989279648894547217);
-void live_h_13(double *state, double *unused, double *out_4317302593675431351);
-void live_H_13(double *state, double *unused, double *out_6114239333584977353);
-void live_h_14(double *state, double *unused, double *out_4605071760982415093);
-void live_H_14(double *state, double *unused, double *out_5318208309251255235);
-void live_h_33(double *state, double *unused, double *out_7683866618272313142);
-void live_H_33(double *state, double *unused, double *out_5356178489114987228);
+void live_H(double *in_vec, double *out_6543170510918427396);
+void live_err_fun(double *nom_x, double *delta_x, double *out_9062015891362533785);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_4758141739877846608);
+void live_H_mod_fun(double *state, double *out_2686832696856623219);
+void live_f_fun(double *state, double dt, double *out_3822263192861325508);
+void live_F_fun(double *state, double dt, double *out_6455895415279509978);
+void live_h_4(double *state, double *unused, double *out_3494769867212102904);
+void live_H_4(double *state, double *unused, double *out_1451768599627278965);
+void live_h_9(double *state, double *unused, double *out_6878791664417266905);
+void live_H_9(double *state, double *unused, double *out_1210578952997688320);
+void live_h_10(double *state, double *unused, double *out_4380493807452258420);
+void live_H_10(double *state, double *unused, double *out_1918065783372193268);
+void live_h_12(double *state, double *unused, double *out_1827589486894337029);
+void live_H_12(double *state, double *unused, double *out_830669574579685298);
+void live_h_35(double *state, double *unused, double *out_8082777464406159763);
+void live_H_35(double *state, double *unused, double *out_1914893457745328411);
+void live_h_32(double *state, double *unused, double *out_3678648917755286173);
+void live_H_32(double *state, double *unused, double *out_7716197829952940969);
+void live_h_13(double *state, double *unused, double *out_2018478861536013333);
+void live_H_13(double *state, double *unused, double *out_3066565144776721745);
+void live_h_14(double *state, double *unused, double *out_6878791664417266905);
+void live_H_14(double *state, double *unused, double *out_1210578952997688320);
+void live_h_33(double *state, double *unused, double *out_5620049043069576208);
+void live_H_33(double *state, double *unused, double *out_5065450462384186015);
 void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -1,162 +0,0 @@
-#!/usr/bin/env python3
-import os
-import sys
-import errno
-import shutil
-import tarfile
-import time
-import traceback
-from common.basedir import BASEDIR
-from common.text_window import TextWindow
-from urllib.request import urlopen
-from glob import glob
-import subprocess
-import importlib.util
-from importlib.metadata import version
-
-# NOTE: Do NOT import anything here that needs be built (e.g. params)
-from common.spinner import Spinner
-
-
-sys.path.append(os.path.join(BASEDIR, "third_party/mapd"))
-OPSPLINE_SPEC = importlib.util.find_spec('scipy')
-OVERPY_SPEC = importlib.util.find_spec('overpy')
-MAX_BUILD_PROGRESS = 100
-TMP_DIR = '/data/tmp'
-THIRD_PARTY_DIR = '/data/openpilot/third_party/mapd'
-THIRD_PARTY_DIR_SP = '/data/third_party_community'
-PRELOADED_DEP_FILE = os.path.join(BASEDIR, "selfdrive/mapd/assets/mapd_deps.tar.xz")
-OPSPLINE_VERSION = "1.11.1"
-OVERPY_VERSION = "0.6"
-SPECS = {
-  'scipy': OPSPLINE_VERSION,
-  'overpy': OVERPY_VERSION,
-}
-
-
-def wait_for_internet_connection(return_on_failure=False):
-  retries = 0
-  while True:
-    try:
-      _ = urlopen('https://www.google.com/', timeout=10)
-      return True
-    except Exception as e:
-      print(f'Wait for internet failed: {e}')
-      if return_on_failure and retries == 15:
-        return False
-      retries += 1
-      time.sleep(2)  # Wait for 2 seconds before retrying
-
-
-def install_dep(spinner):
-  wait_for_internet_connection()
-
-  TOTAL_PIP_STEPS = 2986
-
-  try:
-    os.makedirs(TMP_DIR)
-  except OSError as e:
-    if e.errno != errno.EEXIST:
-      raise
-  my_env = os.environ.copy()
-  my_env['TMPDIR'] = TMP_DIR
-
-  pip_target = [f'--target={THIRD_PARTY_DIR}']
-  packages = []
-  if OPSPLINE_SPEC is None:
-    packages.append(f'scipy=={OPSPLINE_VERSION}')
-  if OVERPY_SPEC is None:
-    packages.append(f'overpy=={OVERPY_VERSION}')
-
-  pip = subprocess.Popen([sys.executable, "-m", "pip", "install", "-v"] + pip_target + packages,
-                          stdout=subprocess.PIPE, env=my_env)
-
-  # Read progress from pip and update spinner
-  steps = 0
-  while True:
-    output = pip.stdout.readline()
-    if pip.poll() is not None:
-      break
-    if output:
-      steps += 1
-      spinner.update_progress(MAX_BUILD_PROGRESS * min(1., steps / TOTAL_PIP_STEPS), 100.)
-      print(output.decode('utf8', 'replace'))
-
-  shutil.rmtree(TMP_DIR)
-  os.unsetenv('TMPDIR')
-
-  # remove numpy installed to THIRD_PARTY_DIR since numpy is already present in the AGNOS image
-  if OPSPLINE_SPEC is None:
-    for directory in glob(f'{THIRD_PARTY_DIR}/numpy*'):
-      shutil.rmtree(directory)
-    if os.path.exists(f'{THIRD_PARTY_DIR}/bin'):
-      shutil.rmtree(f'{THIRD_PARTY_DIR}/bin')
-
-  dup = f'cp -rf {THIRD_PARTY_DIR} {THIRD_PARTY_DIR_SP}'
-  process_dup = subprocess.Popen(dup, stdout=subprocess.PIPE, shell=True)
-
-
-if __name__ == "__main__":
-  reload_required = False
-  for package, req_version in SPECS.items():
-    package_spec = importlib.util.find_spec(package)
-    if package_spec is not None and version(package) != req_version:
-      print(f"SP_LOG: current {package} is {version(package)}, requires {req_version}. Removing directory {THIRD_PARTY_DIR}...")
-      reload_required = True
-  if reload_required:
-    command = f'rm -rf {THIRD_PARTY_DIR}'
-    process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
-  if OPSPLINE_SPEC is None or OVERPY_SPEC is None or reload_required:
-    spinner = Spinner()
-    preload_fault = False
-    try:
-      if os.path.exists(PRELOADED_DEP_FILE):
-        spinner.update("Loading preloaded dependencies")
-        try:
-          with tarfile.open(PRELOADED_DEP_FILE, "r:xz") as tar:
-            for member in tar.getmembers():
-              split_components = member.name.split('/')
-              if len(split_components) > 1:
-                member.name = '/'.join(split_components[1:])
-              tar.extract(member, path=THIRD_PARTY_DIR)
-          print(f"SP_LOG: Preloaded dependencies extracted to {THIRD_PARTY_DIR}")
-        except Exception as e:
-          preload_fault = True
-          command = f'rm -rf {THIRD_PARTY_DIR}'
-          process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
-          print(f"SP_LOG: An error occurred while extracting preloaded dependencies: {e}")
-          print(f"SP_LOG: Cleanup directory {e}")
-      if not os.path.exists(PRELOADED_DEP_FILE) or preload_fault:
-        if os.path.exists(THIRD_PARTY_DIR_SP):
-          try:
-            spinner.update("Loading cached dependencies")
-            command = f'rm -rf {THIRD_PARTY_DIR}; cp -rf {THIRD_PARTY_DIR_SP} {THIRD_PARTY_DIR}'
-            process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
-            print(f"SP_LOG: Removed directory {THIRD_PARTY_DIR}")
-            print(f"SP_LOG: Copied {THIRD_PARTY_DIR_SP} to {THIRD_PARTY_DIR}")
-          except Exception as e:
-            command = f'rm -rf {THIRD_PARTY_DIR}'
-            process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
-            print(f"SP_LOG: An error occurred while loading cached dependencies: {e}")
-            print(f"SP_LOG: Cleanup directory {e}")
-        else:
-          spinner.update("Waiting for internet")
-          try:
-            install_dep(spinner)
-          except Exception as e:
-            command = f'rm -rf {THIRD_PARTY_DIR}'
-            process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
-            print(f"SP_LOG: An error occurred while downloading dependencies: {e}")
-            print(f"SP_LOG: Cleanup directory {e}")
-    except Exception:
-      command = f'rm -rf {THIRD_PARTY_DIR}'
-      process = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)
-      import selfdrive.sentry as sentry
-      sentry.init(sentry.SentryProject.SELFDRIVE)
-      traceback.print_exc()
-      sentry.capture_exception()
-
-      error = traceback.format_exc(-3)
-      error = "Dependency Manager failed to start\n\n" + error
-      with TextWindow(error) as t:
-        t.wait_for_exit()
@@ -1,72 +0,0 @@
-#!/usr/bin/env python3
-import os
-import subprocess
-import sys
-import time
-import traceback
-
-# NOTE: Do NOT import anything here that needs be built (e.g. params)
-from common.basedir import BASEDIR
-from common.params import Params
-from common.spinner import Spinner
-from common.text_window import TextWindow
-import selfdrive.sentry as sentry
-from selfdrive.manager.custom_dep import wait_for_internet_connection
-
-
-def install_local_osm(_spinner):
-  _install(_spinner, "./install_osm.sh", "Installing OSM Server")
-
-
-def install_osm_db(_spinner):
-  _install(_spinner, "./install_osm_db.sh", "Installing OSM DB - " + Params().get("OsmLocationName", encoding="utf-8"))
-
-
-def _install(_spinner, script, title):
-  _spinner.update(title)
-  process = subprocess.Popen(['sh', script], cwd=os.path.join(BASEDIR, 'installer/custom/'),
-                             stdout=subprocess.PIPE)
-  # Read progress from install script and update spinner
-  frame = 0
-  while True:
-    output = process.stdout.readline()
-    if process.poll() is not None:
-      break
-    _spinner.update(title + (".".replace(".", "." * (frame % 5), 1)))
-    frame += 1
-    print(output.decode('utf8', 'replace'))
-    time.sleep(0.1)
-
-
-if __name__ == "__main__":
-  try:
-    from selfdrive.mapd.lib.helpers import is_local_osm_installed, timestamp_local_osm_db, is_osm_db_up_to_date, OSM_LOCAL_PATH
-    sys.path.append(os.path.join(BASEDIR, "third_party/mapd"))
-    params = Params()
-    update_osm_db_check = params.get_bool("OsmDbUpdatesCheck")
-    if not (os.path.exists(f"{OSM_LOCAL_PATH}/db") or
-            os.path.exists(f"{OSM_LOCAL_PATH}/v0.7.57")) or update_osm_db_check:
-      spinner = Spinner()
-      spinner.update("Waiting for internet connection...")
-      if wait_for_internet_connection(return_on_failure=True):
-        is_osm_installed = is_local_osm_installed(params)
-        is_db_updated = is_osm_db_up_to_date()
-        print(f'Local OSM Installer:\nOSM currently installed: {is_osm_installed}\nDB up to date: {is_db_updated}')
-
-        if not is_osm_installed:
-          install_local_osm(spinner)
-        if not is_db_updated:
-          install_osm_db(spinner)
-          timestamp_local_osm_db()
-      spinner.close()
-
-    params.put_bool("OsmDbUpdatesCheck", False)
-  except Exception:
-    sentry.init(sentry.SentryProject.SELFDRIVE)
-    traceback.print_exc()
-    sentry.capture_exception()
-
-    error = traceback.format_exc(-3)
-    error = "OSM Offline Database Manager failed to start\n\n" + error
-    with TextWindow(error) as t:
-      t.wait_for_exit()
@@ -16,13 +16,14 @@ from openpilot.common.text_window import TextWindow
 from openpilot.selfdrive.boardd.set_time import set_time
 from openpilot.system.hardware import HARDWARE, PC
 from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_params
+from openpilot.selfdrive.manager.mapd_installer import VERSION
 from openpilot.selfdrive.manager.process import ensure_running
 from openpilot.selfdrive.manager.process_config import managed_processes
 from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID, is_registered_device
 from openpilot.system.swaglog import cloudlog, add_file_handler
 from openpilot.system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \
                           get_normalized_origin, terms_version, training_version, \
-                           is_tested_branch, is_release_branch
+                           is_tested_branch, is_release_branch, is_release_sp_branch


 sys.path.append(os.path.join(BASEDIR, "third_party/mapd"))
@@ -79,6 +80,7 @@ def manager_init() -> None:
    ("LkasToggle", "0"),
    ("MadsIconToggle", "1"),
    ("MaxTimeOffroad", "9"),
+    ("NNFF", "0"),
    ("OnroadScreenOff", "-2"),
    ("OnroadScreenOffBrightness", "50"),
    ("OnroadScreenOffEvent", "1"),
@@ -90,6 +92,9 @@ def manager_init() -> None:
    ("SpeedLimitEngageType", "0"),
    ("SpeedLimitValueOffset", "0"),
    ("SpeedLimitOffsetType", "0"),
+    ("SpeedLimitWarningType", "0"),
+    ("SpeedLimitWarningValueOffset", "0"),
+    ("SpeedLimitWarningOffsetType", "0"),
    ("StandStillTimer", "0"),
    ("StockLongToyota", "0"),
    ("TorqueDeadzoneDeg", "0"),
@@ -100,6 +105,10 @@ def manager_init() -> None:
    ("TurnVisionControl", "0"),
    ("VisionCurveLaneless", "0"),
    ("VwAccType", "0"),
+    ("OsmDbUpdatesCheck", "0"),
+    ("OsmDownloadedDate", "0"),
+    ("OSMDownloadProgress", "{}"),
+    ("MapdVersion", f"{VERSION}"),
  ]
  if not PC:
    default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
@@ -140,6 +149,7 @@ def manager_init() -> None:
  params.put("GitRemote", get_origin(default=""))
  params.put_bool("IsTestedBranch", is_tested_branch())
  params.put_bool("IsReleaseBranch", is_release_branch())
+  params.put_bool("IsReleaseSPBranch", is_release_sp_branch())

  # set dongle id
  reg_res = register(show_spinner=True)
@@ -148,6 +158,12 @@ def manager_init() -> None:
  else:
    serial = params.get("HardwareSerial")
    raise Exception(f"Registration failed for device {serial}")
+  if params.get("HardwareSerial") is None:
+    try:
+      serial = HARDWARE.get_serial()
+      params.put("HardwareSerial", serial)
+    except Exception:
+      cloudlog.exception("Error getting serial for device")
  os.environ['DONGLE_ID'] = dongle_id  # Needed for swaglog

  if not is_dirty():
@@ -0,0 +1,143 @@
+#!/usr/bin/env python3
+import logging
+import os
+import stat
+import time
+import traceback
+import requests
+from pathlib import Path
+from urllib.request import urlopen
+import openpilot.selfdrive.sentry as sentry
+from cereal import messaging
+from common.spinner import Spinner
+from common.params import Params
+from openpilot.selfdrive.mapd_manager import COMMON_DIR, MAPD_PATH, MAPD_BIN_DIR
+from openpilot.system.version import is_prebuilt
+
+VERSION = 'v1.8.0'
+URL = f"https://github.com/pfeiferj/openpilot-mapd/releases/download/{VERSION}/mapd"
+
+
+class MapdInstallManager:
+  def __init__(self, spinner_ref: Spinner):
+    self._spinner = spinner_ref
+
+  def download(self):
+    self.ensure_directories_exist()
+    self._download_file()
+    self.update_installed_version(VERSION)
+
+  def check_and_download(self):
+    if self.download_needed():
+      self.download()
+
+  @staticmethod
+  def download_needed():
+    return not os.path.exists(MAPD_PATH) or MapdInstallManager.get_installed_version() != VERSION
+
+  @staticmethod
+  def ensure_directories_exist():
+    if not os.path.exists(COMMON_DIR):
+      os.makedirs(COMMON_DIR)
+    if not os.path.exists(MAPD_BIN_DIR):
+      os.makedirs(MAPD_BIN_DIR)
+
+  @staticmethod
+  def _safe_write_and_set_executable(file_path, content):
+    with open(file_path, 'wb') as output:
+      output.write(content)
+      output.flush()
+      os.fsync(output.fileno())
+    current_permissions = stat.S_IMODE(os.lstat(file_path).st_mode)
+    os.chmod(file_path, current_permissions | stat.S_IEXEC)
+
+  def _download_file(self, num_retries=5):
+    temp_file = Path(MAPD_PATH + ".tmp")
+    download_timeout = 60
+    for cnt in range(num_retries):
+      try:
+        response = requests.get(URL, stream=True, timeout=download_timeout)
+        response.raise_for_status()
+        self._safe_write_and_set_executable(temp_file, response.content)
+        # No exceptions encountered. Safe to replace original file.
+        temp_file.replace(MAPD_PATH)
+        return
+      except requests.exceptions.ReadTimeout:
+        self._spinner.update(f"ReadTimeout caught. Timeout is [{download_timeout}]. Retrying download... [{cnt}]")
+        time.sleep(0.5)
+      except requests.exceptions.RequestException as e:
+        self._spinner.update(f"RequestException caught: {e}. Retrying download... [{cnt}]")
+        time.sleep(0.5)
+
+    # Delete temp file if the process was not successful.
+    if temp_file.exists():
+      temp_file.unlink()
+    logging.error("Failed to download file after all retries")
+
+  @staticmethod
+  def update_installed_version(version):
+    Params().put("MapdVersion", version)
+
+  @staticmethod
+  def get_installed_version():
+    return Params().get("MapdVersion", encoding="utf-8")
+
+  def wait_for_internet_connection(self, return_on_failure=False):
+    max_retries = 10
+    for retries in range(max_retries+1):
+      self._spinner.update(f"Waiting for internet connection... [{retries}/{max_retries}]")
+      time.sleep(2)
+      try:
+        _ = urlopen('https://sentry.io', timeout=10)
+        return True
+      except Exception as e:
+        print(f'Wait for internet failed: {e}')
+        if return_on_failure and retries == max_retries:
+          return False
+
+  def non_prebuilt_install(self):
+    sm = messaging.SubMaster(['deviceState'])
+    metered = sm['deviceState'].networkMetered
+
+    if metered:
+      self._spinner.update(f"Can't proceed with mapd install since network is metered!")
+      time.sleep(5)
+      return False
+
+    try:
+      self.ensure_directories_exist()
+      if not self.download_needed():
+        self._spinner.update("Mapd is good!")
+        time.sleep(0.1)
+        return True
+
+      if self.wait_for_internet_connection(return_on_failure=True):
+        self._spinner.update(f"Downloading pfeiferj's mapd [{install_manager.get_installed_version()}] => [{VERSION}].")
+        time.sleep(0.1)
+        self.check_and_download()
+      self._spinner.close()
+
+    except Exception:
+      for i in range(6):
+        self._spinner.update(f"Failed to download OSM maps won't work until properly downloaded!"
+                             f"Try again manually rebooting. "
+                             f"Boot will continue in {5 - i}s...")
+        time.sleep(1)
+
+      sentry.init(sentry.SentryProject.SELFDRIVE)
+      traceback.print_exc()
+      sentry.capture_exception()
+
+
+if __name__ == "__main__":
+  spinner = Spinner()
+  install_manager = MapdInstallManager(spinner)
+  install_manager.ensure_directories_exist()
+  if is_prebuilt():
+    debug_msg = f"[DEBUG] This is prebuilt, no mapd install required. VERSION: [{VERSION}], Param [{install_manager.get_installed_version()}]"
+    spinner.update(debug_msg)
+    install_manager.update_installed_version(VERSION)
+  else:
+    spinner.update(f"Checking if mapd is installed and valid. Prebuilt [{is_prebuilt()}]")
+    time.sleep(1)
+    install_manager.non_prebuilt_install()
@@ -4,6 +4,7 @@ from cereal import car
 from openpilot.common.params import Params
 from openpilot.system.hardware import PC, TICI
 from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
+from openpilot.selfdrive.mapd_manager import MAPD_PATH, COMMON_DIR

 WEBCAM = os.getenv("USE_WEBCAM") is not None

@@ -82,7 +83,11 @@ procs = [
  PythonProcess("statsd", "selfdrive.statsd", always_run),
  NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None), always_watchdog=True),

-  PythonProcess("mapd", "selfdrive.mapd.mapd", only_onroad),
+  # PFEIFER - MAPD {{
+  NativeProcess("mapd", COMMON_DIR, [MAPD_PATH], always_run),
+  PythonProcess("mapd_manager", "selfdrive.mapd_manager", always_run),
+  # }} PFEIFER - MAPD
+
  PythonProcess("otisserv", "selfdrive.navd.otisserv", always_run),
  PythonProcess("fleet_manager", "system.fleetmanager.fleet_manager", always_run),

@@ -1,8 +0,0 @@
-# MapD
-The OpenStreetMap-based speed logical by the [Move Fast team](https://github.com/move-fast), [dragonpilot team](https://github.com/dragonpilot-community/dragonpilot), and additional improvements by [sunnypilot](https://github.com/sunnyhaibin/sunnypilot).
-
-The comma three uses regular SciPy. To have a better experience with `mapd`, please go to [OpenStreetMap](https://openstreetmap.org) to update and improve your area's data (i.e., Speed Limit, Stop Signs, Traffic Lights).
-
-To use `mapd`, you consent to `mapd` uploading the traces. You may opt out of uploading traces at any time.
-
-© OpenStreetMap contributors
@@ -1,8 +0,0 @@
-# Map query config
-
-QUERY_RADIUS = 3000  # mts. Radius to use on OSM data queries.
-QUERY_RADIUS_OFFLINE = 2250  # mts. Radius to use on offline OSM data queries.
-MIN_DISTANCE_FOR_NEW_QUERY = 1000  # mts. Minimum distance to query area edge before issuing a new query.
-FULL_STOP_MAX_SPEED = 1.39  # m/s Max speed for considering car is stopped.
-LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg.
-LANE_WIDTH = 3.7  # Lane width estimate. Used for detecting departures from way.
@@ -1,452 +0,0 @@
-import numpy as np
-from enum import Enum
-from selfdrive.mapd.lib.geo import DIRECTION, R, vectors
-
-from scipy.interpolate import splev, splprep
-
-
-_TURN_CURVATURE_THRESHOLD = 0.002  # 1/mts. A curvature over this value will generate a speed limit section.
-_MAX_LAT_ACC = 2.  # Maximum lateral acceleration in turns.
-_SPLINE_EVAL_STEP = 5  # mts for spline evaluation for curvature calculation
-_MIN_SPEED_SECTION_LENGTH = 100.  # mts. Sections below this value will not be split in smaller sections.
-_MAX_CURV_DEVIATION_FOR_SPLIT = 2.  # Split a speed section if the max curvature deviates from mean by this factor.
-_MAX_CURV_SPLIT_ARC_ANGLE = 90.  # degrees. Arc section to split into new speed section around max curvature.
-_MIN_NODE_DISTANCE = 50.  # mts. Minimum distance between nodes for spline evaluation. Data is enhanced if not met.
-_ADDED_NODES_DIST = 15.  # mts. Distance between added nodes when data is enhanced for spline evaluation.
-_DIVERTION_SEARCH_RANGE = [-200., 50.]  # mt. Range of distance to current location for diversion search.
-
-
-def nodes_raw_data_array_for_wr(wr, drop_last=False, feature_sl=False):
-  """Provides an array of raw node data (id, lat, lon, speed_limit, advisory_speed_limit) for all nodes in way relation
-  """
-  sl = wr.speed_limit
-  asl = wr.advisory_speed_limit
-  data = np.array([(n.id, n.lat, n.lon, sl, asl) for n in wr.way.nodes], dtype=float)
-
-  if feature_sl:
-    for count, node in enumerate(wr.way.nodes):
-      if 'highway' in node.tags:
-        if node.tags['highway'] == 'mini_roundabout':
-          data[count][3] = 4.1667
-
-        if 'direction' in node.tags and (node.tags['highway'] == 'stop' or node.tags['highway'] == 'give_way'):
-          if (wr.direction == DIRECTION.BACKWARD and node.tags['direction'] == 'backward') or (wr.direction == DIRECTION.FORWARD and node.tags['direction'] == 'forward'):
-            if node.tags['highway'] == 'give_way':
-              data[count][3] = 2.7777
-            if node.tags['highway'] == 'stop':
-              data[count][3] = 0.1
-      if 'traffic_calming' in node.tags:
-        if node.tags['traffic_calming'] == 'yes':
-          data[count][3] = 40/3.6
-        if node.tags['traffic_calming'] == 'chicane' or node.tags['traffic_calming'] == 'choker':
-          data[count][3] = 20/3.6
-        if node.tags['traffic_calming'] == 'bump':
-          data[count][3] = 2.24
-        if node.tags['traffic_calming'] == 'hump':
-          data[count][3] = 8.94
-
-  # reverse the order if way direction is backwards
-  if wr.direction == DIRECTION.BACKWARD:
-    data = np.flip(data, axis=0)
-
-  # drop last if requested
-  return data[:-1] if drop_last else data
-
-
-def node_calculations(points):
-  """Provides node calculations based on an array of (lat, lon) points in radians.
-     points is a (N x 1) array where N >= 3
-  """
-  if len(points) < 3:
-    raise(IndexError)
-
-  # Get the vector representation of node points in cartesian plane.
-  # (N-1, 2) array. Not including (0., 0.)
-  v = vectors(points) * R
-
-  # Calculate the vector magnitudes (or distance)
-  # (N-1, 1) array. No distance for v[-1]
-  d = np.linalg.norm(v, axis=1)
-
-  # Calculate the bearing (from true north clockwise) for every node.
-  # (N-1, 1) array. No bearing for v[-1]
-  b = np.arctan2(v[:, 0], v[:, 1])
-
-  # Add origin to vector space. (i.e first node in list)
-  v = np.concatenate(([[0., 0.]], v))
-
-  # Provide distance to previous node and distance to next node
-  dp = np.concatenate(([0.], d))
-  dn = np.concatenate((d, [0.]))
-
-  # Provide cumulative distance on route
-  dr = np.cumsum(dp, axis=0)
-
-  # Bearing of last node should keep bearing from previous.
-  b = np.concatenate((b, [b[-1]]))
-
-  return v, dp, dn, dr, b
-
-
-def spline_curvature_calculations(vect, dist_prev):
-  """Provides an array of curvatures and its distances by applying a spline interpolation
-  to the path described by the nodes data.
-  """
-  # We need to artificially enhance the data before applying spline interpolation to avoid getting
-  # inexistent curvature values close to irregularities on the road when the resolution of nodes data
-  # approaching the irregularity is low.
-
-  # - Find indexes where dist_prev is greater than threshold
-  too_far_idxs = np.nonzero(dist_prev >= _MIN_NODE_DISTANCE)[0]
-
-  # - Traversing in reverse order, enhance data by adding points at the found indexes.
-  for idx in too_far_idxs[::-1]:
-    dp = dist_prev[idx]  # distance of vector that needs to be replaced by higher resolution vectors.
-    n = int(np.ceil(dp / _ADDED_NODES_DIST))  # number of vectors that need to be added.
-    new_v = vect[idx, :] / n  # new relative vector to insert.
-    vect = np.delete(vect, idx, axis=0)  # remove the relative vector to be replaced by the insertion of new vectors.
-    vect = np.insert(vect, [idx] * n, [new_v] * n, axis=0)  # insert n new relative vectors
-
-  # Data is now enhanced, we can proceed with curvature evaluation.
-  # - Create cumulative arrays for distance traveled and vector (x, y)
-  ds = np.cumsum(dist_prev, axis=0)
-  vs = np.cumsum(vect, axis=0)
-
-  # - spline interpolation
-  tck, u = splprep([vs[:, 0], vs[:, 1]])  # pylint: disable=unbalanced-tuple-unpacking
-
-  # - evaluate every _SPLINE_EVAL_STEP mts.
-  n = max(int(ds[-1] / _SPLINE_EVAL_STEP), len(u))
-  unew = np.arange(0, n + 1) / n
-
-  # - get derivatives
-  d1 = splev(unew, tck, der=1)
-  d2 = splev(unew, tck, der=2)
-
-  # - calculate curvatures
-  num = d1[0] * d2[1] - d1[1] * d2[0]
-  den = (d1[0]**2 + d1[1]**2)**(1.5)
-  curv = num / den
-  curv_ds = unew * ds[-1]
-
-  return curv, curv_ds
-
-
-def speed_section(curv_sec):
-  """Map curvature section data into turn speed sections data.
-    Returns: [section start distance, section end distance, speed limit based on max curvature, sing of curvature]
-  """
-  max_curv_idx = np.argmax(curv_sec[:, 0])
-  start = np.amin(curv_sec[:, 2])
-  end = np.amax(curv_sec[:, 2])
-
-  return np.array([start, end, np.sqrt(_MAX_LAT_ACC / curv_sec[max_curv_idx, 0]), curv_sec[max_curv_idx, 1]])
-
-
-def split_speed_section_by_sign(curv_sec):
-  """Will split the given curvature section in subsections if there is a change of sign on the curvature value
-  in the section.
-  """
-  # Find the indexes where the curvatures change signs (if any).
-  c_idx = np.nonzero(np.diff(curv_sec[:, 1]))[0] + 1
-
-  # Split section base on change of sign.
-  return np.split(curv_sec, c_idx)
-
-
-def split_speed_section_by_curv_degree(curv_sec):
-  """Will split the given curvature section in subsections as to isolate peaks of turn with substantially
-  higher curvature values. This will aid on preventing having very long turn sections with low speed limit
-  that is only really necessary for a small region of the section.
-  """
-  # Only consider splitting a section if long enough.
-  length = curv_sec[-1, 2] - curv_sec[0, 2]
-  if length <= _MIN_SPEED_SECTION_LENGTH:
-    return [curv_sec]
-
-  # Only split if max curvature deviates substantially from mean curvature.
-  max_curv_idx = np.argmax(curv_sec[:, 0])
-  max_curv = curv_sec[max_curv_idx, 0]
-  mean_curv = np.mean(curv_sec[:, 0])
-  if max_curv / mean_curv <= _MAX_CURV_DEVIATION_FOR_SPLIT:
-    return [curv_sec]
-
-  # Calculate where to split as to isolate a curve section around the max curvature peak.
-  arc_side = (np.radians(_MAX_CURV_SPLIT_ARC_ANGLE) / max_curv) / 2.
-  arc_side_idx_lenght = int(np.ceil(arc_side / _SPLINE_EVAL_STEP))
-  split_idxs = [max_curv_idx - arc_side_idx_lenght, max_curv_idx + arc_side_idx_lenght]
-  split_idxs = list(filter(lambda idx: idx > 0 and idx < len(curv_sec) - 1, split_idxs))
-
-  # If the arc section to split extendes outside the section, then no need to split.
-  if len(split_idxs) == 0:
-    return [curv_sec]
-
-  # Create the splits and split the resulting sections recursevly.
-  splits = [split_speed_section_by_curv_degree(cs) for cs in np.split(curv_sec, split_idxs)]
-
-  # Flatten the results and return the new list of curvature sections.
-  curv_secs = [cs for split in splits for cs in split]
-  return curv_secs
-
-
-def speed_limits_for_curvatures_data(curv, dist):
-  """Provides the calculations for the speed limits from the curvatures array and distances,
-    by providing distances to curvature sections and corresponding speed limit values as well as
-    curvature direction/sign.
-  """
-  # Prepare a data array for processing with absolute curvature values, curvature sign and distances.
-  curv_abs = np.abs(curv)
-  data = np.column_stack((curv_abs, np.sign(curv), dist))
-
-  # Find where curvatures overshoot turn curvature threshold and define as section
-  is_section = curv_abs >= _TURN_CURVATURE_THRESHOLD
-
-  # Find the indexes where the sections start and end. i.e. change indexes.
-  c_idx = np.nonzero(np.diff(is_section))[0] + 1
-
-  # Create independent arrays for each split section base on change indexes.
-  splits = np.array(np.split(data, c_idx), dtype=object)
-
-  # Filter the splits to keep only the curvature section arrays by getting the odd or even split arrays depending
-  # on whether the first split is a curvature split or not.
-  curv_sec_idxs = np.arange(0 if is_section[0] else 1, len(splits), 2, dtype=int)
-  curv_secs = splits[curv_sec_idxs]
-
-  # Further split the curv sections by sign change
-  sub_secs = [split_speed_section_by_sign(cs) for cs in curv_secs]
-  curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec]
-
-  # Further split the curv sections by degree of curvature
-  sub_secs = [split_speed_section_by_curv_degree(cs) for cs in curv_secs]
-  curv_secs = [cs for sub_sec in sub_secs for cs in sub_sec]
-
-  # Return an array where each row represents a turn speed limit section.
-  # [start, end, speed_limit, curvature_sign]
-  return np.array([speed_section(cs) for cs in curv_secs])
-
-def is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids):
-  """
-  Evaluates if the way relation `wr` is a valid diversion from node with id `node_id`.
-  A valid diversion is a way relation with an edge node with the given `node_id` that is not already included
-  in the list of way relations in the route (`wr_ids`) and that can be travaled in the direction as if starting
-  from node with id `node_id`
-  """
-  if wr.id in wr_ids:
-    return False
-  wr.update_direction_from_starting_node(node_id)
-  return not wr.is_prohibited
-
-
-class SpeedLimitSection():
-  """And object representing a speed limited road section ahead.
-  provides the start and end distance and the speed limit value
-  """
-  def __init__(self, start, end, value):
-    self.start = start
-    self.end = end
-    self.value = value
-
-  def __repr__(self):
-    return f'from: {self.start}, to: {self.end}, limit: {self.value}'
-
-
-class TurnSpeedLimitSection(SpeedLimitSection):
-  def __init__(self, start, end, value, sign):
-    super().__init__(start, end, value)
-    self.curv_sign = sign
-
-  def __repr__(self):
-    return f'{super().__repr__()}, sign: {self.curv_sign}'
-
-
-class NodeDataIdx(Enum):
-  """Column index for data elements on NodesData underlying data store.
-  """
-  node_id = 0
-  lat = 1
-  lon = 2
-  speed_limit = 3
-  advisory_speed_limit = 4
-  x = 5             # x value of cartesian vector representing the section between last node and this node.
-  y = 6             # y value of cartesian vector representing the section between last node and this node.
-  dist_prev = 7     # distance to previous node.
-  dist_next = 8     # distance to next node
-  dist_route = 9    # cumulative distance on route
-  bearing = 10       # bearing of the vector departing from this node.
-
-
-class NodesData:
-  """Container for the list of node data from a ordered list of way relations to be used in a Route
-  """
-  def __init__(self, way_relations, wr_index):
-    self._nodes_data = np.array([])
-    self._divertions = [[]]
-    self._curvature_speed_sections_data = np.array([])
-
-    way_count = len(way_relations)
-    if way_count == 0:
-      return
-
-    # We want all the nodes from the last way section
-    nodes_data = nodes_raw_data_array_for_wr(way_relations[-1])
-
-    # For the ways before the last in the route we want all the nodes but the last, as that one is the first on
-    # the next section. Collect them, append last way node data and concatenate the numpy arrays.
-    if way_count > 1:
-      wrs_data = tuple([nodes_raw_data_array_for_wr(wr, drop_last=True) for wr in way_relations[:-1]])
-      wrs_data += (nodes_data,)
-      nodes_data = np.concatenate(wrs_data)
-
-    # Get a subarray with lat, lon to compute the remaining node values.
-    lat_lon_array = nodes_data[:, [1, 2]]
-    points = np.radians(lat_lon_array)
-    # Ensure we have more than 3 points, if not calculations are not possible.
-    if len(points) <= 3:
-      return
-    vect, dist_prev, dist_next, dist_route, bearing = node_calculations(points)
-
-    # append calculations to nodes_data
-    # nodes_data structure: [id, lat, lon, speed_limit, advisory_speed_limit, x, y, dist_prev, dist_next, dist_route, bearing]
-    self._nodes_data = np.column_stack((nodes_data, vect, dist_prev, dist_next, dist_route, bearing))
-
-    # Build route diversion options data from the wr_index.
-    wr_ids = [wr.id for wr in way_relations]
-    self._divertions = [[wr for wr in wr_index.way_relations_with_edge_node_id(node_id)
-                        if is_wr_a_valid_divertion_from_node(wr, node_id, wr_ids)]
-                        for node_id in nodes_data[:, 0]]
-
-    # Store calculcations for curvature sections speed limits. We need more than 3 points to be able to process.
-    # _curvature_speed_sections_data structure: [dist_start, dist_stop, speed_limits, curv_sign]
-    if len(vect) > 3:
-      curv, curv_ds = spline_curvature_calculations(vect, dist_prev)
-      self._curvature_speed_sections_data = speed_limits_for_curvatures_data(curv, curv_ds)
-
-  @property
-  def count(self):
-    return len(self._nodes_data)
-
-  def get(self, node_data_idx):
-    """Returns the array containing all the elements of a specific NodeDataIdx type.
-    """
-    if len(self._nodes_data) == 0 or node_data_idx.value >= self._nodes_data.shape[1]:
-      return np.array([])
-
-    return self._nodes_data[:, node_data_idx.value]
-
-  def speed_limits_ahead(self, ahead_idx, distance_to_node_ahead):
-    """Returns and array of SpeedLimitSection objects for the actual route ahead of current location
-    """
-    if len(self._nodes_data) == 0 or ahead_idx is None:
-      return []
-
-    # Find the cumulative distances where speed limit changes. Build Speed limit sections for those.
-    dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:]))
-    dist = np.cumsum(dist, axis=0)
-    sl = self.get(NodeDataIdx.speed_limit)[ahead_idx - 1:]
-    sl_next = np.concatenate((sl[1:], [0.]))
-
-    # Create a boolean mask where speed limit changes and filter values
-    sl_change = sl != sl_next
-    distances = dist[sl_change]
-    speed_limits = sl[sl_change]
-
-    # Create speed limits sections combining all continuous nodes that have same speed limit value.
-    start = 0.
-    limits_ahead = []
-    for idx, end in enumerate(distances):
-      limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx]))
-      start = end
-
-    return limits_ahead
-
-
-  def advisory_speed_limits_ahead(self, ahead_idx, distance_to_node_ahead):
-    """Returns and array of SpeedLimitSection objects for the actual route ahead of current location
-    """
-    if len(self._nodes_data) == 0 or ahead_idx is None:
-      return []
-
-    # Find the cumulative distances where speed limit changes. Build Speed limit sections for those.
-    dist = np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:]))
-    dist = np.cumsum(dist, axis=0)
-    sl = self.get(NodeDataIdx.advisory_speed_limit)[ahead_idx - 1:]
-    sl_next = np.concatenate((sl[1:], [0.]))
-
-    # Create a boolean mask where speed limit changes and filter values
-    sl_change = sl != sl_next
-    distances = dist[sl_change]
-    speed_limits = sl[sl_change]
-
-    # Create speed limits sections combining all continuous nodes that have same speed limit value.
-    start = 0.
-    limits_ahead = []
-    for idx, end in enumerate(distances):
-      if speed_limits[idx] != None and speed_limits[idx] > 0:
-        limits_ahead.append(SpeedLimitSection(start, end, speed_limits[idx]))
-      start = end
-
-    return limits_ahead
-
-
-  def distance_to_end(self, ahead_idx, distance_to_node_ahead):
-    if len(self._nodes_data) == 0 or ahead_idx is None:
-      return None
-
-    return np.sum(np.concatenate(([distance_to_node_ahead], self.get(NodeDataIdx.dist_next)[ahead_idx:])))
-
-  def curvatures_speed_limit_sections_ahead(self, ahead_idx, distance_to_node_ahead):
-    """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location for
-       speed limit sections due to curvatures in the road.
-    """
-    if len(self._curvature_speed_sections_data) == 0 or ahead_idx is None:
-      return []
-
-    # Find the current distance traveled so far on the route.
-    dist_curr = self.get(NodeDataIdx.dist_route)[ahead_idx] - distance_to_node_ahead
-
-    # Filter the sections to get only those where the stop distance is ahead of current.
-    sec_filter = self._curvature_speed_sections_data[:, 1] > dist_curr
-    data = self._curvature_speed_sections_data[sec_filter]
-
-    # Offset distances to current distance.
-    data[:, [0, 1]] -= dist_curr
-
-    # Create speed limits sections
-    limits_ahead = [TurnSpeedLimitSection(max(0., d[0]), d[1], d[2], d[3]) for d in data]
-
-    advisory_speed_limits_ahead = self.advisory_speed_limits_ahead(ahead_idx, distance_to_node_ahead)
-    for advisory_limit in advisory_speed_limits_ahead:
-      for limit in limits_ahead:
-        if limit.start >= advisory_limit.start and limit.end <= advisory_limit.end:
-          limit.value = advisory_limit.value
-
-
-    return limits_ahead
-
-  def possible_divertions(self, ahead_idx, distance_to_node_ahead):
-    """ Returns and array with the way relations the route could possible divert to by finding
-        the alternative way diversions on the nodes in the vicinity of the current location.
-    """
-    if len(self._nodes_data) == 0 or ahead_idx is None:
-      return []
-
-    dist_route = self.get(NodeDataIdx.dist_route)
-    rel_dist = dist_route - dist_route[ahead_idx] + distance_to_node_ahead
-    valid_idxs = np.nonzero(np.logical_and(rel_dist >= _DIVERTION_SEARCH_RANGE[0],
-                            rel_dist <= _DIVERTION_SEARCH_RANGE[1]))[0]
-    valid_divertions = [self._divertions[i] for i in valid_idxs]
-
-    return [wr for wrs in valid_divertions for wr in wrs]  # flatten.
-
-  def distance_to_node(self, node_id, ahead_idx, distance_to_node_ahead):
-    """
-    Provides the distance to a specific node in the route identified by `node_id` in reference to the node ahead
-    (`ahead_idx`) and the distance from current location to the node ahead (`distance_to_node_ahead`).
-    """
-    node_ids = self.get(NodeDataIdx.node_id)
-    node_idxs = np.nonzero(node_ids == node_id)[0]
-    if len(self._nodes_data) == 0 or ahead_idx is None or len(node_idxs) == 0:
-      return None
-
-    return self.get(NodeDataIdx.dist_route)[node_idxs[0]] - self.get(NodeDataIdx.dist_route)[ahead_idx] + \
-      distance_to_node_ahead
@@ -1,343 +0,0 @@
-from selfdrive.mapd.lib.NodesData import NodesData, NodeDataIdx
-from selfdrive.mapd.config import QUERY_RADIUS
-from selfdrive.mapd.lib.geo import ref_vectors, R, distance_to_points
-from itertools import compress
-import numpy as np
-
-
-_ACCEPTABLE_BEARING_DELTA_COSINE = -0.7  # Continuation paths with a bearing of 180 +/- 45 degrees.
-_MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE = -0.3420  # bearing delta at route edge must be 180 +/- 70 degrees.
-_MAP_DATA_EDGE_DISTANCE = 50  # mts. Consider edge of map data from this distance to edge of query radius.
-
-
-class Route():
-  """A set of consecutive way relations forming a default driving route.
-  """
-  def __init__(self, current, wr_index, way_collection_id, query_center):
-    """Create a Route object from a given `wr_index` (Way relation index)
-
-    Args:
-        current (WayRelation): The Way Relation that is currently located. It must be active.
-        wr_index (WayRelationIndex): The indexes of WayRelations by node id.
-        way_collection_id (UUID): The id of the Way Collection that created this Route.
-        query_center (Numpy Array): lat, lon] numpy array in radians indicating the center of the data query.
-    """
-    self.way_collection_id = way_collection_id
-    self._ordered_way_relations = []
-    self._nodes_data = None
-    self._reset()
-
-    # An active current way is needed to be able to build a route
-    if not current.active:
-      return
-
-    # Build the route by finding iteratavely the best matching ways continuing after the end of the
-    # current (last_wr) way. Use the index to find the continuation possibilities on each iteration.
-    last_wr = current
-    ordered_way_ids = []
-    split_wrs = []
-    while True:
-      try:
-        # - Append current element to the route list of ordered way relations.
-        self._ordered_way_relations.append(last_wr)
-        ordered_way_ids.append(last_wr.id)
-
-        # - Get the id of the node at the end of the way and then fetch the way relations that share the end node id.
-        last_node_id = last_wr.last_node.id
-        way_relations = wr_index.way_relations_with_edge_node_id(last_node_id)
-
-        # - Add split way relations when necessary and remove parent way relations.
-        split_wrs_to_add = [wr for wr in split_wrs if last_node_id in wr.edge_nodes_ids]
-        way_relations.extend(split_wrs_to_add)
-        parent_ids = [wr.parent_wr_id for wr in split_wrs_to_add]
-        way_relations = [wr for wr in way_relations if wr.id not in parent_ids]
-
-        # - If no more way_relations than last_wr, we have to check if we join another wr on an internal node, and
-        # if we do, we replace such way relation with the split of it and continue.
-        if len(way_relations) == 1:
-          way_relations = wr_index.way_relations_with_node_id(last_node_id)
-          # If no more way_relations than last_wr or its parent, we got to the end.
-          if len(way_relations) == 1:
-            break
-
-          # If last_wr is a split, replace its parent with last_wr
-          way_relations = [last_wr if wr is last_wr.parent else wr for wr in way_relations]
-
-          # If we join a wr on an internal node, then we artificially split the wr in two and pass both wrs as
-          # candidates to the wr selection code below.
-          wr_to_split = [wr for wr in way_relations if wr is not last_wr][0]
-          next_split_way_id = -len(split_wrs) - 1  # Keep split wrs ids unique on Route
-          new_wrs = wr_to_split.split(last_node_id, [next_split_way_id, next_split_way_id - 1])
-          # If it could not be splited, we are done.
-          if len(new_wrs) != 2:
-            break
-
-          # Replace the original way relation for the splitted version on way_relations and track splited wrs.
-          split_wrs.extend(new_wrs)
-          way_relations.remove(wr_to_split)
-          way_relations.extend(new_wrs)
-
-        # - Get the coordinates for the edge node and build the array of coordinates for the nodes before the edge node
-        # on each of the common way relations, then get the vectors in cartesian plane for the end sections of each way.
-        ref_point = last_wr.last_node_coordinates
-        points = np.array([wr.node_before_edge_coordinates(last_node_id) for wr in way_relations])
-        v = ref_vectors(ref_point, points) * R
-
-        # - Calculate the bearing (from true north clockwise) for every end section of each way.
-        b = np.arctan2(v[:, 0], v[:, 1])
-
-        # - Find index of las_wr section and calculate deltas of bearings to the other sections.
-        last_wr_idx = way_relations.index(last_wr)
-        b_ref = b[last_wr_idx]
-        delta = b - b_ref
-
-        # - Update the direction of the possible route continuation ways as starting from last_node_id.
-        # Make sure to exclude any ways already included in the ordered list as to not modify direction when there
-        # are looping roads (like roundabouts). A way will never be included twice in a route anyway.
-        for wr in way_relations:
-          if wr.id not in ordered_way_ids:
-            wr.update_direction_from_starting_node(last_node_id)
-
-        # - Filter the possible route continuation way relations:
-        #   - exclude any way already added to the ordered list.
-        #   - exclude all way relations that are prohibited due to traffic direction.
-        mask = [wr.id not in ordered_way_ids and not wr.is_prohibited for wr in way_relations]
-        way_relations = list(compress(way_relations, mask))
-        delta = delta[mask]
-
-        # if no options left, we got to the end.
-        if len(way_relations) == 0:
-          break
-
-        # - The cosine of the bearing delta will aid us in choosing the way that continues. The cosine is
-        # minimum (-1) for a perfect straight continuation as delta would be pi or -pi.
-        cos_delta = np.cos(delta)
-
-        def pick_best_idx(cos_delta):
-          """Selects the best index on `cos_delta` array for a way that continues the route.
-          In principle we want to choose the way that continues as straight as possible.
-          Bue we need to make sure that if there are 2 or more ways continuing relatively straight, then we
-          need to disambiguate, either by matching the `ref` or `name` value of the continuing way with the
-          last way selected.
-          This can prevent cases where the chosen route could be for instance an exit ramp of a way due to the fact
-          that the ramp has a better match on bearing to previous way. We choose to stay on the road with the same `ref`
-          or `name` value if available.
-          If there is no ambiguity or there are no `name` or `ref` values to disambiguate, then we pick the one with
-          the straightest following direction.
-          """
-          # Find the indexes of the cosine of the deltas that are considered straight enough to continue.
-          idxs = np.nonzero(cos_delta < _ACCEPTABLE_BEARING_DELTA_COSINE)[0]
-
-          # If no amiguity or no way to break it, just return the straightest line.
-          if len(idxs) <= 1 or (last_wr.ref is None and last_wr.name is None):
-            # The section with the best continuation is the one with a bearing delta closest to pi. This is equivalent
-            # to taking the one with the smallest cosine of the bearing delta, as cosine is minimum (-1) on both pi
-            # and -pi.
-            return np.argmin(cos_delta)
-
-          wrs = [way_relations[idx] for idx in idxs]
-
-          # If we find a continuation way with the same reference we just choose it.
-          refs = list(map(lambda wr: wr.ref, wrs))
-          if last_wr.ref is not None:
-            idx = next((idx for idx, ref in enumerate(refs) if ref == last_wr.ref), None)
-            if idx is not None:
-              return idxs[idx]
-
-          # If we find a continuation way with the same name we just choose it.
-          names = list(map(lambda wr: wr.name, wrs))
-          if last_wr.name is not None:
-            idx = next((idx for idx, name in enumerate(names) if name == last_wr.name), None)
-            if idx is not None:
-              return idxs[idx]
-
-          # We did not manage to deambiguate, choose straightest path.
-          return np.argmin(cos_delta)
-
-        # Get the index of the continuation way.
-        best_idx = pick_best_idx(cos_delta)
-
-        # - Make sure to not select as route continuation a way that turns too much if we are close to the border of
-        # map data queried. This is to avoid building a route that takes a sharp turn just because we do not have the
-        # data for the way that actually continues straight.
-        if cos_delta[best_idx] > _MAX_ALLOWED_BEARING_DELTA_COSINE_AT_EDGE:
-          dist_to_center = distance_to_points(query_center, np.array([ref_point]))[0]
-          if dist_to_center > QUERY_RADIUS - _MAP_DATA_EDGE_DISTANCE:
-            break
-
-        # - Select next way.
-        last_wr = way_relations[best_idx]
-      except Exception as e:
-        print("Exception \"", str(e), "\" caught")
-
-    # Build the node data from the ordered list of way relations
-    self._nodes_data = NodesData(self._ordered_way_relations, wr_index)
-
-    # Locate where we are in the route node list.
-    self._locate()
-
-  def __repr__(self):
-    count = self._nodes_data.count if self._nodes_data is not None else None
-    return f'Route: {self.way_collection_id}, idx ahead: {self._ahead_idx} of {count}'
-
-  def _reset(self):
-    self._limits_ahead = None
-    self._cuvature_limits_ahead = None
-    self._curvatures_ahead = None
-    self._ahead_idx = None
-    self._distance_to_node_ahead = None
-
-  @property
-  def located(self):
-    return self._ahead_idx is not None
-
-  def _locate(self):
-    """Will resolve the index in the nodes_data list for the node ahead of the current location.
-       It updates as well the distance from the current location to the node ahead.
-    """
-    current = self.current_wr
-    if current is None:
-      return
-
-    node_ahead_id = current.node_ahead.id
-    self._distance_to_node_ahead = current.distance_to_node_ahead
-    start_idx = self._ahead_idx if self._ahead_idx is not None else 1
-    self._ahead_idx = None
-
-    ids = self._nodes_data.get(NodeDataIdx.node_id)
-    for idx in range(start_idx, len(ids)):
-      if ids[idx] == node_ahead_id:
-        self._ahead_idx = idx
-        break
-
-  @property
-  def current_wr(self):
-    return self._ordered_way_relations[0] if len(self._ordered_way_relations) else None
-
-  def update(self, location_rad, bearing_rad, location_stdev):
-    """Will update the route structure based on the given `location_rad` and `bearing_rad` assuming progress on the
-    route on the original direction. If direction has changed or active point on the route can not be found, the route
-    will become invalid.
-    """
-    if len(self._ordered_way_relations) == 0 or location_rad is None or bearing_rad is None:
-      return
-
-    # Skip if no update on location or bearing.
-    if np.array_equal(self.current_wr.location_rad, location_rad) and self.current_wr.bearing_rad == bearing_rad:
-      return
-
-    # Transverse the way relations on the actual order until we find an active one. From there, rebuild the route
-    # with the way relations remaining ahead.
-    for idx, wr in enumerate(self._ordered_way_relations):
-      active_direction = wr.direction
-      wr.update(location_rad, bearing_rad, location_stdev)
-
-      if not wr.active:
-        continue
-
-      if wr.direction != active_direction:
-        # Driving direction on the route has changed. stop.
-        break
-
-      # We have now the current wr. Repopulate from here till the end and locate
-      self._ordered_way_relations = self._ordered_way_relations[idx:]
-      self._reset()
-      self._locate()
-
-      # If the active way is diverting, check whether there are possibilities to divert from the route in the
-      # vecinity of the current location. If there are possibilities, then stop here to loose the route as we are
-      # most likely driving away. If there are no possibilities, then stick to the route as the diversion is probably
-      # just a matter of GPS accuracy. (It can happen after driving under a bridge)
-      if wr.diverting and len(self._nodes_data.possible_divertions(self._ahead_idx, self._distance_to_node_ahead)) > 0:
-        break
-
-      # The current location in route is valid, return.
-      return
-
-    # if we got here, there is no new active way relation or driving direction has changed. Reset.
-    self._reset()
-
-  @property
-  def speed_limits_ahead(self):
-    """Returns and array of SpeedLimitSection objects for the actual route ahead of current location
-    """
-    if self._limits_ahead is not None:
-      return self._limits_ahead
-
-    if self._nodes_data is None or self._ahead_idx is None:
-      return []
-
-    self._limits_ahead = self._nodes_data.speed_limits_ahead(self._ahead_idx, self._distance_to_node_ahead)
-    return self._limits_ahead
-
-  @property
-  def curvature_speed_limits_ahead(self):
-    """Returns and array of TurnSpeedLimitSection objects for the actual route ahead of current location due
-    to curvatures
-    """
-    if self._cuvature_limits_ahead is not None:
-      return self._cuvature_limits_ahead
-
-    if self._nodes_data is None or self._ahead_idx is None:
-      return []
-
-    self._cuvature_limits_ahead = self._nodes_data. \
-      curvatures_speed_limit_sections_ahead(self._ahead_idx, self._distance_to_node_ahead)
-
-    return self._cuvature_limits_ahead
-
-  @property
-  def current_speed_limit(self):
-    if not self.located:
-      return None
-
-    limits_ahead = self.speed_limits_ahead
-    if len(limits_ahead) == 0 or limits_ahead[0].start != 0:
-      return None
-
-    return limits_ahead[0].value
-
-  @property
-  def current_curvature_speed_limit_section(self):
-    if not self.located:
-      return None
-
-    limits_ahead = self.curvature_speed_limits_ahead
-    if len(limits_ahead) == 0 or limits_ahead[0].start != 0:
-      return None
-
-    return limits_ahead[0]
-
-  @property
-  def next_speed_limit_section(self):
-    if not self.located:
-      return None
-
-    limits_ahead = self.speed_limits_ahead
-    if len(limits_ahead) == 0:
-      return None
-
-    # Find the first section that does not start in 0. i.e. the next section
-    for section in limits_ahead:
-      if section.start > 0:
-        return section
-
-    return None
-
-  def next_curvature_speed_limit_sections(self, horizon_mts):
-    if not self.located:
-      return []
-
-    # Provide the curvature speed sections that start ahead (> 0) and up to horizon
-    return list(filter(lambda la: la.start > 0 and la.start <= horizon_mts, self.curvature_speed_limits_ahead))
-
-  @property
-  def distance_to_end(self):
-    if not self.located:
-      return None
-
-    return self._nodes_data.distance_to_end(self._ahead_idx, self._distance_to_node_ahead)
-
-  @property
-  def current_road_name(self):
-    return self.current_wr.road_name if self.located else None
@@ -1,85 +0,0 @@
-from selfdrive.mapd.lib.WayRelation import WayRelation
-from selfdrive.mapd.lib.WayRelationIndex import WayRelationIndex
-from selfdrive.mapd.lib.Route import Route
-from selfdrive.mapd.config import LANE_WIDTH
-import uuid
-
-
-_ACCEPTABLE_BEARING_DELTA_IND = 0.7071067811865475  # sin(pi/4) | 45 degrees acceptable bearing delta
-
-
-class WayCollection():
-  """A collection of WayRelations to use for maps data analysis.
-  """
-  def __init__(self, areas, ways, query_center):
-    """Creates a WayCollection with a set of OSM way objects.
-
-    Args:
-        ways (Array): Collection of Way objects fetched from OSM in a radius around `query_center`
-        query_center (Numpy Array): [lat, lon] numpy array in radians indicating the center of the data query.
-    """
-    self.id = uuid.uuid4()
-    self.way_relations = [WayRelation(areas, way) for way in ways]
-    self.query_center = query_center
-
-    self.wr_index = WayRelationIndex(self.way_relations)
-
-  def get_route(self, location_rad, bearing_rad, location_stdev):
-    """Provides the best route found in the way collection based on current location and bearing.
-    """
-    if location_rad is None or bearing_rad is None or location_stdev is None:
-      return None
-
-    # Update all way relations in collection to the provided location and bearing.
-    for wr in self.way_relations:
-      wr.update(location_rad, bearing_rad, location_stdev)
-
-    # Get the way relations where a match was found. i.e. those now marked as active as long as the direction of
-    # travel is valid.
-    valid_way_relations = [wr for wr in self.way_relations if wr.active and not wr.is_prohibited]
-
-    # If no active, then we could not find a current way to build a route.
-    if len(valid_way_relations) == 0:
-      return None
-
-    # If only one valid, then pick it as current.
-    if len(valid_way_relations) == 1:
-      current = valid_way_relations[0]
-
-    # If more than one is valid, filter out any valid way relation where the bearing delta indicator is too high.
-    else:
-      wr_acceptable_bearing = list(filter(lambda wr: wr.active_bearing_delta <= _ACCEPTABLE_BEARING_DELTA_IND,
-                                          valid_way_relations))
-
-      # If delta bearing indicator is too high for all, then use as current the one that has the shorter one.
-      if len(wr_acceptable_bearing) == 0:
-        valid_way_relations.sort(key=lambda wr: wr.active_bearing_delta)
-        current = valid_way_relations[0]
-
-      # If only one with acceptable bearing, use it.
-      elif len(wr_acceptable_bearing) == 1:
-        current = wr_acceptable_bearing[0]
-
-      else:
-        # If more than one with acceptable bearing, filter the ones with distance to way lower than 2 standard
-        # deviation from GPS accuracy (95%) + half the road width estimate.
-        wr_accurate_distance = [wr for wr in wr_acceptable_bearing
-                                if wr.distance_to_way <= 2. * location_stdev + wr.lanes * LANE_WIDTH / 2.]
-
-        # If none with accurate distance to way, then select the closest to the way
-        if len(wr_accurate_distance) == 0:
-          wr_acceptable_bearing.sort(key=lambda wr: wr.distance_to_way)
-          current = wr_acceptable_bearing[0]
-
-        # If only one with distance under accuracy, select this one.
-        elif len(wr_accurate_distance) == 1:
-          current = wr_accurate_distance[0]
-
-        # If more than one with distance under accuracy. Then select the one with lowest highway rank.
-        # i.e. preferred motorways over other roads and so on. This is to prevent selecting a small parallel
-        # road to a main road when the accuracy is poor.
-        else:
-          wr_accurate_distance.sort(key=lambda wr: wr.highway_rank)
-          current = wr_accurate_distance[0]
-
-    return Route(current, self.wr_index, self.id, self.query_center)
@@ -1,487 +0,0 @@
-from selfdrive.mapd.lib.geo import DIRECTION, R, vectors, bearing_to_points, distance_to_points, point_on_line
-from selfdrive.mapd.lib.osm import create_way
-from common.conversions import Conversions as CV
-from selfdrive.mapd.config import LANE_WIDTH
-from common.basedir import BASEDIR
-from datetime import datetime as dt
-import numpy as np
-import re
-import json
-
-
-_WAY_BBOX_PADING = 80. / R  # 80 mts of padding to bounding box. (expressed in radians)
-
-with open(BASEDIR + "/selfdrive/mapd/lib/default_speeds_by_region.json", "rb") as f:
-  DEFAULT_SPEEDS_BY_REGION = json.loads(f.read())
-
-with open(BASEDIR + "/selfdrive/mapd/lib/default_speeds.json", "rb") as f:
-  _COUNTRY_LIMITS = json.loads(f.read())
-
-
-_WD = {
-  'Mo': 0,
-  'Tu': 1,
-  'We': 2,
-  'Th': 3,
-  'Fr': 4,
-  'Sa': 5,
-  'Su': 6
-}
-
-_HIGHWAY_RANK = {
-  'motorway': 0,
-  'motorway_link': 1,
-  'trunk': 10,
-  'trunk_link': 11,
-  'primary': 20,
-  'primary_link': 21,
-  'secondary': 30,
-  'secondary_link': 31,
-  'tertiary': 40,
-  'tertiary_link': 41,
-  'unclassified': 50,
-  'residential': 60,
-  'living_street': 61,
-  'service': 62
-}
-
-
-def is_osm_time_condition_active(condition_string):
-  """
-  Will indicate if a time condition for a restriction as described
-  @ https://wiki.openstreetmap.org/wiki/Conditional_restrictions
-  is active for the current date and time of day.
-  """
-  now = dt.now().astimezone()
-  today = now.date()
-  week_days = []
-
-  # Look for days of week matched and validate if today matches criteria.
-  dr = re.findall(r'(Mo|Tu|We|Th|Fr|Sa|Su[-,\s]*?)', condition_string)
-
-  if len(dr) == 1:
-    week_days = [_WD[dr[0]]]
-  # If two or more matches condider it a range of days between 1st and 2nd element.
-  elif len(dr) > 1:
-    week_days = list(range(_WD[dr[0]], _WD[dr[1]] + 1))
-
-  # If valid week days list is not empty and today day is not in the list, then the time-date range is not active.
-  if len(week_days) > 0 and now.weekday() not in week_days:
-    return False
-
-  # Look for time ranges on the day. No time range, means all day
-  tr = re.findall(r'([0-9]{1,2}:[0-9]{2})\s*?-\s*?([0-9]{1,2}:[0-9]{2})', condition_string)
-
-  # if no time range but there were week days set, consider it active during the whole day
-  if len(tr) == 0:
-    return len(dr) > 0
-
-  # Search among time ranges matched, one where now time belongs too. If found range is active.
-  for times_tup in tr:
-    times = list(map(lambda tt: dt.
-                 combine(today, dt.strptime(tt, '%H:%M').time().replace(tzinfo=now.tzinfo)), times_tup))
-    if now >= times[0] and now <= times[1]:
-      return True
-
-  return False
-
-
-def speed_limit_value_for_limit_string(limit_string):
-  # Look for matches of speed by default in kph, or in mph when explicitly noted.
-  v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', limit_string)
-  if v is None:
-    return None
-  conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS
-  return conv * float(v[1])
-
-
-def speed_limit_for_osm_tag_limit_string(limit_string):
-  # https://wiki.openstreetmap.org/wiki/Key:maxspeed
-  if limit_string is None:
-    # When limit is set to 0. is considered not existing.
-    return 0.
-
-  # Attempt to parse limit as simple numeric value considering units.
-  limit = speed_limit_value_for_limit_string(limit_string)
-  if limit is not None:
-    return limit
-
-  # Look for matches of speed with country implicit values.
-  v = re.match(r'^\s*([A-Z]{2}):([a-z_]+):?([0-9]{1,3})?(\s+)?(mph)?\s*', limit_string)
-  if v is None:
-    return 0.
-
-  if v[2] == "zone" and v[3] is not None:
-    conv = CV.MPH_TO_MS if v[5] is not None and v[5] == "mph" else CV.KPH_TO_MS
-    limit = conv * float(v[3])
-  elif f'{v[1]}:{v[2]}' in _COUNTRY_LIMITS:
-    limit = speed_limit_value_for_limit_string(_COUNTRY_LIMITS[f'{v[1]}:{v[2]}'])
-
-  return limit if limit is not None else 0.
-
-
-def conditional_speed_limit_for_osm_tag_limit_string(limit_string):
-  if limit_string is None:
-    # When limit is set to 0. is considered not existing.
-    return 0.
-
-  # Look for matches of the `<restriction-value> @ (<condition>)` format
-  v = re.match(r'^(.*)@\s*\((.*)\).*$', limit_string)
-  if v is None:
-    return 0.  # No valid format match
-
-  value = speed_limit_for_osm_tag_limit_string(v[1])
-  if value == 0.:
-    return 0.  # Invalid speed limit value
-
-  # Look for date-time conditions separated by semicolon
-  v = re.findall(r'(?:;|^)([^;]*)', v[2])
-  for datetime_condition in v:
-    if is_osm_time_condition_active(datetime_condition):
-      return value
-
-  # If we get here, no current date-time condition is active.
-  return 0.
-
-def speed_limit_value_for_highway_type(areas, tags):
-  max_speed = None
-  try:
-    geocode_country = ''
-    geocode_region = ''
-    for area in areas:
-      if area.tags.get('admin_level', '') == "2":
-        if area.tags.get('ISO3166-1:alpha2', '') != '':
-          geocode_country = area.tags.get('ISO3166-1:alpha2', '')
-      elif area.tags.get('admin_level', '') == "4":
-        geocode_region = area.tags.get('name', '')
-    country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {})
-    country_defaults = country_rules.get('Default', [])
-    for rule in country_defaults:
-      rule_valid = all(
-        tag_name in tags
-        and tags[tag_name] == value
-        for tag_name, value in rule['tags'].items()
-      )
-      if rule_valid:
-        max_speed = rule['speed']
-        break #stop searching country
-
-    region_rules = country_rules.get(geocode_region, [])
-    for rule in region_rules:
-      rule_valid = all(
-        tag_name in tags
-        and tags[tag_name] == value
-        for tag_name, value in rule['tags'].items()
-      )
-      if rule_valid:
-        max_speed = rule['speed']
-        break #stop searching region
-  except KeyError as e:
-    print(e)
-  except TypeError as e:
-    print(f"TypeError: {e} object is not iterable.")
-  if max_speed is None:
-    return 0
-  v = re.match(r'^\s*([0-9]{1,3})\s*?(mph)?\s*$', str(max_speed))
-  if v is None:
-    return None
-  conv = CV.MPH_TO_MS if v[2] is not None and v[2] == "mph" else CV.KPH_TO_MS
-  return conv * float(v[1])
-
-
-class WayRelation():
-  """A class that represent the relationship of an OSM way and a given `location` and `bearing` of a driving vehicle.
-  """
-  def __init__(self, areas, way, parent=None):
-    self.way = way
-    self.areas = areas
-    self.parent = parent
-    self.parent_wr_id = parent.id if parent is not None else None  # For WRs created as splits of other WRs
-    self.reset_location_variables()
-    self.direction = DIRECTION.NONE
-    self._speed_limit = None
-    self._advisory_speed_limit = None
-    self._one_way = way.tags.get("oneway")
-    self.name = way.tags.get('name')
-    self.ref = way.tags.get('ref')
-    self.highway_type = way.tags.get("highway")
-    self.highway_rank = _HIGHWAY_RANK.get(self.highway_type, 1000)
-    try:
-      self.lanes = int(way.tags.get('lanes'))
-    except Exception:
-      self.lanes = 2
-
-    # Create numpy arrays with nodes data to support calculations.
-    self._nodes_np = np.radians(np.array([[nd.lat, nd.lon] for nd in way.nodes], dtype=float))
-    self._nodes_ids = np.array([nd.id for nd in way .nodes], dtype=int)
-
-    # Get the vectors representation of the segments betwheen consecutive nodes. (N-1, 2)
-    v = vectors(self._nodes_np) * R
-
-    # Calculate the vector magnitudes (or distance) between nodes. (N-1)
-    self._way_distances = np.linalg.norm(v, axis=1)
-
-    # Calculate the bearing (from true north clockwise) for every section of the way (vectors between nodes). (N-1)
-    self._way_bearings = np.arctan2(v[:, 0], v[:, 1])
-
-    # Define bounding box to ease the process of locating a node in a way.
-    # [[min_lat, min_lon], [max_lat, max_lon]]
-    self.bbox = np.row_stack((np.amin(self._nodes_np, 0) - _WAY_BBOX_PADING,
-                              np.amax(self._nodes_np, 0) + _WAY_BBOX_PADING))
-
-    # Get the edge nodes ids.
-    self.edge_nodes_ids = [way.nodes[0].id, way.nodes[-1].id]
-
-  def __repr__(self):
-    return f'(id: {self.id}, between {self.behind_idx} and {self.ahead_idx}, {self.direction}, active: {self.active})'
-
-  def __eq__(self, other):
-    if isinstance(other, WayRelation):
-      return self.id == other.id
-    return False
-
-  def reset_location_variables(self):
-    self.distance_to_node_ahead = 0.
-    self.location_rad = None
-    self.bearing_rad = None
-    self.active = False
-    self.diverting = False
-    self.ahead_idx = None
-    self.behind_idx = None
-    self._active_bearing_delta = None
-    self._distance_to_way = None
-
-  @property
-  def id(self):
-    return self.way.id
-
-  @property
-  def road_name(self):
-    if self.name is not None:
-      return self.name
-    return self.ref
-
-  def update(self, location_rad, bearing_rad, location_stdev):
-    """Will update and validate the associated way with a given `location_rad` and `bearing_rad`.
-       Specifically it will find the nodes behind and ahead of the current location and bearing.
-       If no proper fit to the way geometry, the way relation is marked as invalid.
-    """
-    self.reset_location_variables()
-
-    # Ignore if location not in way bounding box
-    if not self.is_location_in_bbox(location_rad):
-      return
-
-    # - Get the distance and bearings from location to all nodes. (N)
-    bearings = bearing_to_points(location_rad, self._nodes_np)
-
-    # - Get absolute bearing delta to current driving bearing. (N)
-    delta = np.abs(bearing_rad - bearings)
-
-    # - Nodes are ahead if the cosine of the delta is positive (N)
-    is_ahead = np.cos(delta) >= 0.
-
-    # - Possible locations on the way are those where adjacent nodes change from ahead to behind or vice-versa.
-    possible_idxs = np.nonzero(np.diff(is_ahead))[0]
-
-    # - when no possible locations found, then the location is not in this way.
-    if len(possible_idxs) == 0:
-      return
-
-    projections = point_on_line(self._nodes_np[:-1], self._nodes_np[1:], location_rad)
-    h = distance_to_points(location_rad, projections)
-
-    # - Calculate the delta between driving bearing and way bearings. (N-1)
-    bw_delta = self._way_bearings - bearing_rad
-
-    # - The absolute value of the sin of `bw_delta` indicates how close the bearings match independent of direction.
-    # We will use this value along the distance to the way to aid on way selection. (N-1)
-    abs_sin_bw_delta = np.abs(np.sin(bw_delta))
-
-    # - Get the delta to way bearing indicators and the distance to the way for the possible locations.
-    abs_sin_bw_delta_possible = abs_sin_bw_delta[possible_idxs]
-    h_possible = h[possible_idxs]
-
-    # - Get the index where the distance to the way is minimum. That is the chosen location.
-    min_h_possible_idx = np.argmin(h_possible)
-    min_delta_idx = possible_idxs[min_h_possible_idx]
-    projection = projections[min_delta_idx]
-
-    # - If the distance to the way is over 4 standard deviations of the gps accuracy + the maximum road width
-    # estimate, then we are way too far to stick to this way (i.e. we are not on this way anymore)
-    # In theory the osm path is centered on the road which means half the road width would cover the whole road.
-    # however, often times the osm path is not perfectly centered so we'll make the possible route more lenient by using
-    # the full road width.
-    road_width_estimate = self.lanes * LANE_WIDTH
-    half_road_width_estimate = road_width_estimate / 2.
-    if h_possible[min_h_possible_idx] > 4. * location_stdev + road_width_estimate:
-      return
-
-    # If the distance to the road is greater than 2 standard deviations of the gps accuracy + half the maximum road
-    # width estimate + 1 lane width then we are most likely diverting from this route. Adding a lane width to give
-    # leniency to not perfectly centered osm paths
-    diverting = h_possible[min_h_possible_idx] > 2. * location_stdev + half_road_width_estimate + LANE_WIDTH
-
-    # Populate location variables with result
-    if is_ahead[min_delta_idx]:
-      self.direction = DIRECTION.BACKWARD
-      self.ahead_idx = min_delta_idx
-      self.behind_idx = min_delta_idx + 1
-    else:
-      self.direction = DIRECTION.FORWARD
-      self.ahead_idx = min_delta_idx + 1
-      self.behind_idx = min_delta_idx
-
-    self._distance_to_way = h[min_delta_idx]
-    self._active_bearing_delta = abs_sin_bw_delta_possible[min_h_possible_idx]
-
-    # find the distance to the next node by projecting our location onto the line and finding the delta between that
-    # point and the next point on the route
-    self.distance_to_node_ahead = distance_to_points(projection, np.array([self._nodes_np[self.ahead_idx]]))[0]
-    self.active = True
-    self.diverting = diverting
-    self.location_rad = location_rad
-    self.bearing_rad = bearing_rad
-    self._speed_limit = None
-    self._advisory_speed_limit = None
-
-  def update_direction_from_starting_node(self, start_node_id):
-    self._speed_limit = None
-    self._advisory_speed_limit = None
-    if self.edge_nodes_ids[0] == start_node_id:
-      self.direction = DIRECTION.FORWARD
-    elif self.edge_nodes_ids[-1] == start_node_id:
-      self.direction = DIRECTION.BACKWARD
-    else:
-      self.direction = DIRECTION.NONE
-
-  def is_location_in_bbox(self, location_rad):
-    """Indicates if a given location is contained in the bounding box surrounding the way.
-       self.bbox = [[min_lat, min_lon], [max_lat, max_lon]]
-    """
-    is_g = np.greater_equal(location_rad, self.bbox[0, :])
-    is_l = np.less_equal(location_rad, self.bbox[1, :])
-
-    return np.all(np.concatenate((is_g, is_l)))
-
-  @property
-  def speed_limit(self):
-    if self._speed_limit is not None:
-      return self._speed_limit
-
-    # Get string from corresponding tag, consider conditional limits first.
-    limit_string = self.way.tags.get("maxspeed:conditional")
-    if limit_string is None:
-      if self.direction == DIRECTION.FORWARD:
-        limit_string = self.way.tags.get("maxspeed:forward:conditional")
-      elif self.direction == DIRECTION.BACKWARD:
-        limit_string = self.way.tags.get("maxspeed:backward:conditional")
-
-    limit = conditional_speed_limit_for_osm_tag_limit_string(limit_string)
-
-    # When no conditional limit set, attempt to get from regular speed limit tags.
-    if limit == 0.:
-      limit_string = self.way.tags.get("maxspeed")
-      if limit_string is None:
-        if self.direction == DIRECTION.FORWARD:
-          limit_string = self.way.tags.get("maxspeed:forward")
-        elif self.direction == DIRECTION.BACKWARD:
-          limit_string = self.way.tags.get("maxspeed:backward")
-
-      limit = speed_limit_for_osm_tag_limit_string(limit_string)
-
-    if limit == 0.:
-      limit = speed_limit_value_for_highway_type(self.areas, self.way.tags)
-
-    self._speed_limit = limit
-    return self._speed_limit
-
-
-  @property
-  def advisory_speed_limit(self):
-    if self._advisory_speed_limit is not None:
-      return self._advisory_speed_limit
-
-    limit_string = self.way.tags.get("maxspeed:advisory")
-    limit = speed_limit_for_osm_tag_limit_string(limit_string)
-
-    self._advisory_speed_limit = limit
-    return self._advisory_speed_limit
-
-
-  @property
-  def active_bearing_delta(self):
-    """Returns the sine of the delta between the current location bearing and the exact
-       bearing of the portion of way we are currentluy located at.
-    """
-    return self._active_bearing_delta
-
-  @property
-  def is_one_way(self):
-    return self._one_way in ['yes'] or self.highway_type in ["motorway"]
-
-  @property
-  def is_prohibited(self):
-    # Direction must be defined to asses this property. Default to `True` if not.
-    if self.direction == DIRECTION.NONE:
-      return True
-    return self.is_one_way and self.direction == DIRECTION.BACKWARD
-
-  @property
-  def distance_to_way(self):
-    """Returns the perpendicular (i.e. minimum) distance between current location and the way
-    """
-    return self._distance_to_way
-
-  @property
-  def node_ahead(self):
-    return self.way.nodes[self.ahead_idx] if self.ahead_idx is not None else None
-
-  @property
-  def last_node(self):
-    """Returns the last node on the way considering the traveling direction
-    """
-    if self.direction == DIRECTION.FORWARD:
-      return self.way.nodes[-1]
-    if self.direction == DIRECTION.BACKWARD:
-      return self.way.nodes[0]
-    return None
-
-  @property
-  def last_node_coordinates(self):
-    """Returns the coordinates for the last node on the way considering the traveling direction. (in radians)
-    """
-    if self.direction == DIRECTION.FORWARD:
-      return self._nodes_np[-1]
-    if self.direction == DIRECTION.BACKWARD:
-      return self._nodes_np[0]
-    return None
-
-  def node_before_edge_coordinates(self, node_id):
-    """Returns the coordinates of the node before the edge node identifeid with `node_id`. (in radians)
-    """
-    if self.edge_nodes_ids[0] == node_id:
-      return self._nodes_np[1]
-
-    if self.edge_nodes_ids[-1] == node_id:
-      return self._nodes_np[-2]
-
-    return np.array([0., 0.])
-
-  def split(self, node_id, way_ids=None):
-    """ Returns and array with the way relations resulting from splitting the current way relation at node_id
-    """
-    idxs = np.nonzero(self._nodes_ids == node_id)[0]
-    if len(idxs) == 0:
-      return []
-
-    idx = idxs[0]
-    if idx == 0 or idx == len(self._nodes_ids) - 1:
-      return [self]
-
-    if not isinstance(way_ids, list):
-      way_ids = [-1, -2]  # Default id values.
-
-    ways = [create_way(way_ids[0], node_ids=self._nodes_ids[:idx + 1], from_way=self.way),
-            create_way(way_ids[1], node_ids=self._nodes_ids[idx:], from_way=self.way)]
-    return [WayRelation(self.areas, way, parent=self) for way in ways]
@@ -1,34 +0,0 @@
-
-
-class WayRelationIndex():
-  """
-  A class containing an index of WayRelations by node ids of internal nodes and edge nodes.
-  """
-  def __init__(self, way_relations):
-    self._edge_nodes_index_dict = {}
-    self._full_nodes_index_dict = {}
-
-    for wr in way_relations:
-      self.add(wr)
-
-  def add(self, way_relation):
-    for node in way_relation.way.nodes:
-      node_id = node.id
-      self._full_nodes_index_dict[node_id] = self._full_nodes_index_dict.get(node_id, []) + [way_relation]
-      if node_id in way_relation.edge_nodes_ids:
-        self._edge_nodes_index_dict[node_id] = self._edge_nodes_index_dict.get(node_id, []) + [way_relation]
-
-  def remove(self, way_relation):
-    for node in way_relation.way.nodes:
-      node_id = node.id
-      self._full_nodes_index_dict[node_id] = [wr for wr in self._full_nodes_index_dict.get(node_id, [])
-                                              if wr is not way_relation]
-      if node_id in way_relation.edge_nodes_ids:
-        self._edge_nodes_index_dict[node_id] = [wr for wr in self._edge_nodes_index_dict.get(node_id, [])
-                                                if wr is not way_relation]
-
-  def way_relations_with_edge_node_id(self, node_id):
-    return self._edge_nodes_index_dict.get(node_id, [])
-
-  def way_relations_with_node_id(self, node_id):
-    return self._full_nodes_index_dict.get(node_id, [])
@@ -1,112 +0,0 @@
-{
-  "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits  Special cases have been stripped",
-  "AR:urban": "40",
-  "AR:urban:primary": "60",
-  "AR:urban:secondary": "60",
-  "AR:rural": "110",
-  "AT:urban": "50",
-  "AT:rural": "100",
-  "AT:trunk": "100",
-  "AT:motorway": "130",
-  "BE:urban": "50",
-  "BE-VLG:rural": "70",
-  "BE-WAL:rural": "90",
-  "BE:trunk": "120",
-  "BE:motorway": "120",
-  "CH:urban[1]": "50",
-  "CH:rural": "80",
-  "CH:trunk": "100",
-  "CH:motorway": "120",
-  "CZ:pedestrian_zone": "20",
-  "CZ:living_street": "20",
-  "CZ:urban": "50",
-  "CZ:urban_trunk": "80",
-  "CZ:urban_motorway": "80",
-  "CZ:rural": "90",
-  "CZ:trunk": "110",
-  "CZ:motorway": "130",
-  "DK:urban": "50",
-  "DK:rural": "80",
-  "DK:motorway": "130",
-  "DE:living_street": "10",
-  "DE:service": "10",
-  "DE:residential": "30",
-  "DE:urban": "50",
-  "DE:rural": "100",
-  "DE:trunk": "none",
-  "DE:motorway": "none",
-  "FI:urban": "50",
-  "FI:rural": "80",
-  "FI:trunk": "100",
-  "FI:motorway": "120",
-  "FR:urban": "50",
-  "FR:rural": "80",
-  "FR:trunk": "110",
-  "FR:motorway": "130",
-  "GR:urban": "50",
-  "GR:rural": "90",
-  "GR:trunk": "110",
-  "GR:motorway": "130",
-  "HU:urban": "50",
-  "HU:rural": "90",
-  "HU:trunk": "110",
-  "HU:motorway": "130",
-  "IT:urban": "50",
-  "IT:rural": "90",
-  "IT:trunk": "110",
-  "IT:motorway": "130",
-  "JP:national": "60",
-  "JP:motorway": "100",
-  "LT:living_street": "20",
-  "LT:urban": "50",
-  "LT:rural": "90",
-  "LT:trunk": "120",
-  "LT:motorway": "130",
-  "PL:living_street": "20",
-  "PL:urban": "50",
-  "PL:rural": "90",
-  "PL:trunk": "100",
-  "PL:motorway": "140",
-  "RO:urban": "50",
-  "RO:rural": "90",
-  "RO:trunk": "100",
-  "RO:motorway": "130",
-  "RU:living_street": "20",
-  "RU:urban": "60",
-  "RU:rural": "90",
-  "RU:motorway": "110",
-  "SK:urban": "50",
-  "SK:rural": "90",
-  "SK:trunk": "90",
-  "SK:motorway": "90",
-  "SI:urban": "50",
-  "SI:rural": "90",
-  "SI:trunk": "110",
-  "SI:motorway": "130",
-  "ES:living_street": "20",
-  "ES:urban": "50",
-  "ES:rural": "50",
-  "ES:trunk": "90",
-  "ES:motorway": "120",
-  "SE:urban": "50",
-  "SE:rural": "70",
-  "SE:trunk": "90",
-  "SE:motorway": "110",
-  "GB:nsl_restricted": "30 mph",
-  "GB:nsl_single": "60 mph",
-  "GB:nsl_dual": "70 mph",
-  "GB:motorway": "70 mph",
-  "UA:urban": "50",
-  "UA:rural": "90",
-  "UA:trunk": "110",
-  "UA:motorway": "130",
-  "UZ:living_street": "30",
-  "UZ:urban": "70",
-  "UZ:rural": "100",
-  "UZ:motorway": "110",
-  "ZA:trunk": "120",
-  "ZA:residential": "60",
-  "ZA:rural": "100",
-  "ZA:urban": "60",
-  "ZA:motorway": "120"
-}
@@ -1,624 +0,0 @@
-{
-  "AU": {
-    "Default": [
-      {
-        "speed": "100",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "trunk"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "primary"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "secondary"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "tertiary"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "unclassified"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "residential"
-        }
-      },
-      {
-        "speed": "40",
-        "tags": {
-          "highway": "service"
-        }
-      },
-      {
-        "speed": "90",
-        "tags": {
-          "highway": "motorway_link"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "trunk_link"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "primary_link"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "secondary_link"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "tertiary_link"
-        }
-      },
-      {
-        "speed": "30",
-        "tags": {
-          "highway": "living_street"
-        }
-      }
-    ]
-  },
-  "EE": {
-    "Default": [
-      {
-        "speed": "90",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "90",
-        "tags": {
-          "highway": "trunk"
-        }
-      },
-      {
-        "speed": "90",
-        "tags": {
-          "highway": "primary"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "secondary"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "tertiary"
-        }
-      },
-      {
-        "speed": "90",
-        "tags": {
-          "highway": "unclassified"
-        }
-      },
-      {
-        "speed": "40",
-        "tags": {
-          "highway": "residential"
-        }
-      },
-      {
-        "speed": "40",
-        "tags": {
-          "highway": "service"
-        }
-      },
-      {
-        "speed": "90",
-        "tags": {
-          "highway": "motorway_link"
-        }
-      },
-      {
-        "speed": "70",
-        "tags": {
-          "highway": "trunk_link"
-        }
-      },
-      {
-        "speed": "70",
-        "tags": {
-          "highway": "primary_link"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "secondary_link"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "tertiary_link"
-        }
-      },
-      {
-        "speed": "20",
-        "tags": {
-          "highway": "living_street"
-        }
-      }
-    ]
-  },
-  "CA": {
-    "Default": [
-      {
-        "speed": "100",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "trunk"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "primary"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "secondary"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "tertiary"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "unclassified"
-        }
-      },
-      {
-        "speed": "40",
-        "tags": {
-          "highway": "residential"
-        }
-      },
-      {
-        "speed": "40",
-        "tags": {
-          "highway": "service"
-        }
-      },
-      {
-        "speed": "90",
-        "tags": {
-          "highway": "motorway_link"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "trunk_link"
-        }
-      },
-      {
-        "speed": "80",
-        "tags": {
-          "highway": "primary_link"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "secondary_link"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "highway": "tertiary_link"
-        }
-      },
-      {
-        "speed": "20",
-        "tags": {
-          "highway": "living_street"
-        }
-      }
-    ]
-  },
-  "DE": {
-    "Default": [
-      {
-        "speed": "none",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "10",
-        "tags": {
-          "highway": "living_street"
-        }
-      },
-      {
-        "speed": "30",
-        "tags": {
-          "highway": "residential"
-        }
-      },
-      {
-        "speed": "10",
-        "tags": {
-          "highway": "service"
-        }
-      },
-      {
-        "speed": "100",
-        "tags": {
-          "zone:traffic": "DE:rural"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "zone:traffic": "DE:urban"
-        }
-      },
-      {
-        "speed": "30",
-        "tags": {
-          "zone:maxspeed": "DE:30"
-        }
-      },
-      {
-        "speed": "50",
-        "tags": {
-          "zone:maxspeed": "DE:urban"
-        }
-      },
-      {
-        "speed": "100",
-        "tags": {
-          "zone:maxspeed": "DE:rural"
-        }
-      },
-      {
-        "speed": "none",
-        "tags": {
-          "zone:maxspeed": "DE:motorway"
-        }
-      },
-      {
-        "speed": "30",
-        "tags": {
-          "bicycle_road": "yes"
-        }
-      }
-    ]
-  },
-  "ZA": {
-    "Default": [
-      {
-        "speed": "120",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "120",
-        "tags": {
-          "highway": "motorway_link"
-        }
-      },
-      {
-        "speed": "100",
-        "tags": {
-          "highway": "primary"
-        }
-      },
-      {
-        "speed": "100",
-        "tags": {
-          "highway": "secondary"
-        }
-      },
-      {
-        "speed": "60",
-        "tags": {
-          "highway": "residential"
-        }
-      }
-    ]
-  },
-  "US": {
-    "South Dakota": [
-      {
-        "speed": "80 mph",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "70 mph",
-        "tags": {
-          "highway": "trunk"
-        }
-      },
-      {
-        "speed": "65 mph",
-        "tags": {
-          "highway": "primary"
-        }
-      },
-      {
-        "speed": "70 mph",
-        "tags": {
-          "highway": "trunk_link"
-        }
-      },
-      {
-        "speed": "65 mph",
-        "tags": {
-          "highway": "primary_link"
-        }
-      }
-    ],
-    "Wisconsin": [
-      {
-        "speed": "65 mph",
-        "tags": {
-          "highway": "trunk"
-        }
-      },
-      {
-        "speed": "45 mph",
-        "tags": {
-          "highway": "tertiary"
-        }
-      },
-      {
-        "speed": "35 mph",
-        "tags": {
-          "highway": "unclassified"
-        }
-      },
-      {
-        "speed": "65 mph",
-        "tags": {
-          "highway": "trunk_link"
-        }
-      },
-      {
-        "speed": "45 mph",
-        "tags": {
-          "highway": "tertiary_link"
-        }
-      }
-    ],
-    "Default": [
-      {
-        "speed": "65 mph",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "trunk"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "primary"
-        }
-      },
-      {
-        "speed": "45 mph",
-        "tags": {
-          "highway": "secondary"
-        }
-      },
-      {
-        "speed": "35 mph",
-        "tags": {
-          "highway": "tertiary"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "unclassified"
-        }
-      },
-      {
-        "speed": "25 mph",
-        "tags": {
-          "highway": "residential"
-        }
-      },
-      {
-        "speed": "25 mph",
-        "tags": {
-          "highway": "service"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "motorway_link"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "trunk_link"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "primary_link"
-        }
-      },
-      {
-        "speed": "45 mph",
-        "tags": {
-          "highway": "secondary_link"
-        }
-      },
-      {
-        "speed": "35 mph",
-        "tags": {
-          "highway": "tertiary_link"
-        }
-      },
-      {
-        "speed": "15 mph",
-        "tags": {
-          "highway": "living_street"
-        }
-      }
-    ],
-    "Michigan": [
-      {
-        "speed": "70 mph",
-        "tags": {
-          "highway": "motorway"
-        }
-      }
-    ],
-    "Oregon": [
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "35 mph",
-        "tags": {
-          "highway": "secondary"
-        }
-      },
-      {
-        "speed": "30 mph",
-        "tags": {
-          "highway": "tertiary"
-        }
-      },
-      {
-        "speed": "15 mph",
-        "tags": {
-          "highway": "service"
-        }
-      },
-      {
-        "speed": "35 mph",
-        "tags": {
-          "highway": "secondary_link"
-        }
-      },
-      {
-        "speed": "30 mph",
-        "tags": {
-          "highway": "tertiary_link"
-        }
-      }
-    ],
-    "New York": [
-      {
-        "speed": "65 mph",
-        "tags": {
-          "highway": "motorway"
-        }
-      },
-      {
-        "speed": "45 mph",
-        "tags": {
-          "highway": "primary"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "secondary"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "tertiary"
-        }
-      },
-      {
-        "speed": "30 mph",
-        "tags": {
-          "highway": "residential"
-        }
-      },
-      {
-        "speed": "45 mph",
-        "tags": {
-          "highway": "primary_link"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "secondary_link"
-        }
-      },
-      {
-        "speed": "55 mph",
-        "tags": {
-          "highway": "tertiary_link"
-        }
-      },
-      {
-        "speed": "20 mph",
-        "tags": {
-          "highway": "living_street"
-        }
-      }
-    ]
-  }
-}
@@ -1,78 +0,0 @@
-from enum import Enum
-import numpy as np
-
-
-R = 6373000.0  # approximate radius of earth in mts
-
-
-def vectors(points):
-  """Provides a array of vectors on cartesian space (x, y).
-     Each vector represents the path from a point in `points` to the next.
-     `points` must by a (N, 2) array of [lat, lon] pairs in radians.
-  """
-  latA = points[:-1, 0]
-  latB = points[1:, 0]
-  delta = np.diff(points, axis=0)
-  dlon = delta[:, 1]
-
-  x = np.sin(dlon) * np.cos(latB)
-  y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon))
-
-  return np.column_stack((x, y))
-
-
-def ref_vectors(ref, points):
-  """Provides a array of vectors on cartesian space (x, y).
-     Each vector represents the path from ref to a point in `points`.
-     `points` must by a (N, 2) array of [lat, lon] pairs in radians.
-  """
-  latA = ref[0]
-  latB = points[:, 0]
-  delta = points - ref
-  dlon = delta[:, 1]
-
-  x = np.sin(dlon) * np.cos(latB)
-  y = np.cos(latA) * np.sin(latB) - (np.sin(latA) * np.cos(latB) * np.cos(dlon))
-
-  return np.column_stack((x, y))
-
-
-def bearing_to_points(point, points):
-  """Calculate the bearings (angle from true north clockwise) of the vectors between `point` and each
-  one of the entries in `points`. Both `point` and `points` elements are 2 element arrays containing a latitud,
-  longitude pair in radians.
-  """
-  delta = points - point
-  x = np.sin(delta[:, 1]) * np.cos(points[:, 0])
-  y = np.cos(point[0]) * np.sin(points[:, 0]) - (np.sin(point[0]) * np.cos(points[:, 0]) * np.cos(delta[:, 1]))
-  return np.arctan2(x, y)
-
-def point_on_line(start_points, end_points, point, extend_line = False):
-  """project a single point onto each line for an np array of start points and end points
-  ref: https://stackoverflow.com/a/61342198
-  """
-  ap = np.subtract(point, start_points)
-  ab = np.subtract(end_points, start_points)
-  t = np.array([np.dot(ap[i], ab[i]) / np.dot(ab[i], ab[i]) for i in range(len(ap))])
-  # if you need the the closest point belonging to the segment
-  if not extend_line:
-    t = np.maximum(0, np.minimum(1, t))
-  result = np.add(start_points, np.array([t[i] * ab[i] for i in range(len(t))]))
-  return result
-
-def distance_to_points(point, points):
-  """Calculate the distance of the vectors between `point` and each one of the entries in `points`.
-  Both `point` and `points` elements are 2 element arrays containing a latitud, longitude pair in radians.
-  """
-  delta = points - point
-  a = np.sin(delta[:, 0] / 2)**2 + np.cos(point[0]) * np.cos(points[:, 0]) * np.sin(delta[:, 1] / 2)**2
-  c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
-  return c * R
-
-
-class DIRECTION(Enum):
-  NONE = 0
-  AHEAD = 1
-  BEHIND = 2
-  FORWARD = 3
-  BACKWARD = 4
@@ -1,102 +0,0 @@
-import overpy
-import subprocess
-import email.utils as eut
-import time
-
-from common.params import Params
-from system.version import get_version
-from urllib.request import Request, urlopen
-
-
-OSM_LOCAL_PATH = "/data/media/0/osm"
-OSM_DB_STAMP_FILE = OSM_LOCAL_PATH + "/db_stamp"
-OSM_QUERY = [f"{OSM_LOCAL_PATH}/v0.7.57/bin/osm3s_query", f"--db-dir={OSM_LOCAL_PATH}/db"]
-OSM_DB_STAMP_REMOTE = "https://sunnypilot-osm.s3.us-east-2.amazonaws.com/osm-db/timestamps"
-
-
-def get_current_s3_osm_db_timestamp():
-  try:
-    local_osm_db_name = Params().get("OsmLocationName", encoding="utf8")
-    req = Request(url=f"{OSM_DB_STAMP_REMOTE}/{local_osm_db_name}.txt", headers={"User-Agent": f"sunnypilot-{get_version()}"})
-    r = urlopen(req)
-    if r.status != 200:
-      print(f'Failed to fetch timestamp for S3 OSM db.\n\n{r.status}')
-      return None
-
-    timestamp_string = r.read().decode("utf-8").strip()
-    if timestamp_string is None:
-      print('Timestamp file for S3 OSM db contained no value.')
-      return None
-
-    parsed_date = eut.parsedate(timestamp_string)
-    return time.mktime(parsed_date)
-  except Exception as e:
-    print(f'Could not parse timestamp for S3 local osm db.\n\n{e}')
-    return None
-
-
-def persist_s3_osm_db_timestamp(timestamp):
-  try:
-    with open(OSM_DB_STAMP_FILE, 'w') as file:
-      file.write(f'{timestamp}')
-  except Exception as e:
-    print(f'Failed to timestamp local OSM db.\n\n{e}')
-
-
-def get_local_osm_timestamp():
-  try:
-    with open(OSM_DB_STAMP_FILE, 'r') as file:
-      return float(file.readline())
-  except Exception as e:
-    print(f'Failed to read timestamp for local OSM db.\n\n{e}')
-    return None
-
-
-def is_osm_db_up_to_date():
-  current_osm_ts = get_local_osm_timestamp()
-  if current_osm_ts is None:
-    return False
-
-  current_s3_osm_ts = get_current_s3_osm_db_timestamp()
-  if current_s3_osm_ts is None:
-    return True
-
-  return current_osm_ts == current_s3_osm_ts
-
-
-def timestamp_local_osm_db():
-  current_s3_osm_ts = get_current_s3_osm_db_timestamp()
-  if current_s3_osm_ts is not None:
-    persist_s3_osm_db_timestamp(current_s3_osm_ts)
-
-
-def is_local_osm_installed(params=Params()):
-  api = overpy.Overpass()
-  waypoint = params.get("OsmWayTest", encoding="utf8")
-  if waypoint is None:
-    return False
-  q = f"""
-      way({waypoint});
-      (._;>;);
-      out;
-      """
-
-  try:
-    completion = subprocess.run(OSM_QUERY + [f"--request={q}"], check=True, capture_output=True)
-    print(f'OSM local query returned with exit code: {completion.returncode}')
-
-    if completion.returncode != 0:
-      return False
-
-    print(f'OSM Local query returned:\n\n{completion.stdout}')
-
-    ways = api.parse_xml(completion.stdout).ways
-    success = len(ways) == 1
-    print(f"Test osm script returned {len(ways)} ways")
-    print(f'OSM local server query {"succeeded" if success else "failed"}')
-
-    return success
-
-  except Exception as e:
-    print(e)
-    return False
@@ -1,71 +0,0 @@
-import overpy
-import subprocess
-import numpy as np
-from cereal import custom
-from common.params import Params
-from selfdrive.mapd.lib.geo import R
-from selfdrive.mapd.lib.helpers import is_local_osm_installed, OSM_QUERY
-
-DataType = custom.LiveMapDataSP.DataType
-
-
-def create_way(way_id, node_ids, from_way):
-  """
-  Creates and OSM Way with the given `way_id` and list of `node_ids`, copying attributes and tags from `from_way`
-  """
-  return overpy.Way(way_id, node_ids=node_ids, attributes={}, result=from_way._result,
-                    tags=from_way.tags)
-
-
-class OSM:
-  def __init__(self):
-    self.api = overpy.Overpass()
-    self.param_s = Params()
-    self._osm_local_db_enabled = self.param_s.get_bool("OsmLocalDb")
-    self._local_osm_installed = is_local_osm_installed(self.param_s)
-    # self.api = overpy.Overpass(url='http://3.65.170.21/api/interpreter')
-
-  def _online_query(self, q, area_q):
-    print("Query OSM from remote server")
-    query = self.api.query(q + area_q)
-    areas, ways = query.areas, query.ways
-    data_type = DataType.online
-    return areas, ways, data_type
-
-  def fetch_road_ways_around_location(self, lat, lon, radius):
-    # Calculate the bounding box coordinates for the bbox containing the circle around location.
-    bbox_angle = np.degrees(radius / R)
-    # fetch all ways and nodes on this ways in bbox
-    bbox_str = f'{str(lat - bbox_angle)},{str(lon - bbox_angle)},{str(lat + bbox_angle)},{str(lon + bbox_angle)}'
-    lat_lon = "(%f,%f)" % (lat, lon)
-    q = """
-        way(""" + bbox_str + """)
-          [highway]
-          [highway!~"^(footway|path|corridor|bridleway|steps|cycleway|construction|bus_guideway|escape|service|track)$"];
-        (._;>;);
-        out;"""
-    area_q = """is_in""" + lat_lon + """;area._[admin_level~"[24]"];
-        convert area ::id = id(), admin_level = t['admin_level'],
-        name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out;
-        """
-    try:
-      if self._osm_local_db_enabled and self._local_osm_installed:
-        print("Query OSM from local server")
-        completion = subprocess.run(OSM_QUERY + [f"--request={q}"], check=True, capture_output=True)
-        ways = self.api.parse_xml(completion.stdout).ways
-        if completion.returncode == 0 and len(ways) != 0:
-          try:
-            areas = self.api.query(area_q).areas
-          except Exception as e:
-            print(f'Exception while querying "AREAS" OSM from local server:\n{e}')
-            areas = None
-          data_type = DataType.offline
-        else:
-          areas, ways, data_type = self._online_query(q, area_q)
-      else:
-        areas, ways, data_type = self._online_query(q, area_q)
-    except Exception as e:
-      print(f'Exception while querying OSM:\n{e}')
-      areas, ways, data_type = [], [], DataType.default
-
-    return areas, ways, data_type
@@ -1,294 +0,0 @@
-#!/usr/bin/env python3
-import json
-import threading
-from traceback import print_exception
-import numpy as np
-from time import strftime, gmtime
-from cereal import custom
-import cereal.messaging as messaging
-from common.params import Params
-from common.realtime import set_core_affinity, set_thread_affinity, Ratekeeper
-from selfdrive.mapd.lib.osm import OSM
-from selfdrive.mapd.lib.geo import distance_to_points
-from selfdrive.mapd.lib.WayCollection import WayCollection
-from selfdrive.mapd.config import QUERY_RADIUS, QUERY_RADIUS_OFFLINE, MIN_DISTANCE_FOR_NEW_QUERY, FULL_STOP_MAX_SPEED, LOOK_AHEAD_HORIZON_TIME
-from system.swaglog import cloudlog
-
-DataType = custom.LiveMapDataSP.DataType
-
-
-_DEBUG = False
-_CLOUDLOG_DEBUG = True
-ROAD_NAME_TIMEOUT = 30 # secs
-
-
-def _debug(msg, log_to_cloud=True):
-  if _CLOUDLOG_DEBUG and log_to_cloud:
-    cloudlog.debug(msg)
-  if _DEBUG:
-    print(msg)
-
-
-def excepthook(args):
-  _debug(f'MapD: Threading exception:\n{args}')
-  print_exception(args.exc_type, args.exc_value, args.exc_traceback)
-
-
-threading.excepthook = excepthook
-
-
-class MapD():
-  def __init__(self):
-    self.osm = OSM()
-    self.way_collection = None
-    self.route = None
-    self.last_gps_fix_timestamp = 0
-    self.last_gps = None
-    self.location_deg = None  # The current location in degrees.
-    self.location_rad = None  # The current location in radians as a Numpy array.
-    self.bearing_rad = None
-    self.location_stdev = None  # The current location accuracy in mts. 1 standard devitation.
-    self.gps_speed = 0.
-    self.last_fetch_location = None
-    self.last_route_update_fix_timestamp = 0
-    self.last_publish_fix_timestamp = 0
-    self.data_type = DataType.default
-    self._op_enabled = False
-    self._disengaging = False
-    self._query_thread = None
-    self._lock = threading.RLock()
-    self.gps_sock = 'gpsLocationExternal'
-
-    # dp - use LastGPSPosition as init position (if we are in a undercover car park?)
-    # this way we can prefetch osm data before we get a fix.
-    last_pos = Params().get("LastGPSPosition")
-    if last_pos is not None and last_pos != "":
-      l = json.loads(last_pos)
-      lat = float(l["latitude"])
-      lon = float(l["longitude"])
-      self.location_rad = np.radians(np.array([lat, lon], dtype=float))
-      self.location_deg = (lat, lon)
-      self.bearing_rad = np.radians(0, dtype=float)
-      _debug("Use LastGPSPosition position - lat: %s, lon: %s" % (lat, lon))
-
-  def udpate_state(self, sm):
-    sock = 'carControl'
-    if not sm.updated[sock] or not sm.valid[sock]:
-      return
-
-    hud_control = sm[sock].hudControl
-    self._disengaging = not hud_control.speedVisible and self._op_enabled
-    self._op_enabled = hud_control.speedVisible
-
-  def update_gps(self, sm):
-    self.gps_sock = 'gpsLocationExternal' if sm.rcv_frame['gpsLocationExternal'] > 1 else 'gpsLocation'
-    if not sm.updated[self.gps_sock] or not sm.valid[self.gps_sock]:
-      return
-
-    log = sm[self.gps_sock]
-    self.last_gps = log
-
-    # ignore the message if the fix is invalid
-    if log.flags % 2 == 0:
-      return
-
-    self.last_gps_fix_timestamp = log.unixTimestampMillis  # Unix TS. Milliseconds since January 1, 1970.
-    self.location_rad = np.radians(np.array([log.latitude, log.longitude], dtype=float))
-    self.location_deg = (log.latitude, log.longitude)
-    self.bearing_rad = np.radians(log.bearingDeg, dtype=float)
-    self.gps_speed = log.speed
-    self.location_stdev = log.accuracy if self.gps_sock == 'gpsLocationExternal' else 1  # gpsLocation doesn't report accuracy
-
-    _debug('Mapd: ********* Got GPS fix'
-           + f'Pos: {self.location_deg} +/- {self.location_stdev * 2.} mts.\n'
-           + f'Bearing: {log.bearingDeg} +/- {log.bearingAccuracyDeg * 2.} deg.\n'
-           + f'timestamp: {strftime("%d-%m-%y %H:%M:%S", gmtime(self.last_gps_fix_timestamp * 1e-3))}'
-           + '*******', log_to_cloud=False)
-
-  def _query_osm_not_blocking(self):
-    def query(osm, location_deg, location_rad, radius):
-      _debug(f'Mapd: Start query for OSM map data at {location_deg}')
-      lat, lon = location_deg
-      areas, ways, self.data_type = osm.fetch_road_ways_around_location(lat, lon, radius)
-      _debug(f'Mapd: Query to OSM finished with {len(ways)} ways')
-
-      # Only issue an update if we received some ways. Otherwise it is most likely a connectivity issue.
-      # Will retry on next loop.
-      if len(ways) > 0:
-        new_way_collection = WayCollection(areas, ways, location_rad)
-
-        # Use the lock to update the way_collection as it might be being used to update the route.
-        _debug('Mapd: Locking to write results from osm.', log_to_cloud=False)
-        with self._lock:
-          self.way_collection = new_way_collection
-          self.last_fetch_location = location_rad
-          _debug(f'Mapd: Updated map data @ {location_deg} - got {len(ways)} ways')
-
-        _debug('Mapd: Releasing Lock to write results from osm', log_to_cloud=False)
-
-    # Ignore if we have a query thread already running.
-    if self._query_thread is not None and self._query_thread.is_alive():
-      return
-
-    self._query_thread = threading.Thread(target=query, args=(self.osm, self.location_deg, self.location_rad,
-                                                              QUERY_RADIUS_OFFLINE if self.data_type == DataType.offline else QUERY_RADIUS))
-    set_thread_affinity(self._query_thread, [0, 1, 2, 3])
-    self._query_thread.start()
-
-  def updated_osm_data(self):
-    if self.route is not None:
-      distance_to_end = self.route.distance_to_end
-      if distance_to_end is not None and distance_to_end >= MIN_DISTANCE_FOR_NEW_QUERY:
-        # do not query as long as we have a route with enough distance ahead.
-        return
-
-    if self.location_rad is None:
-      return
-
-    if self.last_fetch_location is not None:
-      distance_since_last = distance_to_points(self.last_fetch_location, np.array([self.location_rad]))[0]
-      if distance_since_last < (QUERY_RADIUS_OFFLINE if self.data_type == DataType.offline else QUERY_RADIUS) - MIN_DISTANCE_FOR_NEW_QUERY:
-        # do not query if are still not close to the border of previous query area
-        return
-
-    self._query_osm_not_blocking()
-
-  def update_route(self):
-    def update_proc():
-      # Ensure we clear the route on op disengage, this way we can correct possible incorrect map data due
-      # to wrongly locating or picking up the wrong route.
-      if self._disengaging:
-        self.route = None
-        _debug('Mapd *****: Clearing Route as system is disengaging. ********')
-
-      if self.way_collection is None or self.location_rad is None or self.bearing_rad is None:
-        _debug('Mapd *****: Can not update route. Missing WayCollection, location or bearing ********')
-        return
-
-      if self.route is not None and self.last_route_update_fix_timestamp == self.last_gps_fix_timestamp:
-        _debug('Mapd *****: Skipping route update. No new fix since last update ********')
-        return
-
-      self.last_route_update_fix_timestamp = self.last_gps_fix_timestamp
-
-      # Create the route if not existent or if it was generated by an older way collection
-      if self.route is None or self.route.way_collection_id != self.way_collection.id:
-        self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev)
-        _debug(f'Mapd *****: Route created: \n{self.route}\n********')
-        return
-
-      # Do not attempt to update the route if the car is going close to a full stop, as the bearing can start
-      # jumping and creating unnecessary losing of the route. Since the route update timestamp has been updated
-      # a new liveMapDataSP message will be published with the current values (which is desirable)
-      if self.gps_speed < FULL_STOP_MAX_SPEED:
-        _debug('Mapd *****: Route Not updated as car has Stopped ********')
-        return
-
-      self.route.update(self.location_rad, self.bearing_rad, self.location_stdev)
-      if self.route.located:
-        _debug(f'Mapd *****: Route updated: \n{self.route}\n********')
-        return
-
-      # if an old route did not mange to locate, attempt to regenerate form way collection.
-      self.route = self.way_collection.get_route(self.location_rad, self.bearing_rad, self.location_stdev)
-      _debug(f'Mapd *****: Failed to update location in route. Regenerated with route: \n{self.route}\n********')
-
-    # We use the lock when updating the route, as it reads `way_collection` which can ben updated by
-    # a new query result from the _query_thread.
-    _debug('Mapd: Locking to update route.', log_to_cloud=False)
-    with self._lock:
-      update_proc()
-
-    _debug('Mapd: Releasing Lock to update route', log_to_cloud=False)
-
-  def publish(self, pm, sm):
-    # Ensure we have a route currently located
-    if self.route is None or not self.route.located:
-      _debug('Mapd: Skipping liveMapDataSP message as there is no route or is not located.')
-      return
-
-    # Ensure we have a route update since last publish
-    if self.last_publish_fix_timestamp == self.last_route_update_fix_timestamp:
-      _debug('Mapd: Skipping liveMapDataSP since there is no new gps fix.')
-      return
-
-    self.last_publish_fix_timestamp = self.last_route_update_fix_timestamp
-
-    speed_limit = self.route.current_speed_limit
-    next_speed_limit_section = self.route.next_speed_limit_section
-    turn_speed_limit_section = self.route.current_curvature_speed_limit_section
-    horizon_mts = self.gps_speed * LOOK_AHEAD_HORIZON_TIME
-    next_turn_speed_limit_sections = self.route.next_curvature_speed_limit_sections(horizon_mts)
-    current_road_name = self.route.current_road_name
-
-    map_data_msg = messaging.new_message('liveMapDataSP')
-    map_data_msg.valid = sm.all_alive(service_list=[self.gps_sock]) and \
-                         sm.all_valid(service_list=[self.gps_sock])
-
-    liveMapDataSP = map_data_msg.liveMapDataSP
-    liveMapDataSP.lastGpsTimestamp = self.last_gps.unixTimestampMillis
-    liveMapDataSP.lastGpsLatitude = float(self.last_gps.latitude)
-    liveMapDataSP.lastGpsLongitude = float(self.last_gps.longitude)
-    liveMapDataSP.lastGpsSpeed = float(self.last_gps.speed)
-    liveMapDataSP.lastGpsBearingDeg = float(self.last_gps.bearingDeg)
-    liveMapDataSP.lastGpsAccuracy = float(self.last_gps.accuracy if self.gps_sock == 'gpsLocationExternal' else 1)  # gpsLocation doesnt report accuracy
-    liveMapDataSP.lastGpsBearingAccuracyDeg = float(self.last_gps.bearingAccuracyDeg)
-
-    liveMapDataSP.speedLimitValid = bool(speed_limit is not None)
-    liveMapDataSP.speedLimit = float(speed_limit if speed_limit is not None else 0.0)
-    liveMapDataSP.speedLimitAheadValid = bool(next_speed_limit_section is not None)
-    liveMapDataSP.speedLimitAhead = float(next_speed_limit_section.value
-                                                     if next_speed_limit_section is not None else 0.0)
-    liveMapDataSP.speedLimitAheadDistance = float(next_speed_limit_section.start
-                                                             if next_speed_limit_section is not None else 0.0)
-
-    liveMapDataSP.turnSpeedLimitValid = bool(turn_speed_limit_section is not None)
-    liveMapDataSP.turnSpeedLimit = float(turn_speed_limit_section.value
-                                                    if turn_speed_limit_section is not None else 0.0)
-    liveMapDataSP.turnSpeedLimitSign = int(turn_speed_limit_section.curv_sign
-                                                      if turn_speed_limit_section is not None else 0)
-    liveMapDataSP.turnSpeedLimitEndDistance = float(turn_speed_limit_section.end
-                                                               if turn_speed_limit_section is not None else 0.0)
-    liveMapDataSP.turnSpeedLimitsAhead = [float(s.value) for s in next_turn_speed_limit_sections]
-    liveMapDataSP.turnSpeedLimitsAheadDistances = [float(s.start) for s in next_turn_speed_limit_sections]
-    liveMapDataSP.turnSpeedLimitsAheadSigns = [float(s.curv_sign) for s in next_turn_speed_limit_sections]
-
-    liveMapDataSP.currentRoadName = str(current_road_name if current_road_name is not None else "")
-
-    liveMapDataSP.dataType = self.data_type
-
-    pm.send('liveMapDataSP', map_data_msg)
-    _debug(f'Mapd *****: Publish: \n{map_data_msg}\n********', log_to_cloud=False)
-
-
-# provides live map data information
-def mapd_thread(sm=None, pm=None):
-  try:
-    set_core_affinity([0, 1, 2, 3])
-  except Exception:
-    cloudlog.exception("mapd: failed to set core affinity")
-  mapd = MapD()
-  rk = Ratekeeper(1., print_delay_threshold=None)  # Keeps rate at 1 hz
-
-  # *** setup messaging
-  if sm is None:
-    sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal', 'carControl'])
-  if pm is None:
-    pm = messaging.PubMaster(['liveMapDataSP'])
-
-  while True:
-    sm.update()
-    mapd.udpate_state(sm)
-    mapd.update_gps(sm)
-    mapd.updated_osm_data()
-    mapd.update_route()
-    mapd.publish(pm, sm)
-    rk.keep_time()
-
-
-def main(sm=None, pm=None):
-  mapd_thread(sm, pm)
-
-
-if __name__ == "__main__":
-  main()
@@ -0,0 +1,162 @@
+import json
+import time
+import platform
+
+import numpy as np
+
+from openpilot.common.params_pyx import Params
+from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
+from openpilot.selfdrive.navd.helpers import Coordinate
+from openpilot.selfdrive.sunnypilot.live_map_data import QUERY_RADIUS
+from openpilot.common.realtime import Ratekeeper, set_core_affinity
+from openpilot.system.swaglog import cloudlog
+import os
+import glob
+import shutil
+
+from openpilot.selfdrive.sunnypilot.live_map_data.osm_map_data import OsmMapData
+from openpilot.selfdrive.sunnypilot.live_map_data import R
+
+# PFEIFER - MAPD {{
+params = Params()
+mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
+# }} PFEIFER - MAPD
+
+COMMON_DIR = '/data/media/0/osm'
+MAPD_BIN_DIR = '/data/openpilot/third_party/pfeiferj-mapd'
+MAPD_PATH = os.path.join(MAPD_BIN_DIR, 'mapd')
+
+
+def get_files_for_cleanup():
+  paths = [
+    f"{COMMON_DIR}/db",
+    f"{COMMON_DIR}/v*"
+  ]
+  files_to_remove = []
+  for path in paths:
+    if os.path.exists(path):
+      files = glob.glob(path + '/**', recursive=True)
+      files_to_remove.extend(files)
+  # check for version and mapd files
+  if not os.path.isfile(MAPD_PATH):
+    files_to_remove.append(MAPD_PATH)
+  return files_to_remove
+
+
+def cleanup_OLD_OSM_data(files_to_remove):
+  for file in files_to_remove:
+    # Remove trailing slash if path is file
+    if file.endswith('/') and os.path.isfile(file[:-1]):
+      file = file[:-1]
+    # Try to remove as file or symbolic link first
+    if os.path.islink(file) or os.path.isfile(file):
+      os.remove(file)
+    elif os.path.isdir(file):  # If it's a directory
+      shutil.rmtree(file, ignore_errors=False)
+
+
+def _get_current_bounding_box(self, radius: float):
+  self.last_query_radius = radius
+  # Calculate the bounding box coordinates for the bbox containing the circle around location.
+  bbox_angle = float(np.degrees(radius / R))
+
+  lat = float(self.last_gps.latitude)
+  lon = float(self.last_gps.longitude)
+
+  return {
+    "min_lat": lat - bbox_angle,
+    "min_lon": lon - bbox_angle,
+    "max_lat": lat + bbox_angle,
+    "max_lon": lon + bbox_angle,
+  }
+
+
+def _request_refresh_osm_bounds_data(self):
+  self.last_refresh_loc = Coordinate(self.last_gps.latitude, self.last_gps.longitude)
+  self.last_query_radius = QUERY_RADIUS
+  current_bounding_box = self._get_current_bounding_box(self.last_query_radius)
+  mem_params.put("OSMDownloadBounds", json.dumps(current_bounding_box))
+
+
+def request_refresh_osm_location_data(nations: [str], states: [str] = None):
+  params.put("OsmDownloadedDate", str(time.time()))
+  params.put_bool("OsmDbUpdatesCheck", False)
+
+  osm_download_locations = json.dumps({
+    "nations": nations,
+    "states": states or []
+  })
+
+  print(f"Downloading maps for {osm_download_locations}")
+  mem_params.put("OSMDownloadLocations", osm_download_locations)
+
+
+def filter_nations_and_states(nations: [str], states: [str] = None):
+  """Filters and prepares nation and state data for OSM map download.
+
+  If the nation is 'US' and a specific state is provided, the nation 'US' is removed from the list.
+  If the nation is 'US' and the state is 'All', the 'All' is removed from the list.
+  The idea behind these filters is that if a specific state in the US is provided,
+  there's no need to download map data for the entire US. Conversely,
+  if the state is unspecified (i.e., 'All'), we intend to download map data for the whole US,
+  and 'All' isn't a valid state name, so it's removed.
+
+  Parameters:
+  nations (list): A list of nations for which the map data is to be downloaded.
+  states (list, optional): A list of states for which the map data is to be downloaded. Defaults to None.
+
+  Returns:
+  tuple: Two lists. The first list is filtered nations and the second list is filtered states.
+  """
+
+  if "US" in nations and states and not any(x.lower() == "all" for x in states):
+    # If a specific state in the US is provided, remove 'US' from nations
+    nations.remove("US")
+  elif "US" in nations and states and any(x.lower() == "all" for x in states):
+    # If 'All' is provided as a state (case invariant), remove those instances from states
+    states = [x for x in states if x.lower() != "all"]
+  elif "US" not in nations and states and any(x.lower() == "all" for x in states):
+    states.remove("All")
+  return nations, states or []
+
+
+def update_osm_db():
+  # last_downloaded_date = float(params.get('OsmDownloadedDate', encoding='utf-8') or 0.0)
+  # if params.get_bool("OsmDbUpdatesCheck") or time.time() - last_downloaded_date >= 604800:  # 7 days * 24 hours/day * 60
+  if params.get_bool("OsmDbUpdatesCheck"):
+    cleanup_OLD_OSM_data(get_files_for_cleanup())
+    country = params.get('OsmLocationName', encoding='utf-8')
+    state = params.get('OsmStateName', encoding='utf-8') or "All"
+    filtered_nations, filtered_states = filter_nations_and_states([country], [state])
+    request_refresh_osm_location_data(filtered_nations, filtered_states)
+
+  if not mem_params.get("OSMDownloadBounds"):
+    mem_params.put("OSMDownloadBounds", "")
+
+  if not mem_params.get("LastGPSPosition"):
+    mem_params.put("LastGPSPosition", "{}")
+
+
+def main_thread(sm=None, pm=None):
+  try:
+    set_core_affinity([0, 1, 2, 3])
+  except Exception:
+    cloudlog.exception("mapd: failed to set core affinity")
+  rk = Ratekeeper(1, print_delay_threshold=None)
+  live_map_sp = OsmMapData()
+
+  while True:
+    show_alert = get_files_for_cleanup() and params.get_bool("OsmLocal")
+    set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
+
+    update_osm_db()
+    live_map_sp.tick()
+    rk.keep_time()
+
+
+def main():
+  main_thread()
+
+
+if __name__ == "__main__":
+  main()
@@ -1,11 +1,13 @@
 """Install exception handler for process crash."""
 import sentry_sdk
+import subprocess
 from enum import Enum
 from typing import Tuple
 from sentry_sdk.integrations.threading import ThreadingIntegration

+from openpilot.common.basedir import BASEDIR
 from openpilot.common.params import Params
-#from openpilot.selfdrive.athena.registration import is_registered_device
+from openpilot.selfdrive.athena.registration import UNREGISTERED_DONGLE_ID, is_registered_device
 from openpilot.system.hardware import HARDWARE, PC
 from openpilot.system.swaglog import cloudlog
 from openpilot.system.version import get_branch, get_commit, get_origin, get_version, \
@@ -31,6 +33,7 @@ def report_tombstone(fn: str, message: str, contents: str) -> None:
  cloudlog.error({'tombstone': message})

  with sentry_sdk.configure_scope() as scope:
+    bind_user()
    scope.set_extra("tombstone_fn", fn)
    scope.set_extra("tombstone", contents)
    sentry_sdk.capture_message(message=message)
@@ -42,8 +45,7 @@ def capture_exception(*args, **kwargs) -> None:
  cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1))

  try:
-    dongle_id, ip, gitname = get_properties()
-    bind_user(id=dongle_id, ip_address=ip, name=gitname)
+    bind_user()
    sentry_sdk.capture_exception(*args, **kwargs)
    sentry_sdk.flush()  # https://github.com/getsentry/sentry-python/issues/291
  except Exception:
@@ -70,20 +72,19 @@ def save_exception(exc_text: str) -> None:
  print('Logged current crash to {}'.format(files))


-def bind_user(**kwargs) -> None:
-  sentry_sdk.set_user(kwargs)
+def bind_user() -> None:
+  dongle_id, gitname = get_properties()
+  sentry_sdk.set_user({"id": dongle_id, "ip_address": IP_ADDRESS, "name": gitname})


 def capture_warning(warning_string: str) -> None:
-  dongle_id, ip, gitname = get_properties()
-  bind_user(id=dongle_id, ip_address=ip, name=gitname)
+  bind_user()
  sentry_sdk.capture_message(warning_string, level='warning')
  sentry_sdk.flush()


 def capture_info(info_string: str) -> None:
-  dongle_id, ip, gitname = get_properties()
-  bind_user(id=dongle_id, ip_address=ip, name=gitname)
+  bind_user()
  sentry_sdk.capture_message(info_string, level='info')
  sentry_sdk.flush()

@@ -92,19 +93,29 @@ def set_tag(key: str, value: str) -> None:
  sentry_sdk.set_tag(key, value)


-def get_properties() -> Tuple[str, str, str]:
+def get_properties() -> Tuple[str, str]:
  params = Params()
-  try:
-    dongle_id = params.get("DongleId", encoding='utf-8')
-  except AttributeError:
-    dongle_id = "None"
-  try:
-    gitname = params.get("GithubUsername", encoding='utf-8')
-  except Exception:
+  dongle_id = params.get("DongleId", encoding='utf-8')
+  if dongle_id in (None, UNREGISTERED_DONGLE_ID):
+    hardware_serial = params.get("HardwareSerial", encoding='utf-8')
+    hardware_serial = "" if hardware_serial is None else hardware_serial
+    dongle_id = UNREGISTERED_DONGLE_ID + hardware_serial
+  gitname = params.get("GithubUsername", encoding='utf-8')
+  if gitname is None:
    gitname = ""
-  ip = IP_ADDRESS

-  return dongle_id, ip, gitname
+  return dongle_id, gitname
+
+
+def get_init() -> None:
+  params = Params()
+  dongle_id, _ = get_properties()
+  route_name = params.get("CurrentRoute", encoding='utf-8')
+  subprocess.call(["./bootlog", "--started"], cwd=os.path.join(BASEDIR, "system/loggerd"))
+  with sentry_sdk.configure_scope() as scope:
+    if route_name is not None:
+      sentry_sdk.set_tag("route_name", dongle_id + "|" + route_name)
+      scope.add_attachment(path=os.path.join("/data/media/0/realdata/params", route_name))


 def init(project: SentryProject) -> bool:
@@ -115,8 +126,7 @@ def init(project: SentryProject) -> bool:

  #env = "release" if is_tested_branch() else "master"
  env = get_branch_type()
-  dongle_id = Params().get("DongleId", encoding='utf-8')
-  gitname = Params().get("GithubUsername", encoding='utf-8')
+  dongle_id, gitname = get_properties()

  integrations = []
  if project == SentryProject.SELFDRIVE:
--- a/Show More
+++ b/Show More