Update interface.py

.github/FUNDING.yml

@@ -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']

CHANGELOGS.md

@@ -1,3 +1,28 @@
+sunnypilot - 0.9.6.1 (2023-xx-xx)
+========================
+* UPDATED: Dynamic Experimental Control (DEC)
+  * Synced with dragonpilot-community/dragonpilot:beta3 commit dd4c663
+* 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
+* 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
+  * DISABLED: Map-based Turn Speed Control (M-TSC)
+    * Reimplementation in near future updates
+* 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

body/board/obj/gitversion.h

@@ -1 +1 @@
-const uint8_t gitversion[8] = "5bcb1122";
+const uint8_t gitversion[8] = "00000000";

common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "0.9.5.2-staging"
+#define COMMA_VERSION "0.9.6.1-staging"

installer/custom/install_osm.sh

@@ -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}

installer/custom/install_osm_db.sh

@@ -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

launch_chffrplus.sh

@@ -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
 }

selfdrive/car/car_helpers.py

@@ -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

selfdrive/car/chrysler/interface.py

@@ -89,7 +89,7 @@ class CarInterface(CarInterfaceBase):
 
     if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
       # TODO: allow these cars to steer down to 13 m/s if already engaged.
-      ret.minSteerSpeed = 17.5  # m/s 17 on the way up, 13 on the way down once engaged.
+      ret.minSteerSpeed = 0.0  # m/s 17 on the way up, 13 on the way down once engaged.
 
     ret.centerToFront = ret.wheelbase * 0.44
     ret.enableBsm = 720 in fingerprint[0]

selfdrive/car/honda/carstate.py

@@ -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

selfdrive/car/honda/interface.py

@@ -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),

selfdrive/car/honda/values.py

@@ -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',

selfdrive/car/hyundai/carcontroller.py

@@ -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

selfdrive/car/hyundai/hyundaican.py

@@ -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
 

selfdrive/car/hyundai/interface.py

@@ -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):

selfdrive/car/hyundai/values.py

@@ -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),

selfdrive/car/interfaces.py

@@ -757,6 +757,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)

selfdrive/car/sunnypilot_carname.json

@@ -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",

selfdrive/car/torque_data/override.yaml

@@ -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]

selfdrive/car/toyota/carstate.py

@@ -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))

selfdrive/controls/lib/alerts_offroad.json

@@ -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
   }
 }

selfdrive/controls/lib/dynamic_experimental_controller.py

@@ -1,123 +1,260 @@
-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 = 0.1.2
+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 = 1.55
 
-_DP_E2E_BLINKER_COUNT = 5
+HIGHWAY_CRUISE_KPH = 75
+
+STOP_AND_GO_FRAME = 500
+
+MODE_SWITCH_DELAY_FRAME = 500
+
+
+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._mode_switch_allowed = True
+    self._mode_switch_frame = 0
+    self._frame = 0
 
-    # conditional e2e
-    self.dp_e2e_has_lead = False
-    self.dp_e2e_lead_last = False
-    self.dp_e2e_lead_count = 0
-    self.dp_e2e_sng = False
-    self.dp_e2e_sng_count = 0
-    self.dp_e2e_standstill_last = False
-    self.dp_e2e_swap_count = 0
-    self.dp_e2e_stop_count = 0
-    self.dp_e2e_tf_count = 0
+    self._lead_gmac = GenericMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
+    self._has_lead_filtered = False
+    self._has_lead_filtered_prev = False
 
-    self.dp_e2e_blinker_count = 0
+    self._slow_down_gmac = GenericMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
+    self._has_slow_down = False
+    self._has_slow_down_prev = False
+
+    self._has_blinkers = False
+    self._has_blinkers_prev = False
+
+    self._slowness_gmac = GenericMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._has_slowness = False
+    self._has_slowness_prev = False
+
+    self._has_nav_enabled = False
+    self._has_nav_enabled_prev = False
+
+    self._dangerous_ttc_gmac = GenericMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._has_dangerous_ttc = False
+    self._has_dangerous_ttc_prev = False
+
+    self._v_ego_kph = 0.
+    self._v_cruise_kph = 0.
+
+    self._has_lead = False
+    self._has_lead_prev = False
+
+    self._has_standstill = False
+    self._has_standstill_prev = False
+
+    self._sng_transit_frame = 0
+    self._sng_state = SNG_State.off
     pass
 
-  def _set_dp_e2e_mode(self, mode, force=False):
-    if force:
-      self.dp_e2e_swap_count = 0
-      self._mode = mode
-      return
+  def _update(self, car_state, lead_one, md, controls_state, radar_unavailable):
+    self._v_ego_kph = car_state.vEgo * 3.6
+    self._v_cruise_kph = controls_state.vCruise
+    self._has_lead = lead_one.status
+    self._has_standstill = car_state.standstill
 
-    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
+    # nav enable detection
+    self._has_nav_enabled = md.navEnabled
 
-      if self.dp_e2e_swap_count >= _DP_E2E_SWAP_COUNT:
-        self._mode = mode
-
-  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
+    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_slowness_prev = self._has_slowness
+    self._has_slow_down_prev = self._has_slow_down
+    self._has_lead_filtered_prev = self._has_lead_filtered
+    self._frame += 1
+
+  def _blended_priority_mode(self):
+    # when blinker is on and speed is driving below highway cruise speed: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
+      self._mode = 'blended'
+      return
+
+    # when at highway cruise and SNG: blended
+    # ensuring blended mode is used because acc is bad at catching SNG lead car
+    # especially those that accel very fast and then brake very hard.
+    if self._sng_state == SNG_State.going and self._v_cruise_kph >= HIGHWAY_CRUISE_KPH:
+      self._mode = 'blended'
+      return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._mode = 'blended'
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._mode = 'blended'
+      return
+
+    # when detecting lead slow down: blended
+    # use blended for higher braking capability
+    if self._has_dangerous_ttc:
+      self._mode = 'blended'
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._mode = 'acc'
+      return
+
+    self._mode = 'blended'
+
+  def _acc_priority_mode(self):
+    # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
+    #if self._has_lead_filtered and not self._has_standstill:
+    #  self._mode = 'acc'
+    #  return
+
+    # when blinker is on and speed is driving below highway cruise speed: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    if self._has_blinkers and self._v_ego_kph < HIGHWAY_CRUISE_KPH:
+      self._mode = 'blended'
+      return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._mode = 'blended'
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._mode = 'blended'
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._mode = 'acc'
+      return
+
+    self._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, radar_unavailable)
+      if self._frame > self._mode_switch_frame:
+        self._mode_switch_allowed = True
+      if radar_unavailable:
+        self._blended_priority_mode()
+      else:
+        self._acc_priority_mode()
 
+    self._mode_prev = self._mode
     return self._mode
 
   def set_enabled(self, enabled):

selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json

@@ -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,

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_sfunction_lat.c

@@ -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

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun.h

@@ -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

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun.h

@@ -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

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun.h

@@ -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

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/make_sfun_lat.m

@@ -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)

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_sfunction_long.c

@@ -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

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_h_constraint.h

@@ -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

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun.h

@@ -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

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun.h

@@ -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

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun.h

@@ -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

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/make_sfun_long.m

@@ -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)

selfdrive/controls/lib/longitudinal_planner.py

@@ -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)
 
@@ -241,30 +241,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)
 
     # 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
 
     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

selfdrive/controls/lib/sunnypilot/speed_limit_controller.py

@@ -1,4 +1,3 @@
-import numpy as np
 import time
 from common.numpy_fast import clip, interp
 from common.conversions import Conversions as CV
@@ -42,7 +41,6 @@ class SpeedLimitController:
     self._state = SpeedLimitControlState.inactive
     self._state_prev = SpeedLimitControlState.inactive
     self._gas_pressed = False
-    self._a_target = 0.
 
     self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
     self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
@@ -75,10 +73,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
@@ -165,7 +159,7 @@ class SpeedLimitController:
 
     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))
 
   def transition_state_from_inactive(self):
     """ Make state transition from inactive state """
@@ -262,12 +256,6 @@ 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 not self.is_active:
       if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \
@@ -301,5 +289,4 @@ class SpeedLimitController:
     self._update_params(CP)
     self._update_calculations()
     self._state_transition()
-    self._update_solution()
     self._update_events(events)

selfdrive/controls/lib/sunnypilot/speed_limit_resolver.py

@@ -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):

selfdrive/controls/lib/turn_speed_controller.py

@@ -52,12 +52,6 @@ class TurnSpeedController():
 
     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
-
   @property
   def state(self):
     return self._state
@@ -209,26 +203,6 @@ class TurnSpeedController():
       if self._v_offset < LIMIT_SPEED_OFFSET_TH and self.distance > 0.:
         self.state = TurnSpeedControlState.adapting
 
-  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)
-
-    # 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
@@ -240,4 +214,3 @@ class TurnSpeedController():
     self._update_params()
     self._update_calculations()
     self._state_transition(sm)
-    self._update_solution()

selfdrive/controls/lib/vision_turn_controller.py

@@ -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()

selfdrive/locationd/models/generated/car.cpp

@@ -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_5539204589909662238) {
+   out_5539204589909662238[0] = delta_x[0] + nom_x[0];
+   out_5539204589909662238[1] = delta_x[1] + nom_x[1];
+   out_5539204589909662238[2] = delta_x[2] + nom_x[2];
+   out_5539204589909662238[3] = delta_x[3] + nom_x[3];
+   out_5539204589909662238[4] = delta_x[4] + nom_x[4];
+   out_5539204589909662238[5] = delta_x[5] + nom_x[5];
+   out_5539204589909662238[6] = delta_x[6] + nom_x[6];
+   out_5539204589909662238[7] = delta_x[7] + nom_x[7];
+   out_5539204589909662238[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_6836761615042086214) {
+   out_6836761615042086214[0] = -nom_x[0] + true_x[0];
+   out_6836761615042086214[1] = -nom_x[1] + true_x[1];
+   out_6836761615042086214[2] = -nom_x[2] + true_x[2];
+   out_6836761615042086214[3] = -nom_x[3] + true_x[3];
+   out_6836761615042086214[4] = -nom_x[4] + true_x[4];
+   out_6836761615042086214[5] = -nom_x[5] + true_x[5];
+   out_6836761615042086214[6] = -nom_x[6] + true_x[6];
+   out_6836761615042086214[7] = -nom_x[7] + true_x[7];
+   out_6836761615042086214[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_5300350899020605715) {
+   out_5300350899020605715[0] = 1.0;
+   out_5300350899020605715[1] = 0;
+   out_5300350899020605715[2] = 0;
+   out_5300350899020605715[3] = 0;
+   out_5300350899020605715[4] = 0;
+   out_5300350899020605715[5] = 0;
+   out_5300350899020605715[6] = 0;
+   out_5300350899020605715[7] = 0;
+   out_5300350899020605715[8] = 0;
+   out_5300350899020605715[9] = 0;
+   out_5300350899020605715[10] = 1.0;
+   out_5300350899020605715[11] = 0;
+   out_5300350899020605715[12] = 0;
+   out_5300350899020605715[13] = 0;
+   out_5300350899020605715[14] = 0;
+   out_5300350899020605715[15] = 0;
+   out_5300350899020605715[16] = 0;
+   out_5300350899020605715[17] = 0;
+   out_5300350899020605715[18] = 0;
+   out_5300350899020605715[19] = 0;
+   out_5300350899020605715[20] = 1.0;
+   out_5300350899020605715[21] = 0;
+   out_5300350899020605715[22] = 0;
+   out_5300350899020605715[23] = 0;
+   out_5300350899020605715[24] = 0;
+   out_5300350899020605715[25] = 0;
+   out_5300350899020605715[26] = 0;
+   out_5300350899020605715[27] = 0;
+   out_5300350899020605715[28] = 0;
+   out_5300350899020605715[29] = 0;
+   out_5300350899020605715[30] = 1.0;
+   out_5300350899020605715[31] = 0;
+   out_5300350899020605715[32] = 0;
+   out_5300350899020605715[33] = 0;
+   out_5300350899020605715[34] = 0;
+   out_5300350899020605715[35] = 0;
+   out_5300350899020605715[36] = 0;
+   out_5300350899020605715[37] = 0;
+   out_5300350899020605715[38] = 0;
+   out_5300350899020605715[39] = 0;
+   out_5300350899020605715[40] = 1.0;
+   out_5300350899020605715[41] = 0;
+   out_5300350899020605715[42] = 0;
+   out_5300350899020605715[43] = 0;
+   out_5300350899020605715[44] = 0;
+   out_5300350899020605715[45] = 0;
+   out_5300350899020605715[46] = 0;
+   out_5300350899020605715[47] = 0;
+   out_5300350899020605715[48] = 0;
+   out_5300350899020605715[49] = 0;
+   out_5300350899020605715[50] = 1.0;
+   out_5300350899020605715[51] = 0;
+   out_5300350899020605715[52] = 0;
+   out_5300350899020605715[53] = 0;
+   out_5300350899020605715[54] = 0;
+   out_5300350899020605715[55] = 0;
+   out_5300350899020605715[56] = 0;
+   out_5300350899020605715[57] = 0;
+   out_5300350899020605715[58] = 0;
+   out_5300350899020605715[59] = 0;
+   out_5300350899020605715[60] = 1.0;
+   out_5300350899020605715[61] = 0;
+   out_5300350899020605715[62] = 0;
+   out_5300350899020605715[63] = 0;
+   out_5300350899020605715[64] = 0;
+   out_5300350899020605715[65] = 0;
+   out_5300350899020605715[66] = 0;
+   out_5300350899020605715[67] = 0;
+   out_5300350899020605715[68] = 0;
+   out_5300350899020605715[69] = 0;
+   out_5300350899020605715[70] = 1.0;
+   out_5300350899020605715[71] = 0;
+   out_5300350899020605715[72] = 0;
+   out_5300350899020605715[73] = 0;
+   out_5300350899020605715[74] = 0;
+   out_5300350899020605715[75] = 0;
+   out_5300350899020605715[76] = 0;
+   out_5300350899020605715[77] = 0;
+   out_5300350899020605715[78] = 0;
+   out_5300350899020605715[79] = 0;
+   out_5300350899020605715[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_7290139574394395979) {
+   out_7290139574394395979[0] = state[0];
+   out_7290139574394395979[1] = state[1];
+   out_7290139574394395979[2] = state[2];
+   out_7290139574394395979[3] = state[3];
+   out_7290139574394395979[4] = state[4];
+   out_7290139574394395979[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_7290139574394395979[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_7290139574394395979[7] = state[7];
+   out_7290139574394395979[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_3369922846592360961) {
+   out_3369922846592360961[0] = 1;
+   out_3369922846592360961[1] = 0;
+   out_3369922846592360961[2] = 0;
+   out_3369922846592360961[3] = 0;
+   out_3369922846592360961[4] = 0;
+   out_3369922846592360961[5] = 0;
+   out_3369922846592360961[6] = 0;
+   out_3369922846592360961[7] = 0;
+   out_3369922846592360961[8] = 0;
+   out_3369922846592360961[9] = 0;
+   out_3369922846592360961[10] = 1;
+   out_3369922846592360961[11] = 0;
+   out_3369922846592360961[12] = 0;
+   out_3369922846592360961[13] = 0;
+   out_3369922846592360961[14] = 0;
+   out_3369922846592360961[15] = 0;
+   out_3369922846592360961[16] = 0;
+   out_3369922846592360961[17] = 0;
+   out_3369922846592360961[18] = 0;
+   out_3369922846592360961[19] = 0;
+   out_3369922846592360961[20] = 1;
+   out_3369922846592360961[21] = 0;
+   out_3369922846592360961[22] = 0;
+   out_3369922846592360961[23] = 0;
+   out_3369922846592360961[24] = 0;
+   out_3369922846592360961[25] = 0;
+   out_3369922846592360961[26] = 0;
+   out_3369922846592360961[27] = 0;
+   out_3369922846592360961[28] = 0;
+   out_3369922846592360961[29] = 0;
+   out_3369922846592360961[30] = 1;
+   out_3369922846592360961[31] = 0;
+   out_3369922846592360961[32] = 0;
+   out_3369922846592360961[33] = 0;
+   out_3369922846592360961[34] = 0;
+   out_3369922846592360961[35] = 0;
+   out_3369922846592360961[36] = 0;
+   out_3369922846592360961[37] = 0;
+   out_3369922846592360961[38] = 0;
+   out_3369922846592360961[39] = 0;
+   out_3369922846592360961[40] = 1;
+   out_3369922846592360961[41] = 0;
+   out_3369922846592360961[42] = 0;
+   out_3369922846592360961[43] = 0;
+   out_3369922846592360961[44] = 0;
+   out_3369922846592360961[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_3369922846592360961[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_3369922846592360961[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3369922846592360961[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3369922846592360961[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_3369922846592360961[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_3369922846592360961[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_3369922846592360961[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3369922846592360961[53] = -9.8000000000000007*dt;
+   out_3369922846592360961[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_3369922846592360961[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_3369922846592360961[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3369922846592360961[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3369922846592360961[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_3369922846592360961[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_3369922846592360961[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_3369922846592360961[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3369922846592360961[62] = 0;
+   out_3369922846592360961[63] = 0;
+   out_3369922846592360961[64] = 0;
+   out_3369922846592360961[65] = 0;
+   out_3369922846592360961[66] = 0;
+   out_3369922846592360961[67] = 0;
+   out_3369922846592360961[68] = 0;
+   out_3369922846592360961[69] = 0;
+   out_3369922846592360961[70] = 1;
+   out_3369922846592360961[71] = 0;
+   out_3369922846592360961[72] = 0;
+   out_3369922846592360961[73] = 0;
+   out_3369922846592360961[74] = 0;
+   out_3369922846592360961[75] = 0;
+   out_3369922846592360961[76] = 0;
+   out_3369922846592360961[77] = 0;
+   out_3369922846592360961[78] = 0;
+   out_3369922846592360961[79] = 0;
+   out_3369922846592360961[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_213866987500689272) {
+   out_213866987500689272[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_8338833395014389035) {
+   out_8338833395014389035[0] = 0;
+   out_8338833395014389035[1] = 0;
+   out_8338833395014389035[2] = 0;
+   out_8338833395014389035[3] = 0;
+   out_8338833395014389035[4] = 0;
+   out_8338833395014389035[5] = 0;
+   out_8338833395014389035[6] = 1;
+   out_8338833395014389035[7] = 0;
+   out_8338833395014389035[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_6813311767306827479) {
+   out_6813311767306827479[0] = state[4];
+   out_6813311767306827479[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_3470018529986682183) {
+   out_3470018529986682183[0] = 0;
+   out_3470018529986682183[1] = 0;
+   out_3470018529986682183[2] = 0;
+   out_3470018529986682183[3] = 0;
+   out_3470018529986682183[4] = 1;
+   out_3470018529986682183[5] = 0;
+   out_3470018529986682183[6] = 0;
+   out_3470018529986682183[7] = 0;
+   out_3470018529986682183[8] = 0;
+   out_3470018529986682183[9] = 0;
+   out_3470018529986682183[10] = 0;
+   out_3470018529986682183[11] = 0;
+   out_3470018529986682183[12] = 0;
+   out_3470018529986682183[13] = 0;
+   out_3470018529986682183[14] = 1;
+   out_3470018529986682183[15] = 0;
+   out_3470018529986682183[16] = 0;
+   out_3470018529986682183[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_3161592977856174233) {
+   out_3161592977856174233[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_3811137064886780837) {
+   out_3811137064886780837[0] = 0;
+   out_3811137064886780837[1] = 0;
+   out_3811137064886780837[2] = 0;
+   out_3811137064886780837[3] = 0;
+   out_3811137064886780837[4] = 1;
+   out_3811137064886780837[5] = 0;
+   out_3811137064886780837[6] = 0;
+   out_3811137064886780837[7] = 0;
+   out_3811137064886780837[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_4105102830757836668) {
+   out_4105102830757836668[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_4597330076140332811) {
+   out_4597330076140332811[0] = 0;
+   out_4597330076140332811[1] = 0;
+   out_4597330076140332811[2] = 0;
+   out_4597330076140332811[3] = 0;
+   out_4597330076140332811[4] = 0;
+   out_4597330076140332811[5] = 0;
+   out_4597330076140332811[6] = 0;
+   out_4597330076140332811[7] = 1;
+   out_4597330076140332811[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_8017746421296364907) {
+   out_8017746421296364907[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_1636373753086355926) {
+   out_1636373753086355926[0] = 0;
+   out_1636373753086355926[1] = 0;
+   out_1636373753086355926[2] = 0;
+   out_1636373753086355926[3] = 1;
+   out_1636373753086355926[4] = 0;
+   out_1636373753086355926[5] = 0;
+   out_1636373753086355926[6] = 0;
+   out_1636373753086355926[7] = 0;
+   out_1636373753086355926[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_3764908862532651750) {
+   out_3764908862532651750[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_4321368409201173021) {
+   out_4321368409201173021[0] = 0;
+   out_4321368409201173021[1] = 1;
+   out_4321368409201173021[2] = 0;
+   out_4321368409201173021[3] = 0;
+   out_4321368409201173021[4] = 0;
+   out_4321368409201173021[5] = 0;
+   out_4321368409201173021[6] = 0;
+   out_4321368409201173021[7] = 0;
+   out_4321368409201173021[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_8845296254147321726) {
+   out_8845296254147321726[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_6284998680766499272) {
+   out_6284998680766499272[0] = 1;
+   out_6284998680766499272[1] = 0;
+   out_6284998680766499272[2] = 0;
+   out_6284998680766499272[3] = 0;
+   out_6284998680766499272[4] = 0;
+   out_6284998680766499272[5] = 0;
+   out_6284998680766499272[6] = 0;
+   out_6284998680766499272[7] = 0;
+   out_6284998680766499272[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_7667405663253359913) {
+   out_7667405663253359913[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_3971121973906981335) {
+   out_3971121973906981335[0] = 0;
+   out_3971121973906981335[1] = 0;
+   out_3971121973906981335[2] = 0;
+   out_3971121973906981335[3] = 0;
+   out_3971121973906981335[4] = 0;
+   out_3971121973906981335[5] = 0;
+   out_3971121973906981335[6] = 0;
+   out_3971121973906981335[7] = 0;
+   out_3971121973906981335[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_5539204589909662238) {
+  err_fun(nom_x, delta_x, out_5539204589909662238);
 }
-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_6836761615042086214) {
+  inv_err_fun(nom_x, true_x, out_6836761615042086214);
 }
-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_5300350899020605715) {
+  H_mod_fun(state, out_5300350899020605715);
 }
-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_7290139574394395979) {
+  f_fun(state,  dt, out_7290139574394395979);
 }
-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_3369922846592360961) {
+  F_fun(state,  dt, out_3369922846592360961);
 }
-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_213866987500689272) {
+  h_25(state, unused, out_213866987500689272);
 }
-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_8338833395014389035) {
+  H_25(state, unused, out_8338833395014389035);
 }
-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_6813311767306827479) {
+  h_24(state, unused, out_6813311767306827479);
 }
-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_3470018529986682183) {
+  H_24(state, unused, out_3470018529986682183);
 }
-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_3161592977856174233) {
+  h_30(state, unused, out_3161592977856174233);
 }
-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_3811137064886780837) {
+  H_30(state, unused, out_3811137064886780837);
 }
-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_4105102830757836668) {
+  h_26(state, unused, out_4105102830757836668);
 }
-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_4597330076140332811) {
+  H_26(state, unused, out_4597330076140332811);
 }
-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_8017746421296364907) {
+  h_27(state, unused, out_8017746421296364907);
 }
-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_1636373753086355926) {
+  H_27(state, unused, out_1636373753086355926);
 }
-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_3764908862532651750) {
+  h_29(state, unused, out_3764908862532651750);
 }
-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_4321368409201173021) {
+  H_29(state, unused, out_4321368409201173021);
 }
-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_8845296254147321726) {
+  h_28(state, unused, out_8845296254147321726);
 }
-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_6284998680766499272) {
+  H_28(state, unused, out_6284998680766499272);
 }
-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_7667405663253359913) {
+  h_31(state, unused, out_7667405663253359913);
 }
-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_3971121973906981335) {
+  H_31(state, unused, out_3971121973906981335);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
   predict(in_x, in_P, in_Q, dt);

selfdrive/locationd/models/generated/car.h

@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_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_5539204589909662238);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_6836761615042086214);
+void car_H_mod_fun(double *state, double *out_5300350899020605715);
+void car_f_fun(double *state, double dt, double *out_7290139574394395979);
+void car_F_fun(double *state, double dt, double *out_3369922846592360961);
+void car_h_25(double *state, double *unused, double *out_213866987500689272);
+void car_H_25(double *state, double *unused, double *out_8338833395014389035);
+void car_h_24(double *state, double *unused, double *out_6813311767306827479);
+void car_H_24(double *state, double *unused, double *out_3470018529986682183);
+void car_h_30(double *state, double *unused, double *out_3161592977856174233);
+void car_H_30(double *state, double *unused, double *out_3811137064886780837);
+void car_h_26(double *state, double *unused, double *out_4105102830757836668);
+void car_H_26(double *state, double *unused, double *out_4597330076140332811);
+void car_h_27(double *state, double *unused, double *out_8017746421296364907);
+void car_H_27(double *state, double *unused, double *out_1636373753086355926);
+void car_h_29(double *state, double *unused, double *out_3764908862532651750);
+void car_H_29(double *state, double *unused, double *out_4321368409201173021);
+void car_h_28(double *state, double *unused, double *out_8845296254147321726);
+void car_H_28(double *state, double *unused, double *out_6284998680766499272);
+void car_h_31(double *state, double *unused, double *out_7667405663253359913);
+void car_H_31(double *state, double *unused, double *out_3971121973906981335);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);

selfdrive/locationd/models/generated/gnss.cpp

@@ -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_3532684675618107860) {
+   out_3532684675618107860[0] = delta_x[0] + nom_x[0];
+   out_3532684675618107860[1] = delta_x[1] + nom_x[1];
+   out_3532684675618107860[2] = delta_x[2] + nom_x[2];
+   out_3532684675618107860[3] = delta_x[3] + nom_x[3];
+   out_3532684675618107860[4] = delta_x[4] + nom_x[4];
+   out_3532684675618107860[5] = delta_x[5] + nom_x[5];
+   out_3532684675618107860[6] = delta_x[6] + nom_x[6];
+   out_3532684675618107860[7] = delta_x[7] + nom_x[7];
+   out_3532684675618107860[8] = delta_x[8] + nom_x[8];
+   out_3532684675618107860[9] = delta_x[9] + nom_x[9];
+   out_3532684675618107860[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_4040997643746645524) {
+   out_4040997643746645524[0] = -nom_x[0] + true_x[0];
+   out_4040997643746645524[1] = -nom_x[1] + true_x[1];
+   out_4040997643746645524[2] = -nom_x[2] + true_x[2];
+   out_4040997643746645524[3] = -nom_x[3] + true_x[3];
+   out_4040997643746645524[4] = -nom_x[4] + true_x[4];
+   out_4040997643746645524[5] = -nom_x[5] + true_x[5];
+   out_4040997643746645524[6] = -nom_x[6] + true_x[6];
+   out_4040997643746645524[7] = -nom_x[7] + true_x[7];
+   out_4040997643746645524[8] = -nom_x[8] + true_x[8];
+   out_4040997643746645524[9] = -nom_x[9] + true_x[9];
+   out_4040997643746645524[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_5907768448464933327) {
+   out_5907768448464933327[0] = 1.0;
+   out_5907768448464933327[1] = 0;
+   out_5907768448464933327[2] = 0;
+   out_5907768448464933327[3] = 0;
+   out_5907768448464933327[4] = 0;
+   out_5907768448464933327[5] = 0;
+   out_5907768448464933327[6] = 0;
+   out_5907768448464933327[7] = 0;
+   out_5907768448464933327[8] = 0;
+   out_5907768448464933327[9] = 0;
+   out_5907768448464933327[10] = 0;
+   out_5907768448464933327[11] = 0;
+   out_5907768448464933327[12] = 1.0;
+   out_5907768448464933327[13] = 0;
+   out_5907768448464933327[14] = 0;
+   out_5907768448464933327[15] = 0;
+   out_5907768448464933327[16] = 0;
+   out_5907768448464933327[17] = 0;
+   out_5907768448464933327[18] = 0;
+   out_5907768448464933327[19] = 0;
+   out_5907768448464933327[20] = 0;
+   out_5907768448464933327[21] = 0;
+   out_5907768448464933327[22] = 0;
+   out_5907768448464933327[23] = 0;
+   out_5907768448464933327[24] = 1.0;
+   out_5907768448464933327[25] = 0;
+   out_5907768448464933327[26] = 0;
+   out_5907768448464933327[27] = 0;
+   out_5907768448464933327[28] = 0;
+   out_5907768448464933327[29] = 0;
+   out_5907768448464933327[30] = 0;
+   out_5907768448464933327[31] = 0;
+   out_5907768448464933327[32] = 0;
+   out_5907768448464933327[33] = 0;
+   out_5907768448464933327[34] = 0;
+   out_5907768448464933327[35] = 0;
+   out_5907768448464933327[36] = 1.0;
+   out_5907768448464933327[37] = 0;
+   out_5907768448464933327[38] = 0;
+   out_5907768448464933327[39] = 0;
+   out_5907768448464933327[40] = 0;
+   out_5907768448464933327[41] = 0;
+   out_5907768448464933327[42] = 0;
+   out_5907768448464933327[43] = 0;
+   out_5907768448464933327[44] = 0;
+   out_5907768448464933327[45] = 0;
+   out_5907768448464933327[46] = 0;
+   out_5907768448464933327[47] = 0;
+   out_5907768448464933327[48] = 1.0;
+   out_5907768448464933327[49] = 0;
+   out_5907768448464933327[50] = 0;
+   out_5907768448464933327[51] = 0;
+   out_5907768448464933327[52] = 0;
+   out_5907768448464933327[53] = 0;
+   out_5907768448464933327[54] = 0;
+   out_5907768448464933327[55] = 0;
+   out_5907768448464933327[56] = 0;
+   out_5907768448464933327[57] = 0;
+   out_5907768448464933327[58] = 0;
+   out_5907768448464933327[59] = 0;
+   out_5907768448464933327[60] = 1.0;
+   out_5907768448464933327[61] = 0;
+   out_5907768448464933327[62] = 0;
+   out_5907768448464933327[63] = 0;
+   out_5907768448464933327[64] = 0;
+   out_5907768448464933327[65] = 0;
+   out_5907768448464933327[66] = 0;
+   out_5907768448464933327[67] = 0;
+   out_5907768448464933327[68] = 0;
+   out_5907768448464933327[69] = 0;
+   out_5907768448464933327[70] = 0;
+   out_5907768448464933327[71] = 0;
+   out_5907768448464933327[72] = 1.0;
+   out_5907768448464933327[73] = 0;
+   out_5907768448464933327[74] = 0;
+   out_5907768448464933327[75] = 0;
+   out_5907768448464933327[76] = 0;
+   out_5907768448464933327[77] = 0;
+   out_5907768448464933327[78] = 0;
+   out_5907768448464933327[79] = 0;
+   out_5907768448464933327[80] = 0;
+   out_5907768448464933327[81] = 0;
+   out_5907768448464933327[82] = 0;
+   out_5907768448464933327[83] = 0;
+   out_5907768448464933327[84] = 1.0;
+   out_5907768448464933327[85] = 0;
+   out_5907768448464933327[86] = 0;
+   out_5907768448464933327[87] = 0;
+   out_5907768448464933327[88] = 0;
+   out_5907768448464933327[89] = 0;
+   out_5907768448464933327[90] = 0;
+   out_5907768448464933327[91] = 0;
+   out_5907768448464933327[92] = 0;
+   out_5907768448464933327[93] = 0;
+   out_5907768448464933327[94] = 0;
+   out_5907768448464933327[95] = 0;
+   out_5907768448464933327[96] = 1.0;
+   out_5907768448464933327[97] = 0;
+   out_5907768448464933327[98] = 0;
+   out_5907768448464933327[99] = 0;
+   out_5907768448464933327[100] = 0;
+   out_5907768448464933327[101] = 0;
+   out_5907768448464933327[102] = 0;
+   out_5907768448464933327[103] = 0;
+   out_5907768448464933327[104] = 0;
+   out_5907768448464933327[105] = 0;
+   out_5907768448464933327[106] = 0;
+   out_5907768448464933327[107] = 0;
+   out_5907768448464933327[108] = 1.0;
+   out_5907768448464933327[109] = 0;
+   out_5907768448464933327[110] = 0;
+   out_5907768448464933327[111] = 0;
+   out_5907768448464933327[112] = 0;
+   out_5907768448464933327[113] = 0;
+   out_5907768448464933327[114] = 0;
+   out_5907768448464933327[115] = 0;
+   out_5907768448464933327[116] = 0;
+   out_5907768448464933327[117] = 0;
+   out_5907768448464933327[118] = 0;
+   out_5907768448464933327[119] = 0;
+   out_5907768448464933327[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_8719739038902116923) {
+   out_8719739038902116923[0] = dt*state[3] + state[0];
+   out_8719739038902116923[1] = dt*state[4] + state[1];
+   out_8719739038902116923[2] = dt*state[5] + state[2];
+   out_8719739038902116923[3] = state[3];
+   out_8719739038902116923[4] = state[4];
+   out_8719739038902116923[5] = state[5];
+   out_8719739038902116923[6] = dt*state[7] + state[6];
+   out_8719739038902116923[7] = dt*state[8] + state[7];
+   out_8719739038902116923[8] = state[8];
+   out_8719739038902116923[9] = state[9];
+   out_8719739038902116923[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_6368958041110140577) {
+   out_6368958041110140577[0] = 1;
+   out_6368958041110140577[1] = 0;
+   out_6368958041110140577[2] = 0;
+   out_6368958041110140577[3] = dt;
+   out_6368958041110140577[4] = 0;
+   out_6368958041110140577[5] = 0;
+   out_6368958041110140577[6] = 0;
+   out_6368958041110140577[7] = 0;
+   out_6368958041110140577[8] = 0;
+   out_6368958041110140577[9] = 0;
+   out_6368958041110140577[10] = 0;
+   out_6368958041110140577[11] = 0;
+   out_6368958041110140577[12] = 1;
+   out_6368958041110140577[13] = 0;
+   out_6368958041110140577[14] = 0;
+   out_6368958041110140577[15] = dt;
+   out_6368958041110140577[16] = 0;
+   out_6368958041110140577[17] = 0;
+   out_6368958041110140577[18] = 0;
+   out_6368958041110140577[19] = 0;
+   out_6368958041110140577[20] = 0;
+   out_6368958041110140577[21] = 0;
+   out_6368958041110140577[22] = 0;
+   out_6368958041110140577[23] = 0;
+   out_6368958041110140577[24] = 1;
+   out_6368958041110140577[25] = 0;
+   out_6368958041110140577[26] = 0;
+   out_6368958041110140577[27] = dt;
+   out_6368958041110140577[28] = 0;
+   out_6368958041110140577[29] = 0;
+   out_6368958041110140577[30] = 0;
+   out_6368958041110140577[31] = 0;
+   out_6368958041110140577[32] = 0;
+   out_6368958041110140577[33] = 0;
+   out_6368958041110140577[34] = 0;
+   out_6368958041110140577[35] = 0;
+   out_6368958041110140577[36] = 1;
+   out_6368958041110140577[37] = 0;
+   out_6368958041110140577[38] = 0;
+   out_6368958041110140577[39] = 0;
+   out_6368958041110140577[40] = 0;
+   out_6368958041110140577[41] = 0;
+   out_6368958041110140577[42] = 0;
+   out_6368958041110140577[43] = 0;
+   out_6368958041110140577[44] = 0;
+   out_6368958041110140577[45] = 0;
+   out_6368958041110140577[46] = 0;
+   out_6368958041110140577[47] = 0;
+   out_6368958041110140577[48] = 1;
+   out_6368958041110140577[49] = 0;
+   out_6368958041110140577[50] = 0;
+   out_6368958041110140577[51] = 0;
+   out_6368958041110140577[52] = 0;
+   out_6368958041110140577[53] = 0;
+   out_6368958041110140577[54] = 0;
+   out_6368958041110140577[55] = 0;
+   out_6368958041110140577[56] = 0;
+   out_6368958041110140577[57] = 0;
+   out_6368958041110140577[58] = 0;
+   out_6368958041110140577[59] = 0;
+   out_6368958041110140577[60] = 1;
+   out_6368958041110140577[61] = 0;
+   out_6368958041110140577[62] = 0;
+   out_6368958041110140577[63] = 0;
+   out_6368958041110140577[64] = 0;
+   out_6368958041110140577[65] = 0;
+   out_6368958041110140577[66] = 0;
+   out_6368958041110140577[67] = 0;
+   out_6368958041110140577[68] = 0;
+   out_6368958041110140577[69] = 0;
+   out_6368958041110140577[70] = 0;
+   out_6368958041110140577[71] = 0;
+   out_6368958041110140577[72] = 1;
+   out_6368958041110140577[73] = dt;
+   out_6368958041110140577[74] = 0;
+   out_6368958041110140577[75] = 0;
+   out_6368958041110140577[76] = 0;
+   out_6368958041110140577[77] = 0;
+   out_6368958041110140577[78] = 0;
+   out_6368958041110140577[79] = 0;
+   out_6368958041110140577[80] = 0;
+   out_6368958041110140577[81] = 0;
+   out_6368958041110140577[82] = 0;
+   out_6368958041110140577[83] = 0;
+   out_6368958041110140577[84] = 1;
+   out_6368958041110140577[85] = dt;
+   out_6368958041110140577[86] = 0;
+   out_6368958041110140577[87] = 0;
+   out_6368958041110140577[88] = 0;
+   out_6368958041110140577[89] = 0;
+   out_6368958041110140577[90] = 0;
+   out_6368958041110140577[91] = 0;
+   out_6368958041110140577[92] = 0;
+   out_6368958041110140577[93] = 0;
+   out_6368958041110140577[94] = 0;
+   out_6368958041110140577[95] = 0;
+   out_6368958041110140577[96] = 1;
+   out_6368958041110140577[97] = 0;
+   out_6368958041110140577[98] = 0;
+   out_6368958041110140577[99] = 0;
+   out_6368958041110140577[100] = 0;
+   out_6368958041110140577[101] = 0;
+   out_6368958041110140577[102] = 0;
+   out_6368958041110140577[103] = 0;
+   out_6368958041110140577[104] = 0;
+   out_6368958041110140577[105] = 0;
+   out_6368958041110140577[106] = 0;
+   out_6368958041110140577[107] = 0;
+   out_6368958041110140577[108] = 1;
+   out_6368958041110140577[109] = 0;
+   out_6368958041110140577[110] = 0;
+   out_6368958041110140577[111] = 0;
+   out_6368958041110140577[112] = 0;
+   out_6368958041110140577[113] = 0;
+   out_6368958041110140577[114] = 0;
+   out_6368958041110140577[115] = 0;
+   out_6368958041110140577[116] = 0;
+   out_6368958041110140577[117] = 0;
+   out_6368958041110140577[118] = 0;
+   out_6368958041110140577[119] = 0;
+   out_6368958041110140577[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_2271110864362118697) {
+   out_2271110864362118697[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_1862672885827164935) {
+   out_1862672885827164935[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_1862672885827164935[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_1862672885827164935[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_1862672885827164935[3] = 0;
+   out_1862672885827164935[4] = 0;
+   out_1862672885827164935[5] = 0;
+   out_1862672885827164935[6] = 1;
+   out_1862672885827164935[7] = 0;
+   out_1862672885827164935[8] = 0;
+   out_1862672885827164935[9] = 0;
+   out_1862672885827164935[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_4324669159508799845) {
+   out_4324669159508799845[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_2084788552121881515) {
+   out_2084788552121881515[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_2084788552121881515[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_2084788552121881515[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_2084788552121881515[3] = 0;
+   out_2084788552121881515[4] = 0;
+   out_2084788552121881515[5] = 0;
+   out_2084788552121881515[6] = 1;
+   out_2084788552121881515[7] = 0;
+   out_2084788552121881515[8] = 0;
+   out_2084788552121881515[9] = 1;
+   out_2084788552121881515[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_5010606036331411486) {
+   out_5010606036331411486[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_3676152182946046175) {
+   out_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[6] = 0;
+   out_3676152182946046175[7] = 1;
+   out_3676152182946046175[8] = 0;
+   out_3676152182946046175[9] = 0;
+   out_3676152182946046175[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_5010606036331411486) {
+   out_5010606036331411486[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_3676152182946046175) {
+   out_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[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_3676152182946046175[6] = 0;
+   out_3676152182946046175[7] = 1;
+   out_3676152182946046175[8] = 0;
+   out_3676152182946046175[9] = 0;
+   out_3676152182946046175[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_3532684675618107860) {
+  err_fun(nom_x, delta_x, out_3532684675618107860);
 }
-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_4040997643746645524) {
+  inv_err_fun(nom_x, true_x, out_4040997643746645524);
 }
-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_5907768448464933327) {
+  H_mod_fun(state, out_5907768448464933327);
 }
-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_8719739038902116923) {
+  f_fun(state,  dt, out_8719739038902116923);
 }
-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_6368958041110140577) {
+  F_fun(state,  dt, out_6368958041110140577);
 }
-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_2271110864362118697) {
+  h_6(state, sat_pos, out_2271110864362118697);
 }
-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_1862672885827164935) {
+  H_6(state, sat_pos, out_1862672885827164935);
 }
-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_4324669159508799845) {
+  h_20(state, sat_pos, out_4324669159508799845);
 }
-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_2084788552121881515) {
+  H_20(state, sat_pos, out_2084788552121881515);
 }
-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_5010606036331411486) {
+  h_7(state, sat_pos_vel, out_5010606036331411486);
 }
-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_3676152182946046175) {
+  H_7(state, sat_pos_vel, out_3676152182946046175);
 }
-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_5010606036331411486) {
+  h_21(state, sat_pos_vel, out_5010606036331411486);
 }
-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_3676152182946046175) {
+  H_21(state, sat_pos_vel, out_3676152182946046175);
 }
 void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
   predict(in_x, in_P, in_Q, dt);

selfdrive/locationd/models/generated/gnss.h

@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void gnss_err_fun(double *nom_x, double *delta_x, double *out_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_3532684675618107860);
+void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_4040997643746645524);
+void gnss_H_mod_fun(double *state, double *out_5907768448464933327);
+void gnss_f_fun(double *state, double dt, double *out_8719739038902116923);
+void gnss_F_fun(double *state, double dt, double *out_6368958041110140577);
+void gnss_h_6(double *state, double *sat_pos, double *out_2271110864362118697);
+void gnss_H_6(double *state, double *sat_pos, double *out_1862672885827164935);
+void gnss_h_20(double *state, double *sat_pos, double *out_4324669159508799845);
+void gnss_H_20(double *state, double *sat_pos, double *out_2084788552121881515);
+void gnss_h_7(double *state, double *sat_pos_vel, double *out_5010606036331411486);
+void gnss_H_7(double *state, double *sat_pos_vel, double *out_3676152182946046175);
+void gnss_h_21(double *state, double *sat_pos_vel, double *out_5010606036331411486);
+void gnss_H_21(double *state, double *sat_pos_vel, double *out_3676152182946046175);
 void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/locationd/models/generated/live.h

@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
 void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_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_4549882242449710464);
+void live_err_fun(double *nom_x, double *delta_x, double *out_8967192143908981963);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_27256379367550061);
+void live_H_mod_fun(double *state, double *out_4824081692195624245);
+void live_f_fun(double *state, double dt, double *out_2925287045854594900);
+void live_F_fun(double *state, double dt, double *out_3843924678963003983);
+void live_h_4(double *state, double *unused, double *out_4595523822163121539);
+void live_H_4(double *state, double *unused, double *out_4041185209348209430);
+void live_h_9(double *state, double *unused, double *out_8234121719314103006);
+void live_H_9(double *state, double *unused, double *out_115982527006568053);
+void live_h_10(double *state, double *unused, double *out_7026349636546874626);
+void live_H_10(double *state, double *unused, double *out_9081879764692710014);
+void live_h_12(double *state, double *unused, double *out_5365386081988364470);
+void live_H_12(double *state, double *unused, double *out_4662284234395803097);
+void live_h_35(double *state, double *unused, double *out_6678787183634411294);
+void live_H_35(double *state, double *unused, double *out_7407847266720816806);
+void live_h_32(double *state, double *unused, double *out_2417329670678050885);
+void live_H_32(double *state, double *unused, double *out_3367483271847903307);
+void live_h_13(double *state, double *unused, double *out_5805477272828296767);
+void live_H_13(double *state, double *unused, double *out_1027302974098499320);
+void live_h_14(double *state, double *unused, double *out_8234121719314103006);
+void live_H_14(double *state, double *unused, double *out_115982527006568053);
+void live_h_33(double *state, double *unused, double *out_7147939535237127299);
+void live_H_33(double *state, double *unused, double *out_7888339802349877206);
 void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

selfdrive/manager/custom_dep.py

@@ -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()

selfdrive/manager/local_osm_install.py

@@ -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()

selfdrive/manager/manager.py

@@ -16,6 +16,7 @@ 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
@@ -100,6 +101,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')))

selfdrive/manager/mapd_installer.py

@@ -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()

selfdrive/manager/process_config.py

@@ -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),
 

selfdrive/mapd/README.md

@@ -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

selfdrive/mapd/config.py

@@ -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.

selfdrive/mapd/lib/NodesData.py

@@ -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

selfdrive/mapd/lib/Route.py

@@ -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

selfdrive/mapd/lib/WayCollection.py

@@ -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)

selfdrive/mapd/lib/WayRelation.py

@@ -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]

selfdrive/mapd/lib/WayRelationIndex.py

@@ -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, [])

selfdrive/mapd/lib/default_speeds.json

@@ -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"
-}

selfdrive/mapd/lib/default_speeds_by_region.json

@@ -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"
-        }
-      }
-    ]
-  }
-}

selfdrive/mapd/lib/geo.py

@@ -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

selfdrive/mapd/lib/helpers.py

@@ -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

selfdrive/mapd/lib/osm.py

@@ -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

selfdrive/mapd/mapd.py

@@ -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()

selfdrive/mapd_manager.py

@@ -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()

selfdrive/sentry.py

@@ -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,27 @@ 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):
+    dongle_id = UNREGISTERED_DONGLE_ID
+  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 +124,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:

selfdrive/sunnypilot/live_map_data/__init__.py

@@ -0,0 +1,18 @@
+from cereal import custom
+from openpilot.system.swaglog import cloudlog
+
+LOOK_AHEAD_HORIZON_TIME = 15.  # s. Time horizon for look ahead of turn speed sections to provide on liveMapDataSP msg.
+_DEBUG = False
+_CLOUDLOG_DEBUG = False
+ROAD_NAME_TIMEOUT = 30  # secs
+DataType = custom.LiveMapDataSP.DataType
+R = 6373000.0  # approximate radius of earth in mts
+QUERY_RADIUS = 3000  # mts. Radius to use on OSM data queries.
+QUERY_RADIUS_OFFLINE = 2250  # mts. Radius to use on offline OSM data queries.
+
+
+def get_debug(msg, log_to_cloud=True):
+  if _CLOUDLOG_DEBUG and log_to_cloud:
+    cloudlog.debug(msg)
+  if _DEBUG:
+    print(msg)

selfdrive/sunnypilot/live_map_data/base_map_data.py

@@ -0,0 +1,106 @@
+import math
+from abc import abstractmethod, ABC
+
+from cereal import custom, messaging
+from openpilot.selfdrive.navd.helpers import Coordinate
+from openpilot.selfdrive.sunnypilot.live_map_data import get_debug
+
+
+class BaseMapData(ABC):
+  def __init__(self):
+    self._last_gps: Coordinate | None = None
+    self._gps_sock = None
+    self._data_type = custom.LiveMapDataSP.DataType.default
+    self._sub_master = messaging.SubMaster(['liveLocationKalman', 'carControl'])
+    self._pub_master = messaging.PubMaster(['liveMapDataSP'])
+
+  @abstractmethod
+  def update_location(self, current_location: Coordinate):
+    pass
+
+  @abstractmethod
+  def get_current_speed_limit(self) -> float:
+    pass
+
+  @abstractmethod
+  def get_next_speed_limit_and_distance(self) -> (float, float):
+    pass
+
+  @abstractmethod
+  def get_current_road_name(self) -> str:
+    pass
+
+  def _is_gps_data_valid(self) -> bool:
+    all_sock_alive = self._sub_master.all_alive(service_list=[self._gps_sock])
+    all_sock_valid = self._sub_master.all_valid(service_list=[self._gps_sock])
+    return all_sock_alive and all_sock_valid
+
+  def get_current_location(self) -> Coordinate | None:
+    self._gps_sock = "liveLocationKalman"
+    if not self._sub_master.updated[self._gps_sock] or not self._sub_master.valid[self._gps_sock]:
+      return None
+
+    _last_gps = self._sub_master[self._gps_sock]
+    # ignore the message if the fix is invalid
+    if not _last_gps.positionGeodetic.valid:
+      return None
+
+    kalman_bearing_deg = math.degrees(_last_gps.calibratedOrientationNED.value[2])
+    kalman_speed = _last_gps.velocityCalibrated.value[2]
+    kalman_latitude = _last_gps.positionGeodetic.value[0];
+    kalman_longitude = _last_gps.positionGeodetic.value[1]
+
+    result = Coordinate(kalman_latitude, kalman_longitude)
+    result.annotations['unixTimestampMillis'] = _last_gps.unixTimestampMillis
+    result.annotations['speed'] = kalman_speed
+    result.annotations['bearingDeg'] = kalman_bearing_deg
+    result.annotations['accuracy'] = 1  # Hardcoded since liveLocationKalman does not report this.
+    result.annotations['bearingAccuracyDeg'] = 1.  # you'll need to assign this if available
+
+    return result
+
+  def get_live_map_data_sp(self, speed_limit, next_speed_limit, next_speed_limit_distance, current_road_name):
+    last_gps = self.get_current_location()
+    map_data_msg = messaging.new_message('liveMapDataSP')
+    map_data_msg.valid = self._is_gps_data_valid()
+
+    live_map_data = map_data_msg.liveMapDataSP
+    if last_gps:
+      live_map_data.lastGpsTimestamp = last_gps.annotations.get('unixTimestampMillis', 0)
+      live_map_data.lastGpsLatitude = last_gps.latitude
+      live_map_data.lastGpsLongitude = last_gps.longitude
+      live_map_data.lastGpsSpeed = last_gps.annotations.get('speed', 0)
+      live_map_data.lastGpsBearingDeg = last_gps.annotations.get('bearingDeg', 0)
+      live_map_data.lastGpsAccuracy = last_gps.annotations.get('accuracy', 0)
+      live_map_data.lastGpsBearingAccuracyDeg = last_gps.annotations.get('bearingAccuracyDeg', 0)
+
+    live_map_data.speedLimitValid = bool(speed_limit > 0)
+    live_map_data.speedLimit = speed_limit
+    live_map_data.speedLimitAheadValid = bool(next_speed_limit > 0)
+    live_map_data.speedLimitAhead = float(next_speed_limit)
+    live_map_data.speedLimitAheadDistance = float(next_speed_limit_distance)
+    live_map_data.currentRoadName = str(current_road_name)
+    live_map_data.dataType = self._data_type
+
+    return map_data_msg
+
+  def publish(self):
+    speed_limit = self.get_current_speed_limit()
+    current_road_name = self.get_current_road_name()
+    next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
+
+    live_map_data_sp = self.get_live_map_data_sp(
+      speed_limit,
+      next_speed_limit,
+      next_speed_limit_distance,
+      current_road_name
+    )
+
+    self._pub_master.send('liveMapDataSP', live_map_data_sp)
+    get_debug(f"SRC: [{self.__class__.__name__}] | SLC: [{speed_limit}] | NSL: [{next_speed_limit}] | NSLD: [{next_speed_limit_distance}] | CRN: [{current_road_name}] | GPS: [{self._last_gps}] Annotations: [{', '.join(f'{key}: {value}' for key, value in self._last_gps.annotations.items()) if self._last_gps else []}]")
+
+  def tick(self):
+    self._sub_master.update()
+    self._last_gps = self.get_current_location()
+    self.update_location(self._last_gps)
+    self.publish()

selfdrive/sunnypilot/live_map_data/debug.py

@@ -0,0 +1,50 @@
+# DISCLAIMER: This code is intended principally for development and debugging purposes.
+# Although it provides a standalone entry point to the program, users should refer
+# to the actual implementations for consumption. Usage outside of development scenarios
+# is not advised and could lead to unpredictable results.
+
+import threading
+import traceback
+
+from cereal import messaging
+from openpilot.common.realtime import set_core_affinity
+from openpilot.selfdrive.controls.lib.sunnypilot.common import Policy
+from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_resolver import SpeedLimitResolver
+from openpilot.selfdrive.sunnypilot.live_map_data import get_debug
+from openpilot.system.swaglog import cloudlog
+
+
+def excepthook(args):
+  get_debug(f'MapD: Threading exception:\n{args}')
+  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
+
+
+def live_map_data_sp_thread():
+  try:
+    set_core_affinity([0, 1, 2, 3])
+  except Exception:
+    cloudlog.exception("mapd: failed to set core affinity")
+
+  while True:
+    live_map_data_sp_thread_debug()
+
+
+def live_map_data_sp_thread_debug():
+  _sub_master = messaging.SubMaster(['carState', 'navInstruction', 'liveLocationKalman', 'liveMapDataSP', 'longitudinalPlanSP'])
+  _sub_master.update()
+
+  v_ego = _sub_master['carState'].vEgo
+  long_spl = _sub_master['longitudinalPlanSP'].speedLimit
+  _policy = Policy.car_state_priority
+  _resolver = SpeedLimitResolver(_policy)
+  _speed_limit, _distance, _source = _resolver.resolve(v_ego, long_spl, _sub_master)
+  print(_speed_limit, _distance, _source, " <-> ", long_spl)
+
+
+def main():
+  threading.excepthook = excepthook
+  live_map_data_sp_thread()
+
+
+if __name__ == "__main__":
+  main()

selfdrive/sunnypilot/live_map_data/osm_map_data.py

@@ -0,0 +1,50 @@
+import json
+import platform
+
+from openpilot.common.params_pyx import Params
+from openpilot.selfdrive.navd.helpers import Coordinate
+from openpilot.selfdrive.sunnypilot.live_map_data import DataType
+from openpilot.selfdrive.sunnypilot.live_map_data.base_map_data import BaseMapData
+
+
+class OsmMapData(BaseMapData):
+  def __init__(self):
+    super().__init__()
+    self.last_gps = Coordinate(0.0, 0.0)
+    self.params = Params()
+    self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
+    self.data_type = DataType.offline
+
+  def update_location(self, current_location):
+    self.last_gps = current_location
+    if not self.last_gps:
+      return
+
+    last_gps_position_for_osm = {
+      "latitude": self.last_gps.latitude,
+      "longitude": self.last_gps.longitude,
+      "bearing": self.last_gps.annotations.get("bearingDeg", 0)
+    }
+    self.mem_params.put("LastGPSPosition", json.dumps(last_gps_position_for_osm))
+
+  def get_current_speed_limit(self):
+    speed_limit = self.mem_params.get("MapSpeedLimit", encoding='utf8')
+    return float(speed_limit) if speed_limit else 0.0
+
+  def get_current_road_name(self):
+    current_road_name = self.mem_params.get("RoadName", encoding='utf8')
+    return current_road_name if current_road_name else ""
+
+  def get_next_speed_limit_and_distance(self):
+    next_speed_limit_section_str = self.mem_params.get("NextMapSpeedLimit", encoding='utf8')
+    next_speed_limit_section = json.loads(next_speed_limit_section_str) if next_speed_limit_section_str else {}
+    next_speed_limit = next_speed_limit_section.get('speedlimit', 0.0)
+    next_speed_limit_latitude = next_speed_limit_section.get('latitude')
+    next_speed_limit_longitude = next_speed_limit_section.get('longitude')
+    next_speed_limit_distance = 0
+
+    if next_speed_limit_latitude and next_speed_limit_longitude:
+      next_speed_limit_coordinates = Coordinate(next_speed_limit_latitude, next_speed_limit_longitude)
+      next_speed_limit_distance = (self.last_gps or Coordinate(0, 0)).distance_to(next_speed_limit_coordinates)
+
+    return next_speed_limit, next_speed_limit_distance

selfdrive/sunnypilot/live_map_data/standalone.py

@@ -0,0 +1,40 @@
+# DISCLAIMER: This code is intended principally for development and debugging purposes.
+# Although it provides a standalone entry point to the program, users should refer
+# to the actual implementations for consumption. Usage outside of development scenarios
+# is not advised and could lead to unpredictable results.
+
+import threading
+import traceback
+
+from openpilot.common.realtime import Ratekeeper, set_core_affinity
+from openpilot.selfdrive.sunnypilot.live_map_data import get_debug
+from openpilot.selfdrive.sunnypilot.live_map_data.osm_map_data import OsmMapData
+from openpilot.system.swaglog import cloudlog
+
+
+def excepthook(args):
+  get_debug(f'MapD: Threading exception:\n{args}')
+  traceback.print_exception(args.exc_type, args.exc_value, args.exc_traceback)
+
+
+def live_map_data_sp_thread():
+  try:
+    set_core_affinity([0, 1, 2, 3])
+  except Exception:
+    cloudlog.exception("mapd: failed to set core affinity")
+
+  live_map_sp = OsmMapData()
+  rk = Ratekeeper(1, print_delay_threshold=None)
+
+  while True:
+    live_map_sp.tick()
+    rk.keep_time()
+
+
+def main():
+  threading.excepthook = excepthook
+  live_map_data_sp_thread()
+
+
+if __name__ == "__main__":
+  main()