IQ.Pilot Release Commit @ e46d557
This commit is contained in:
IQ.Lvbs CI [bot]
58
iqpilot/common/auto_units.py
Normal file
58
iqpilot/common/auto_units.py
Normal file
@@ -0,0 +1,58 @@
|
||||
import time
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.iqpilot.common.geo_regions import UNKNOWN_REGION, region_for_position, region_is_metric
|
||||
|
||||
CHECK_INTERVAL = 10.0
|
||||
CONFIRMATIONS = 3
|
||||
|
||||
|
||||
class AutoUnits:
|
||||
def __init__(self, params: Params | None = None):
|
||||
self.params = params or Params()
|
||||
self._next_check = 0.0
|
||||
self._candidate = UNKNOWN_REGION
|
||||
self._confirmations = 0
|
||||
|
||||
def _position(self) -> tuple[float, float, bool]:
|
||||
from openpilot.selfdrive.ui.lib.nav_helpers import current_or_last_gps_position
|
||||
|
||||
lat, lon, _, valid = current_or_last_gps_position(self.params)
|
||||
return lat, lon, valid
|
||||
|
||||
def update(self, now: float | None = None) -> None:
|
||||
if not self.params.get_bool("IQAutoUnits"):
|
||||
self._candidate = UNKNOWN_REGION
|
||||
self._confirmations = 0
|
||||
return
|
||||
|
||||
now = time.monotonic() if now is None else now
|
||||
if now < self._next_check:
|
||||
return
|
||||
self._next_check = now + CHECK_INTERVAL
|
||||
|
||||
lat, lon, valid = self._position()
|
||||
region = region_for_position(lat, lon) if valid else UNKNOWN_REGION
|
||||
if region == UNKNOWN_REGION:
|
||||
self._confirmations = 0
|
||||
return
|
||||
|
||||
if region != self._candidate:
|
||||
self._candidate = region
|
||||
self._confirmations = 1
|
||||
return
|
||||
|
||||
self._confirmations += 1
|
||||
if self._confirmations < CONFIRMATIONS:
|
||||
return
|
||||
|
||||
if region == self.params.get("IQAutoUnitsRegion"):
|
||||
return
|
||||
|
||||
self.params.put("IQAutoUnitsRegion", region)
|
||||
|
||||
metric = region_is_metric(region)
|
||||
if metric != self.params.get_bool("IsMetric"):
|
||||
self.params.put_bool("IsMetric", metric)
|
||||
cloudlog.warning(f"auto units: {region} detected, switching to {'km/h' if metric else 'mph'}")
|
||||
140
iqpilot/common/geo_regions.py
Normal file
140
iqpilot/common/geo_regions.py
Normal file
@@ -0,0 +1,140 @@
|
||||
MPH_REGIONS = ("US", "GB", "LR")
|
||||
METRIC_REGION = "METRIC"
|
||||
UNKNOWN_REGION = ""
|
||||
|
||||
_US_CONUS = [
|
||||
(-123.32, 49.00), (-117.03, 49.00), (-110.00, 49.00), (-104.05, 49.00), (-97.23, 49.00), (-95.15, 49.00),
|
||||
(-95.15, 49.38), (-94.82, 49.30), (-94.68, 48.77), (-93.85, 48.63), (-93.35, 48.62), (-92.72, 48.54),
|
||||
(-92.30, 48.24), (-91.55, 48.10), (-90.84, 48.24), (-89.99, 48.02), (-89.60, 48.02), (-89.10, 48.32),
|
||||
(-88.40, 48.30), (-87.00, 47.80), (-85.60, 47.15), (-84.60, 46.75), (-84.42, 46.56), (-84.30, 46.49),
|
||||
(-84.12, 46.28), (-83.90, 46.05), (-83.40, 45.75), (-82.90, 45.05), (-82.55, 44.00), (-82.42, 43.00),
|
||||
(-82.70, 42.47), (-82.93, 42.34), (-83.00, 42.33), (-83.05, 42.32), (-83.075, 42.312), (-83.13, 42.25),
|
||||
(-83.15, 42.18), (-83.11, 42.10), (-83.09, 42.02),
|
||||
(-82.50, 41.70), (-81.50, 42.00), (-80.20, 42.40), (-79.06, 42.85), (-79.05, 43.27), (-78.00, 43.45),
|
||||
(-77.00, 43.65), (-76.40, 44.10), (-75.80, 44.50), (-74.75, 45.00), (-73.35, 45.01), (-71.50, 45.01),
|
||||
(-71.29, 45.30), (-70.90, 45.30), (-70.72, 45.42), (-70.31, 45.86), (-70.05, 46.44), (-69.99, 46.70),
|
||||
(-69.24, 47.46), (-68.90, 47.20), (-68.38, 47.29), (-67.79, 47.07), (-67.78, 45.94), (-67.42, 45.60),
|
||||
(-67.03, 44.80), (-68.00, 44.30), (-69.06, 43.80), (-70.20, 43.60), (-70.80, 42.85), (-70.00, 41.90),
|
||||
(-70.00, 41.55), (-71.20, 41.30), (-72.00, 41.05), (-73.90, 40.55), (-74.20, 39.60), (-75.05, 38.45),
|
||||
(-75.90, 37.05), (-75.50, 35.20), (-78.50, 33.85), (-80.90, 32.00), (-81.40, 30.70), (-80.03, 26.80),
|
||||
(-80.15, 25.15), (-81.20, 24.55), (-82.00, 26.40), (-82.80, 27.80), (-83.00, 29.15), (-84.30, 29.90),
|
||||
(-85.30, 29.65), (-87.50, 30.25), (-89.00, 29.15), (-89.40, 28.95), (-91.30, 29.10), (-93.80, 29.65),
|
||||
(-95.00, 29.10), (-97.10, 27.80), (-97.14, 25.96), (-98.30, 26.05), (-99.10, 26.40), (-99.50, 27.60),
|
||||
(-100.40, 28.50), (-101.40, 29.77), (-102.30, 29.88), (-102.90, 29.30), (-103.30, 29.00), (-104.37, 29.56),
|
||||
(-104.68, 30.13), (-105.30, 30.80), (-105.85, 31.30), (-106.15, 31.50), (-106.30, 31.68), (-106.45, 31.755),
|
||||
(-106.53, 31.786), (-108.21, 31.783), (-108.21, 31.33), (-111.07, 31.33), (-114.72, 32.72),
|
||||
(-117.13, 32.53), (-118.40, 33.75), (-119.80, 34.40), (-120.65, 35.10), (-121.90, 36.60), (-122.52, 37.78),
|
||||
(-123.75, 39.40), (-124.20, 40.45), (-124.15, 42.00), (-124.05, 43.35), (-123.95, 46.25), (-124.75, 48.40),
|
||||
(-123.30, 48.25), (-123.15, 48.70),
|
||||
]
|
||||
|
||||
_US_ALASKA = [
|
||||
(-141.00, 70.20), (-141.00, 60.30), (-139.05, 60.35), (-137.45, 58.95), (-136.47, 59.63), (-135.03, 59.57),
|
||||
(-134.30, 58.90), (-133.40, 58.20), (-132.20, 56.90), (-130.60, 56.20), (-130.01, 54.80), (-131.80, 54.70),
|
||||
(-133.80, 55.90), (-136.60, 58.20), (-140.00, 59.70), (-145.00, 60.00), (-149.20, 59.10), (-152.30, 57.30),
|
||||
(-155.20, 55.60), (-160.00, 54.60), (-164.50, 54.40), (-162.00, 57.50), (-165.00, 60.20), (-167.50, 62.50),
|
||||
(-164.00, 64.50), (-168.10, 65.60), (-166.00, 68.30), (-161.00, 70.30), (-156.50, 71.40), (-150.00, 70.50),
|
||||
]
|
||||
|
||||
_US_ALEUTIANS_EAST = [(-180.00, 51.00), (-158.50, 51.00), (-158.50, 56.00), (-180.00, 56.00)]
|
||||
_US_ALEUTIANS_WEST = [(172.00, 51.00), (180.00, 51.00), (180.00, 54.00), (172.00, 54.00)]
|
||||
_US_HAWAII = [(-160.50, 18.80), (-154.70, 18.80), (-154.70, 22.30), (-160.50, 22.30)]
|
||||
_US_PUERTO_RICO = [(-67.35, 17.85), (-64.55, 17.85), (-64.55, 18.55), (-67.35, 18.55)]
|
||||
_US_MARIANAS = [(144.50, 13.10), (146.20, 13.10), (146.20, 20.60), (144.50, 20.60)]
|
||||
_US_SAMOA = [(-171.20, -14.60), (-168.10, -14.60), (-168.10, -11.00), (-171.20, -11.00)]
|
||||
|
||||
_GB_BRITAIN = [
|
||||
(-5.72, 50.07), (-4.20, 50.32), (-3.41, 50.62), (-2.45, 50.52), (-1.80, 50.72), (-0.90, 50.77),
|
||||
(0.58, 50.85), (1.35, 51.13), (1.38, 51.38), (1.15, 51.79), (1.35, 51.95), (1.75, 52.48),
|
||||
(1.30, 52.94), (0.49, 52.94), (0.34, 53.15), (-0.08, 53.57), (-0.08, 54.12), (-0.61, 54.49),
|
||||
(-1.18, 54.69), (-1.38, 54.91), (-1.50, 55.13), (-2.00, 55.77), (-2.52, 56.00), (-2.62, 56.28),
|
||||
(-2.47, 56.55), (-2.21, 56.96), (-2.08, 57.14), (-1.77, 57.50), (-2.00, 57.70), (-2.96, 57.68),
|
||||
(-3.90, 57.60), (-4.22, 57.48), (-4.05, 57.81), (-3.85, 58.01), (-3.65, 58.12), (-3.09, 58.44),
|
||||
(-3.01, 58.67), (-3.35, 58.62), (-3.52, 58.60), (-4.99, 58.62), (-5.05, 58.45), (-5.16, 57.90), (-5.70, 57.72),
|
||||
(-5.72, 57.28), (-5.83, 57.00), (-5.72, 56.65), (-5.47, 56.41), (-5.79, 55.60), (-5.62, 55.31),
|
||||
(-4.82, 55.64), (-4.63, 55.46), (-4.85, 55.24), (-5.12, 54.84), (-4.86, 54.63), (-4.44, 54.87),
|
||||
(-4.05, 54.83), (-3.26, 54.98), (-3.05, 54.90), (-3.50, 54.72), (-3.23, 54.07), (-3.05, 53.82),
|
||||
(-3.40, 53.34), (-3.83, 53.33), (-4.63, 53.42), (-4.72, 53.28), (-4.35, 53.12), (-4.76, 52.80),
|
||||
(-4.06, 52.72), (-4.09, 52.41), (-4.66, 52.09), (-5.31, 51.88), (-5.06, 51.70), (-4.70, 51.67),
|
||||
(-4.30, 51.62), (-3.95, 51.56), (-3.70, 51.48), (-3.17, 51.45), (-2.99, 51.55), (-2.67, 51.62),
|
||||
(-2.48, 51.72), (-2.30, 51.85), (-2.70, 51.50), (-2.98, 51.35), (-3.00, 51.20), (-3.47, 51.21),
|
||||
(-4.12, 51.21), (-4.55, 50.83), (-5.08, 50.42), (-5.48, 50.21),
|
||||
]
|
||||
|
||||
_GB_NORTHERN_IRELAND = [
|
||||
(-6.03, 54.05), (-6.28, 54.10), (-6.65, 54.17), (-6.86, 54.33), (-7.16, 54.34), (-7.31, 54.12),
|
||||
(-7.62, 54.14), (-8.00, 54.31), (-8.18, 54.47), (-8.20, 54.52), (-7.90, 54.55), (-7.85, 54.72), (-7.55, 54.75),
|
||||
(-7.44, 54.94), (-7.25, 55.06), (-6.95, 55.22), (-6.50, 55.25), (-6.25, 55.31), (-6.03, 55.22),
|
||||
(-5.43, 54.62), (-5.53, 54.24),
|
||||
]
|
||||
|
||||
_GB_ISLE_OF_MAN = [(-4.85, 54.03), (-4.30, 54.03), (-4.30, 54.42), (-4.85, 54.42)]
|
||||
_GB_CHANNEL_ISLANDS = [(-2.75, 49.15), (-1.95, 49.15), (-1.95, 49.80), (-2.75, 49.80)]
|
||||
_GB_ISLE_OF_WIGHT = [(-1.60, 50.55), (-1.05, 50.55), (-1.05, 50.80), (-1.60, 50.80)]
|
||||
_GB_OUTER_HEBRIDES = [(-7.75, 56.75), (-6.05, 56.75), (-6.05, 58.55), (-7.75, 58.55)]
|
||||
_GB_INNER_HEBRIDES = [(-7.00, 55.45), (-5.55, 55.45), (-5.55, 57.85), (-7.00, 57.85)]
|
||||
_GB_ORKNEY = [(-3.50, 58.70), (-2.35, 58.70), (-2.35, 59.45), (-3.50, 59.45)]
|
||||
_GB_SHETLAND = [(-1.85, 59.80), (-0.65, 59.80), (-0.65, 60.90), (-1.85, 60.90)]
|
||||
|
||||
_LR_LIBERIA = [
|
||||
(-11.46, 6.77), (-11.30, 6.95), (-11.16, 7.15), (-11.05, 7.40), (-10.85, 7.75), (-10.60, 8.00),
|
||||
(-10.28, 8.49), (-9.70, 8.54), (-9.35, 7.80),
|
||||
(-8.85, 7.40), (-8.48, 7.55), (-8.30, 6.90), (-7.95, 6.20), (-7.60, 5.20), (-7.40, 4.55),
|
||||
(-7.74, 4.33), (-8.46, 4.61), (-9.06, 4.97), (-9.52, 5.36), (-10.08, 5.85), (-10.40, 6.11),
|
||||
(-10.83, 6.27),
|
||||
]
|
||||
|
||||
_REGION_RINGS = {
|
||||
"US": (_US_CONUS, _US_ALASKA, _US_ALEUTIANS_EAST, _US_ALEUTIANS_WEST, _US_HAWAII, _US_PUERTO_RICO,
|
||||
_US_MARIANAS, _US_SAMOA),
|
||||
"GB": (_GB_BRITAIN, _GB_NORTHERN_IRELAND, _GB_ISLE_OF_MAN, _GB_CHANNEL_ISLANDS, _GB_ISLE_OF_WIGHT,
|
||||
_GB_OUTER_HEBRIDES, _GB_INNER_HEBRIDES, _GB_ORKNEY, _GB_SHETLAND),
|
||||
"LR": (_LR_LIBERIA,),
|
||||
}
|
||||
|
||||
|
||||
def _bounded(rings):
|
||||
out = []
|
||||
for ring in rings:
|
||||
lons = [p[0] for p in ring]
|
||||
lats = [p[1] for p in ring]
|
||||
out.append(((min(lons), min(lats), max(lons), max(lats)), ring))
|
||||
return tuple(out)
|
||||
|
||||
|
||||
_REGIONS = tuple((region, _bounded(rings)) for region, rings in _REGION_RINGS.items())
|
||||
|
||||
|
||||
def _point_in_ring(lat: float, lon: float, ring) -> bool:
|
||||
inside = False
|
||||
count = len(ring)
|
||||
j = count - 1
|
||||
for i in range(count):
|
||||
lon_i, lat_i = ring[i]
|
||||
lon_j, lat_j = ring[j]
|
||||
if (lat_i > lat) != (lat_j > lat):
|
||||
crossing = (lon_j - lon_i) * (lat - lat_i) / (lat_j - lat_i) + lon_i
|
||||
if lon < crossing:
|
||||
inside = not inside
|
||||
j = i
|
||||
return inside
|
||||
|
||||
|
||||
def valid_position(lat: float, lon: float) -> bool:
|
||||
return abs(lat) <= 90.0 and abs(lon) <= 180.0 and (abs(lat) > 1e-4 or abs(lon) > 1e-4)
|
||||
|
||||
|
||||
def region_for_position(lat: float, lon: float) -> str:
|
||||
if not valid_position(lat, lon):
|
||||
return UNKNOWN_REGION
|
||||
|
||||
for region, rings in _REGIONS:
|
||||
for (min_lon, min_lat, max_lon, max_lat), ring in rings:
|
||||
if min_lon <= lon <= max_lon and min_lat <= lat <= max_lat and _point_in_ring(lat, lon, ring):
|
||||
return region
|
||||
|
||||
return METRIC_REGION
|
||||
|
||||
|
||||
def region_is_metric(region: str) -> bool:
|
||||
return bool(region) and region not in MPH_REGIONS
|
||||
@@ -4,27 +4,22 @@ EARTH_RADIUS = 6378137
|
||||
# Mapbox API limits
|
||||
FREE_MAPBOX_REQUESTS = 100_000
|
||||
|
||||
# Speed limit offset maps for different unit systems
|
||||
# Each entry is (min_speed_ms, max_speed_ms, param_name)
|
||||
# Speed limit offset zones for different unit systems
|
||||
# Each entry is (min_speed_ms, max_speed_ms, param_name); the param value is a
|
||||
# percent offset applied to the resolved limit (e.g. 10 -> +10%), lower bound inclusive
|
||||
|
||||
OFFSET_PERCENT_MAX = 50.0
|
||||
|
||||
OFFSET_MAP_IMPERIAL = [
|
||||
(0, 11.2, "speed_limit_offset1"), # 0-24 mph
|
||||
(11.2, 15.2, "speed_limit_offset2"), # 25-34 mph
|
||||
(15.2, 19.6, "speed_limit_offset3"), # 35-44 mph
|
||||
(19.6, 24.1, "speed_limit_offset4"), # 45-54 mph
|
||||
(24.1, 28.6, "speed_limit_offset5"), # 55-64 mph
|
||||
(28.6, 33.1, "speed_limit_offset6"), # 65-74 mph
|
||||
(33.1, 44.2, "speed_limit_offset7"), # 75-99 mph
|
||||
(0, 8.94, "speed_limit_offset1"), # 0-20 mph
|
||||
(8.94, 17.88, "speed_limit_offset2"), # 20-40 mph
|
||||
(17.88, float("inf"), "speed_limit_offset3"), # 40+ mph
|
||||
]
|
||||
|
||||
OFFSET_MAP_METRIC = [
|
||||
(0, 8.1, "speed_limit_offset1"), # 0-29 km/h
|
||||
(8.1, 13.6, "speed_limit_offset2"), # 30-49 km/h
|
||||
(13.6, 16.4, "speed_limit_offset3"), # 50-59 km/h
|
||||
(16.4, 21.9, "speed_limit_offset4"), # 60-79 km/h
|
||||
(21.9, 27.5, "speed_limit_offset5"), # 80-99 km/h
|
||||
(27.5, 33.1, "speed_limit_offset6"), # 100-119 km/h
|
||||
(33.1, 38.9, "speed_limit_offset7"), # 120-140 km/h
|
||||
(0, 8.33, "speed_limit_offset1"), # 0-30 km/h
|
||||
(8.33, 16.67, "speed_limit_offset2"), # 30-60 km/h
|
||||
(16.67, float("inf"), "speed_limit_offset3"), # 60+ km/h
|
||||
]
|
||||
|
||||
# Speed limit filler constants
|
||||
|
||||
0
iqpilot/konn3kt/canlive/__init__.py
Normal file
0
iqpilot/konn3kt/canlive/__init__.py
Normal file
108
iqpilot/konn3kt/canlive/canlived.py
Executable file
108
iqpilot/konn3kt/canlive/canlived.py
Executable file
@@ -0,0 +1,108 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
canlived — live CAN bridge to konn3kt.
|
||||
|
||||
Streams the device's live CAN bus to the konn3kt server so it can be viewed remotely
|
||||
in Cabana (via a local ZMQ proxy on the laptop). This is the remote analogue of running
|
||||
`./cereal/messaging/bridge` locally: instead of re-publishing CAN over a LAN ZMQ socket,
|
||||
canlived opens its OWN websocket to konn3kt and forwards the raw capnp `Event` frames.
|
||||
|
||||
It is deliberately a separate daemon (not part of hephaestusd's control websocket):
|
||||
* hephaestusd's send path fragments everything as TEXT frames through one queue, which
|
||||
cannot carry binary capnp and would head-of-line-block the control plane at 100Hz.
|
||||
* a dedicated socket means CAN traffic and control traffic never contend.
|
||||
|
||||
Lifecycle: the manager launches canlived only while the `CanLiveStreaming` param is set.
|
||||
hephaestusd sets/clears that param via startCanLive/stopCanLive, which the konn3kt server
|
||||
calls when the first viewer connects / the last viewer disconnects. So canlived runs only
|
||||
during an active debug session — no idle connections, no battery/data cost otherwise.
|
||||
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved.
|
||||
"""
|
||||
import os
|
||||
import threading
|
||||
|
||||
from websocket import ABNF, create_connection
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.api import Api
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
# Cabana's live "Device" stream subscribes only to "can", so that's all we forward to
|
||||
# match the local experience exactly. (sendcan/TX is not shown by the live device view.)
|
||||
CAN_SERVICES = ["can"]
|
||||
|
||||
# Reconnect backoff bounds (seconds).
|
||||
RECONNECT_MIN = 1.0
|
||||
RECONNECT_MAX = 10.0
|
||||
|
||||
|
||||
def _api_host() -> str:
|
||||
# Same host hephaestusd talks to; force the websocket scheme.
|
||||
host = os.getenv("HEPHAESTUS_HOST") or os.getenv("KONN3KT_API_HOST") or "wss://api-iqlabs.konn3kt.com"
|
||||
host = host.rstrip("/")
|
||||
if host.startswith("https://"):
|
||||
host = "wss://" + host[len("https://"):]
|
||||
elif host.startswith("http://"):
|
||||
host = "ws://" + host[len("http://"):]
|
||||
return host
|
||||
|
||||
|
||||
def _stream_once(dongle_id: str, ws_uri: str, token: str, exit_event: threading.Event) -> None:
|
||||
"""Open one websocket and pump CAN until it drops or we're asked to exit."""
|
||||
ws = create_connection(ws_uri, cookie="jwt=" + token, enable_multithread=True, timeout=30.0)
|
||||
cloudlog.info("canlived: connected to %s", ws_uri)
|
||||
try:
|
||||
# Blocking receive with a short timeout so we periodically re-check exit_event and
|
||||
# the socket stays responsive to shutdown even when the bus is quiet.
|
||||
socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in CAN_SERVICES]
|
||||
while not exit_event.is_set():
|
||||
got_any = False
|
||||
for sock in socks:
|
||||
while True:
|
||||
raw = sock.receive(non_blocking=True)
|
||||
if raw is None:
|
||||
break
|
||||
got_any = True
|
||||
# Forward the exact capnp Event bytes as a single binary frame. canlived owns
|
||||
# this socket, so there is no fragmentation/interleaving to worry about.
|
||||
ws.send_frame(ABNF.create_frame(raw, ABNF.OPCODE_BINARY, 1))
|
||||
if not got_any:
|
||||
# Nothing pending across any sub — yield briefly instead of busy-spinning.
|
||||
exit_event.wait(0.005)
|
||||
finally:
|
||||
try:
|
||||
ws.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def main(exit_event: threading.Event | None = None) -> None:
|
||||
if exit_event is None:
|
||||
exit_event = threading.Event()
|
||||
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId", encoding="utf-8")
|
||||
if not dongle_id:
|
||||
cloudlog.error("canlived: no DongleId, cannot stream")
|
||||
return
|
||||
|
||||
api = Api(dongle_id)
|
||||
host = _api_host()
|
||||
ws_uri = f"{host}/ws/can/{dongle_id}"
|
||||
|
||||
backoff = RECONNECT_MIN
|
||||
while not exit_event.is_set():
|
||||
try:
|
||||
token = api.get_token(expiry_hours=1)
|
||||
_stream_once(dongle_id, ws_uri, token, exit_event)
|
||||
backoff = RECONNECT_MIN # clean disconnect, reset backoff
|
||||
except Exception as e:
|
||||
cloudlog.exception("canlived: stream error: %s", e)
|
||||
exit_event.wait(backoff)
|
||||
backoff = min(backoff * 2, RECONNECT_MAX)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -17,7 +17,7 @@ from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
|
||||
from openpilot.iqpilot.common.slc_utilities import calculate_bearing_offset, is_url_pingable
|
||||
from openpilot.iqpilot.common.slc_variables import FREE_MAPBOX_REQUESTS, OFFSET_MAP_IMPERIAL, OFFSET_MAP_METRIC
|
||||
from openpilot.iqpilot.common.slc_variables import FREE_MAPBOX_REQUESTS, OFFSET_MAP_IMPERIAL, OFFSET_MAP_METRIC, OFFSET_PERCENT_MAX
|
||||
|
||||
try:
|
||||
import requests
|
||||
@@ -354,6 +354,8 @@ class SpeedLimitController:
|
||||
pass
|
||||
|
||||
self.executor = ThreadPoolExecutor(max_workers=1)
|
||||
self._offset_cache = {}
|
||||
self._offset_cache_t = 0.0
|
||||
self._last_mapbox_log_t = 0.0
|
||||
self._last_mapbox_diag_t = 0.0
|
||||
self._last_mapbox_diag_message = None
|
||||
@@ -416,17 +418,33 @@ class SpeedLimitController:
|
||||
return self._assist.output_a_target
|
||||
|
||||
def get_offset(self, is_metric):
|
||||
target = self._assist.target
|
||||
# offsets only apply to real limit sources: fallback set-speed publishes "None",
|
||||
# construction clamps must never be inflated
|
||||
if target <= 0 or self._assist.source in ("None", "Construction"):
|
||||
return 0.0
|
||||
offset_map = OFFSET_MAP_METRIC if is_metric else OFFSET_MAP_IMPERIAL
|
||||
for low, high, offset_param in offset_map:
|
||||
if low < self._assist.target < high:
|
||||
offset_value = self.params.get(offset_param)
|
||||
if offset_value is not None:
|
||||
if isinstance(offset_value, bytes):
|
||||
return float(offset_value.decode("utf-8"))
|
||||
return float(offset_value)
|
||||
return 0.0
|
||||
if low <= target < high:
|
||||
percent = float(np.clip(self._get_offset_percent(offset_param), -OFFSET_PERCENT_MAX, OFFSET_PERCENT_MAX))
|
||||
return target * percent / 100.0
|
||||
return 0.0
|
||||
|
||||
def _get_offset_percent(self, offset_param):
|
||||
now_mono = time.monotonic()
|
||||
if now_mono - self._offset_cache_t >= 5.0:
|
||||
self._offset_cache.clear()
|
||||
self._offset_cache_t = now_mono
|
||||
if offset_param not in self._offset_cache:
|
||||
offset_value = self.params.get(offset_param)
|
||||
try:
|
||||
if isinstance(offset_value, bytes):
|
||||
offset_value = offset_value.decode("utf-8")
|
||||
self._offset_cache[offset_param] = float(offset_value) if offset_value is not None else 0.0
|
||||
except (ValueError, TypeError):
|
||||
self._offset_cache[offset_param] = 0.0
|
||||
return self._offset_cache[offset_param]
|
||||
|
||||
@staticmethod
|
||||
def _is_alive(sm, key):
|
||||
if hasattr(sm, "alive"):
|
||||
|
||||
@@ -2,9 +2,11 @@
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from datetime import datetime
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
|
||||
from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
|
||||
from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
|
||||
|
||||
@@ -92,7 +94,7 @@ def test_speed_limit_controller_resolves_source_by_priority():
|
||||
slc_params = _base_slc_params_controller()
|
||||
slc_params["slc_policy"] = POLICY_MAP_DATA_PRIORITY
|
||||
|
||||
controller.update_limits(25.0, None, True, 30.0, 27.0, sm, slc_params)
|
||||
controller.update_limits(25.0, datetime.now(), True, 30.0, 27.0, sm, slc_params)
|
||||
assert controller.active_source == "Map Data"
|
||||
assert controller.active_target == 18.0
|
||||
|
||||
@@ -112,7 +114,7 @@ def test_speed_limit_controller_combined_mode_prefers_smallest_limit():
|
||||
slc_params = _base_slc_params_controller()
|
||||
slc_params["slc_policy"] = POLICY_COMBINED
|
||||
|
||||
controller.update_limits(28.0, None, True, 31.0, 27.0, sm, slc_params)
|
||||
controller.update_limits(28.0, datetime.now(), True, 31.0, 27.0, sm, slc_params)
|
||||
assert controller.active_source == "Map Data"
|
||||
assert controller.active_target == 16.0
|
||||
|
||||
@@ -128,6 +130,7 @@ def test_slc_vcruise_applies_target_without_increasing_cruise():
|
||||
|
||||
slc._get_slc_params = lambda: {
|
||||
"speed_limit_controller": True,
|
||||
"speed_limit_mode": 3,
|
||||
"show_speed_limits": False,
|
||||
"is_metric": True,
|
||||
"slc_policy": POLICY_MAP_DATA_PRIORITY,
|
||||
@@ -163,6 +166,7 @@ def test_slc_vcruise_show_only_does_not_modify_cruise():
|
||||
slc.slc.active_source = "Map Data"
|
||||
slc._get_slc_params = lambda: {
|
||||
"speed_limit_controller": False,
|
||||
"speed_limit_mode": 1,
|
||||
"show_speed_limits": True,
|
||||
"is_metric": True,
|
||||
"slc_policy": POLICY_MAP_DATA_PRIORITY,
|
||||
@@ -198,6 +202,7 @@ def test_slc_vcruise_auto_raises_for_higher_limit_when_confirmation_disabled():
|
||||
|
||||
slc._get_slc_params = lambda: {
|
||||
"speed_limit_controller": True,
|
||||
"speed_limit_mode": 3,
|
||||
"show_speed_limits": False,
|
||||
"is_metric": True,
|
||||
"slc_policy": POLICY_MAP_DATA_PRIORITY,
|
||||
@@ -232,6 +237,7 @@ def test_slc_vcruise_does_not_auto_raise_when_higher_confirmation_enabled():
|
||||
|
||||
slc._get_slc_params = lambda: {
|
||||
"speed_limit_controller": True,
|
||||
"speed_limit_mode": 3,
|
||||
"show_speed_limits": False,
|
||||
"is_metric": True,
|
||||
"slc_policy": POLICY_MAP_DATA_PRIORITY,
|
||||
@@ -392,6 +398,50 @@ def test_construction_zone_never_raises_cruise_even_with_auto_raise():
|
||||
assert abs(out - 60.0 * CV.MPH_TO_MS) < 1e-6
|
||||
|
||||
|
||||
def _offset_controller(pct1=10.0, pct2=5.0, pct3=8.0):
|
||||
params = FakeParams()
|
||||
params.put("speed_limit_offset1", pct1)
|
||||
params.put("speed_limit_offset2", pct2)
|
||||
params.put("speed_limit_offset3", pct3)
|
||||
controller = SpeedLimitController(params)
|
||||
controller._assist.source = "Map Data"
|
||||
return controller
|
||||
|
||||
|
||||
def test_get_offset_percent_per_zone():
|
||||
controller = _offset_controller()
|
||||
|
||||
controller._assist.target = 6.7 # ~15 mph -> zone 1
|
||||
assert abs(controller.get_offset(False) - 6.7 * 0.10) < 1e-9
|
||||
|
||||
controller._assist.target = 13.4 # ~30 mph -> zone 2
|
||||
assert abs(controller.get_offset(False) - 13.4 * 0.05) < 1e-9
|
||||
|
||||
controller._assist.target = 31.3 # ~70 mph -> zone 3 (open-ended)
|
||||
assert abs(controller.get_offset(False) - 31.3 * 0.08) < 1e-9
|
||||
|
||||
|
||||
def test_get_offset_zone_lower_bound_inclusive():
|
||||
controller = _offset_controller()
|
||||
boundary = OFFSET_MAP_IMPERIAL[1][0]
|
||||
controller._assist.target = boundary
|
||||
assert abs(controller.get_offset(False) - boundary * 0.05) < 1e-9
|
||||
|
||||
|
||||
def test_get_offset_zero_without_real_limit_source():
|
||||
for source in ("None", "Construction"):
|
||||
controller = _offset_controller()
|
||||
controller._assist.source = source
|
||||
controller._assist.target = 30.0
|
||||
assert controller.get_offset(False) == 0.0
|
||||
|
||||
|
||||
def test_get_offset_percent_clamped():
|
||||
controller = _offset_controller(pct3=500.0)
|
||||
controller._assist.target = 30.0
|
||||
assert abs(controller.get_offset(False) - 30.0 * 0.50) < 1e-9
|
||||
|
||||
|
||||
def test_construction_zone_fires_event_once_per_zone_entry():
|
||||
from cereal import custom
|
||||
event = custom.IQOnroadEvent.EventName.constructionZoneDetected
|
||||
|
||||
91
iqpilot/tests/test_auto_units.py
Normal file
91
iqpilot/tests/test_auto_units.py
Normal file
@@ -0,0 +1,91 @@
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.iqpilot.common.auto_units import CONFIRMATIONS, AutoUnits
|
||||
|
||||
SEATTLE = (47.6062, -122.3321)
|
||||
BERLIN = (52.5200, 13.4050)
|
||||
LONDON = (51.5074, -0.1278)
|
||||
|
||||
|
||||
class StubAutoUnits(AutoUnits):
|
||||
def __init__(self, params):
|
||||
super().__init__(params)
|
||||
self.position = (0.0, 0.0, False)
|
||||
|
||||
def _position(self):
|
||||
return self.position
|
||||
|
||||
|
||||
def settle(auto_units, position, count=CONFIRMATIONS, start=0.0):
|
||||
auto_units.position = position
|
||||
for i in range(count):
|
||||
auto_units.update(now=start + i * 100.0)
|
||||
return start + count * 100.0
|
||||
|
||||
|
||||
class TestAutoUnits:
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
self.params.put_bool("IQAutoUnits", True)
|
||||
self.params.remove("IQAutoUnitsRegion")
|
||||
self.params.put_bool("IsMetric", False)
|
||||
self.auto_units = StubAutoUnits(self.params)
|
||||
|
||||
def test_no_fix_does_nothing(self):
|
||||
settle(self.auto_units, (0.0, 0.0, False))
|
||||
assert self.params.get("IQAutoUnitsRegion") is None
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_disabled_does_nothing(self):
|
||||
self.params.put_bool("IQAutoUnits", False)
|
||||
settle(self.auto_units, (*BERLIN, True))
|
||||
assert self.params.get("IQAutoUnitsRegion") is None
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_metric_region_switches_to_metric(self):
|
||||
settle(self.auto_units, (*BERLIN, True))
|
||||
assert self.params.get("IQAutoUnitsRegion") == "METRIC"
|
||||
assert self.params.get_bool("IsMetric")
|
||||
|
||||
def test_mph_region_stays_imperial(self):
|
||||
settle(self.auto_units, (*SEATTLE, True))
|
||||
assert self.params.get("IQAutoUnitsRegion") == "US"
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_uk_stays_imperial(self):
|
||||
self.params.put_bool("IsMetric", True)
|
||||
settle(self.auto_units, (*LONDON, True))
|
||||
assert self.params.get("IQAutoUnitsRegion") == "GB"
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_border_crossing_switches_units(self):
|
||||
now = settle(self.auto_units, (*SEATTLE, True))
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
settle(self.auto_units, (*BERLIN, True), start=now)
|
||||
assert self.params.get("IQAutoUnitsRegion") == "METRIC"
|
||||
assert self.params.get_bool("IsMetric")
|
||||
|
||||
def test_manual_override_is_kept_within_a_region(self):
|
||||
now = settle(self.auto_units, (*SEATTLE, True))
|
||||
self.params.put_bool("IsMetric", True)
|
||||
|
||||
settle(self.auto_units, (*SEATTLE, True), start=now)
|
||||
assert self.params.get_bool("IsMetric")
|
||||
|
||||
def test_unconfirmed_region_is_not_applied(self):
|
||||
settle(self.auto_units, (*BERLIN, True), count=CONFIRMATIONS - 1)
|
||||
assert self.params.get("IQAutoUnitsRegion") is None
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_flapping_region_resets_confirmations(self):
|
||||
now = 0.0
|
||||
for position in (BERLIN, SEATTLE, BERLIN, SEATTLE):
|
||||
now = settle(self.auto_units, (*position, True), count=1, start=now)
|
||||
assert self.params.get("IQAutoUnitsRegion") is None
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_rate_limited(self):
|
||||
self.auto_units.position = (*BERLIN, True)
|
||||
for _ in range(CONFIRMATIONS * 4):
|
||||
self.auto_units.update(now=1.0)
|
||||
assert self.params.get("IQAutoUnitsRegion") is None
|
||||
165
iqpilot/tests/test_geo_regions.py
Normal file
165
iqpilot/tests/test_geo_regions.py
Normal file
@@ -0,0 +1,165 @@
|
||||
import pytest
|
||||
|
||||
from openpilot.iqpilot.common.geo_regions import METRIC_REGION, UNKNOWN_REGION, region_for_position, region_is_metric
|
||||
|
||||
US_POINTS = [
|
||||
(47.6062, -122.3321, "Seattle"),
|
||||
(42.3314, -83.0458, "Detroit"),
|
||||
(42.8864, -78.8784, "Buffalo"),
|
||||
(25.7617, -80.1918, "Miami"),
|
||||
(29.7604, -95.3698, "Houston"),
|
||||
(32.7157, -117.1611, "San Diego"),
|
||||
(34.0522, -118.2437, "Los Angeles"),
|
||||
(61.2181, -149.9003, "Anchorage"),
|
||||
(58.3019, -134.4197, "Juneau"),
|
||||
(64.8378, -147.7164, "Fairbanks"),
|
||||
(21.3069, -157.8583, "Honolulu"),
|
||||
(18.4655, -66.1057, "San Juan"),
|
||||
(13.4757, 144.7489, "Guam"),
|
||||
(44.9778, -93.2650, "Minneapolis"),
|
||||
(40.7128, -74.0060, "New York"),
|
||||
(41.8781, -87.6298, "Chicago"),
|
||||
(31.7900, -106.4300, "El Paso"),
|
||||
(26.2034, -98.2300, "McAllen"),
|
||||
(44.8016, -68.7712, "Bangor"),
|
||||
(48.7519, -122.4787, "Bellingham"),
|
||||
(46.8772, -96.7898, "Fargo"),
|
||||
(48.6023, -93.4093, "International Falls"),
|
||||
(46.4953, -84.3453, "Sault Ste. Marie MI"),
|
||||
(47.1211, -88.5694, "Houghton"),
|
||||
(41.6528, -83.5379, "Toledo"),
|
||||
(42.1370, -83.1930, "Trenton MI"),
|
||||
(39.7392, -104.9903, "Denver"),
|
||||
(33.4484, -112.0740, "Phoenix"),
|
||||
(30.3322, -81.6557, "Jacksonville"),
|
||||
(42.3601, -71.0589, "Boston"),
|
||||
(38.9072, -77.0369, "Washington DC"),
|
||||
]
|
||||
|
||||
GB_POINTS = [
|
||||
(51.5074, -0.1278, "London"),
|
||||
(54.5973, -5.9301, "Belfast"),
|
||||
(55.8642, -4.2518, "Glasgow"),
|
||||
(51.4816, -3.1791, "Cardiff"),
|
||||
(51.4545, -2.5879, "Bristol"),
|
||||
(53.4084, -2.9916, "Liverpool"),
|
||||
(53.4808, -2.2426, "Manchester"),
|
||||
(55.9533, -3.1883, "Edinburgh"),
|
||||
(52.4862, -1.8904, "Birmingham"),
|
||||
(53.8008, -1.5491, "Leeds"),
|
||||
(57.4778, -4.2247, "Inverness"),
|
||||
(57.1497, -2.0943, "Aberdeen"),
|
||||
(56.4620, -2.9707, "Dundee"),
|
||||
(58.6373, -3.0689, "John o' Groats"),
|
||||
(54.1509, -4.4814, "Douglas"),
|
||||
(49.1858, -2.1064, "St Helier"),
|
||||
(55.0000, -7.3200, "Derry"),
|
||||
(54.3438, -7.6315, "Enniskillen"),
|
||||
(54.1751, -6.3402, "Newry"),
|
||||
(54.4783, -8.0906, "Belleek"),
|
||||
(54.5973, -7.3095, "Omagh"),
|
||||
(55.2053, -6.6570, "Portrush"),
|
||||
(58.2090, -6.3890, "Stornoway"),
|
||||
(58.9809, -2.9605, "Kirkwall"),
|
||||
(60.1546, -1.1494, "Lerwick"),
|
||||
(50.7184, -3.5339, "Exeter"),
|
||||
(50.3755, -4.1427, "Plymouth"),
|
||||
(52.6309, 1.2974, "Norwich"),
|
||||
(54.9783, -1.6178, "Newcastle"),
|
||||
(51.8642, -2.2382, "Gloucester"),
|
||||
(52.4140, -4.0810, "Aberystwyth"),
|
||||
(51.6214, -3.9436, "Swansea"),
|
||||
(50.6938, -1.3040, "Newport IoW"),
|
||||
]
|
||||
|
||||
LR_POINTS = [
|
||||
(6.3005, -10.7969, "Monrovia"),
|
||||
(4.3750, -7.7169, "Harper"),
|
||||
(6.9956, -9.4722, "Gbarnga"),
|
||||
(6.0667, -8.1333, "Zwedru"),
|
||||
(8.4219, -9.7478, "Voinjama"),
|
||||
(5.8808, -10.0467, "Buchanan"),
|
||||
(5.0100, -9.0400, "Greenville"),
|
||||
(7.3500, -8.7200, "Ganta"),
|
||||
]
|
||||
|
||||
METRIC_POINTS = [
|
||||
(49.2827, -123.1207, "Vancouver"),
|
||||
(48.4284, -123.3656, "Victoria"),
|
||||
(43.6532, -79.3832, "Toronto"),
|
||||
(42.3149, -83.0364, "Windsor"),
|
||||
(46.5136, -84.3358, "Sault Ste. Marie ON"),
|
||||
(42.9745, -82.4066, "Sarnia"),
|
||||
(43.2557, -79.8711, "Hamilton"),
|
||||
(42.9849, -81.2453, "London ON"),
|
||||
(45.5019, -73.5674, "Montreal"),
|
||||
(45.4765, -75.7013, "Gatineau"),
|
||||
(46.8139, -71.2080, "Quebec City"),
|
||||
(46.0878, -64.7782, "Moncton"),
|
||||
(44.6488, -63.5752, "Halifax"),
|
||||
(49.8951, -97.1384, "Winnipeg"),
|
||||
(51.0447, -114.0719, "Calgary"),
|
||||
(53.5461, -113.4938, "Edmonton"),
|
||||
(52.1332, -106.6700, "Saskatoon"),
|
||||
(50.6745, -120.3273, "Kamloops"),
|
||||
(32.5149, -117.0382, "Tijuana"),
|
||||
(31.7000, -106.4700, "Ciudad Juarez"),
|
||||
(25.6866, -100.3161, "Monterrey"),
|
||||
(27.5060, -99.5075, "Nuevo Laredo"),
|
||||
(19.4326, -99.1332, "Mexico City"),
|
||||
(53.3498, -6.2603, "Dublin"),
|
||||
(51.8985, -8.4756, "Cork"),
|
||||
(53.2707, -9.0568, "Galway"),
|
||||
(54.9503, -7.7345, "Letterkenny"),
|
||||
(54.0000, -6.4000, "Dundalk"),
|
||||
(54.2489, -6.9683, "Monaghan"),
|
||||
(54.2766, -8.4761, "Sligo"),
|
||||
(54.6538, -8.1096, "Donegal"),
|
||||
(52.5200, 13.4050, "Berlin"),
|
||||
(52.2297, 21.0122, "Warsaw"),
|
||||
(48.8566, 2.3522, "Paris"),
|
||||
(60.1699, 24.9384, "Helsinki"),
|
||||
(50.4501, 30.5234, "Kyiv"),
|
||||
(-33.8688, 151.2093, "Sydney"),
|
||||
(35.6762, 139.6503, "Tokyo"),
|
||||
(8.4844, -13.2299, "Freetown"),
|
||||
(7.8767, -11.1875, "Kenema"),
|
||||
(8.2783, -10.5733, "Kailahun"),
|
||||
(9.6412, -13.5784, "Conakry"),
|
||||
(7.7562, -8.8179, "Nzerekore"),
|
||||
(7.4125, -7.5539, "Man"),
|
||||
(5.3600, -4.0083, "Abidjan"),
|
||||
]
|
||||
|
||||
|
||||
@pytest.mark.parametrize("lat, lon, name", US_POINTS)
|
||||
def test_us_positions(lat, lon, name):
|
||||
assert region_for_position(lat, lon) == "US", name
|
||||
|
||||
|
||||
@pytest.mark.parametrize("lat, lon, name", GB_POINTS)
|
||||
def test_gb_positions(lat, lon, name):
|
||||
assert region_for_position(lat, lon) == "GB", name
|
||||
|
||||
|
||||
@pytest.mark.parametrize("lat, lon, name", LR_POINTS)
|
||||
def test_lr_positions(lat, lon, name):
|
||||
assert region_for_position(lat, lon) == "LR", name
|
||||
|
||||
|
||||
@pytest.mark.parametrize("lat, lon, name", METRIC_POINTS)
|
||||
def test_metric_positions(lat, lon, name):
|
||||
assert region_for_position(lat, lon) == METRIC_REGION, name
|
||||
|
||||
|
||||
@pytest.mark.parametrize("lat, lon", [(0.0, 0.0), (0.0, 0.00001), (91.0, 10.0), (10.0, 181.0)])
|
||||
def test_invalid_positions(lat, lon):
|
||||
assert region_for_position(lat, lon) == UNKNOWN_REGION
|
||||
|
||||
|
||||
def test_region_is_metric():
|
||||
assert not region_is_metric("US")
|
||||
assert not region_is_metric("GB")
|
||||
assert not region_is_metric("LR")
|
||||
assert region_is_metric(METRIC_REGION)
|
||||
assert not region_is_metric(UNKNOWN_REGION)
|
||||
@@ -47,6 +47,8 @@ class IQHudRenderer(HudRenderer):
|
||||
has_limit = self.speed_limit_renderer.speed_limit_valid or self.speed_limit_renderer.speed_limit_last_valid
|
||||
self.limit_available = has_limit
|
||||
self.limit_speed_text = str(round(self.speed_limit_renderer.speed_limit_last)) if has_limit else "---"
|
||||
offset = round(self.speed_limit_renderer.speed_limit_offset)
|
||||
self.limit_offset_text = f"{offset:+d}" if has_limit and offset != 0 else ""
|
||||
self.turn_signal_controller.update()
|
||||
self.speed_renderer.update()
|
||||
self.soft_warning_renderer.update()
|
||||
|
||||
Reference in New Issue
Block a user