diff --git a/README.md b/README.md
index 7b5da512aa2a91..f105957ed31826 100644
--- a/README.md
+++ b/README.md
@@ -51,54 +51,54 @@ Also, we have a several thousand people community on [slack](https://slack.comma
Hardware
------
-Right now openpilot supports the [EON Dashcam DevKit](https://comma.ai/shop/products/eon-dashcam-devkit). We'd like to support other platforms as well.
+At the moment openpilot supports the [EON Dashcam DevKit](https://comma.ai/shop/products/eon-dashcam-devkit). A [panda](https://shop.comma.ai/products/panda-obd-ii-dongle) and a [giraffe](https://comma.ai/shop/products/giraffe/) are recommended tools to interface the EON with the car. We'd like to support other platforms as well.
Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` during NEOS setup.
Supported Cars
------
-| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
-| ---------------------| ------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
-| Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | Nidec |
-| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec |
-| Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| GMC3| Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
-| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | Bosch |
-| Honda | Civic 2016-18 | Honda Sensing | Yes | Yes | 0mph | 12mph | Nidec |
-| Honda | Civic 2017-18 *(Hatch)* | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
-| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph1| 12mph | Nidec |
-| Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
-| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 0mph | Inverted Nidec |
-| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
-| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec |
-| Honda | Ridgeline 2017-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
-| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6|
-| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6|
-| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6|
-| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6|
-| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6|
-| Lexus | RX Hybrid 2016-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
-| Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | Toyota |
-| Toyota | C-HR 2017-184| All | Yes | Stock | 0mph | 0mph | Toyota |
-| Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota |
-| Toyota | Highlander 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
-| Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota |
-| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota |
-| Toyota | Prius 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
-| Toyota | Prius Prime 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
-| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | Toyota |
-| Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota |
-| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
-
-1[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
-2When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)
-3[GM installation guide](https://zoneos.com/volt/).
-4It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).
-528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
+| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
+| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
+| Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph1| 25mph | Nidec |
+| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph1| 12mph | Nidec |
+| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
+| Chevrolet3| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
+| Cadillac3 | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
+| GMC3 | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
+| Holden3 | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7|
+| Honda | Accord 2018 | All | Yes | Stock | 0mph | 3mph | Bosch |
+| Honda | Civic Sedan/Coupe 2016-18| Honda Sensing | Yes | Yes | 0mph | 12mph | Nidec |
+| Honda | Civic Hatchback 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
+| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph1| 12mph | Nidec |
+| Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
+| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 0mph | Inverted Nidec |
+| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
+| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec |
+| Honda | Ridgeline 2017-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
+| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom6|
+| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom6|
+| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom6|
+| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6|
+| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6|
+| Lexus | RX Hybrid 2016-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | Toyota |
+| Toyota | C-HR 2017-184 | All | Yes | Stock | 0mph | 0mph | Toyota |
+| Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota |
+| Toyota | Highlander 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Toyota | Highlander Hybrid 2018 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Toyota | Prius 2016 | TSS-P | Yes | Yes2| 0mph | 0mph | Toyota |
+| Toyota | Prius 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Toyota | Prius Prime 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Toyota | Rav4 2016 | TSS-P | Yes | Yes2| 20mph | 0mph | Toyota |
+| Toyota | Rav4 2017-18 | All | Yes | Yes2| 20mph | 0mph | Toyota |
+| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+
+1[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
+2When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota)
+3[GM installation guide](https://zoneos.com/volt/).
+4It needs an extra 120Ohm resistor ([pic1](https://i.imgur.com/CmdKtTP.jpg), [pic2](https://i.imgur.com/s2etUo6.jpg)) on bus 3 and giraffe switches set to 01X1 (11X1 for stock LKAS), where X depends on if you have the [comma power](https://comma.ai/shop/products/power/).
+528mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
6Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed ofor the 2019 Sante Fe; pinout may differ for other Hyundais.
7Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/)
diff --git a/RELEASES.md b/RELEASES.md
index eef675a304271c..216feb83cfcf61 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,13 @@
+Version 0.5.7 (2018-12-06)
+========================
+ * Speed limit from OpenStreetMap added to UI
+ * Highlight speed limit when speed exceeds road speed limit plus a delta
+ * Option to limit openpilot max speed to road speed limit plus a delta
+ * Cadillac ATS support thanks to vntarasov!
+ * GMC Acadia support thanks to CryptoKylan!
+ * Decrease GPU power consumption
+ * NEOSv8 autoupdate
+
Version 0.5.6 (2018-11-16)
========================
* Refresh settings layout and add feature descriptions
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
index a9e5738a40cd3c..84ebf24ae42ecd 100644
Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ
diff --git a/cereal/Makefile b/cereal/Makefile
index b86fa68f585816..dc6b7f9d51fa03 100644
--- a/cereal/Makefile
+++ b/cereal/Makefile
@@ -6,13 +6,16 @@ GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
JS := gen/js/car.capnp.js gen/js/log.capnp.js
UNAME_M ?= $(shell uname -m)
-
# only generate C++ for docker tests
ifneq ($(OPTEST),1)
GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
ifeq ($(UNAME_M),x86_64)
- GENS += gen/java/Car.java gen/java/Log.java
+ ifneq (, $(shell which capnpc-java))
+ GENS += gen/java/Car.java gen/java/Log.java
+ else
+ $(warning capnpc-java not found, skipping java build)
+ endif
endif
endif
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 2f25aa0d1ac02e..bf982f8aba0da6 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -355,6 +355,7 @@ struct CarParams {
radarOffCan @47 :Bool; # True when radar objects aren't visible on CAN
steerActuatorDelay @48 :Float32; # Steering wheel actuator delay in seconds
+ openpilotLongitudinalControl @50 :Bool; # is openpilot doing the longitudinal control?
enum SteerControlType {
torque @0;
diff --git a/cereal/log.capnp b/cereal/log.capnp
index 389b550ad421ae..e643728e0031ec 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -276,7 +276,8 @@ struct ThermalData {
startedTs @13 :UInt64;
thermalStatus @14 :ThermalStatus;
- chargerDisabled @17 :Bool;
+ chargingError @17 :Bool;
+ chargingDisabled @18 :Bool;
enum ThermalStatus {
green @0; # all processes run
@@ -344,6 +345,7 @@ struct LiveCalibrationData {
warpMatrix @0 :List(Float32);
# camera_frame_from_model_frame
warpMatrix2 @5 :List(Float32);
+ warpMatrixBig @6 :List(Float32);
calStatus @1 :Int8;
calCycle @2 :Int32;
calPerc @3 :Int8;
@@ -562,6 +564,10 @@ struct Plan {
gpsPlannerActive @19 :Bool;
+ # maps
+ vCurvature @21 :Float32;
+ decelForTurn @22 :Bool;
+
struct GpsTrajectory {
x @0 :List(Float32);
y @1 :List(Float32);
@@ -1567,8 +1573,17 @@ struct LiveParametersData {
}
struct LiveMapData {
- valid @0 :Bool;
+ speedLimitValid @0 :Bool;
speedLimit @1 :Float32;
+ curvatureValid @2 :Bool;
+ curvature @3 :Float32;
+ wayId @4 :UInt64;
+ roadX @5 :List(Float32);
+ roadY @6 :List(Float32);
+ lastGps @7: GpsLocationData;
+ roadCurvatureX @8 :List(Float32);
+ roadCurvature @9 :List(Float32);
+ distToTurn @10 :Float32;
}
diff --git a/common/params.py b/common/params.py
index 3981a341f4e28c..cc137d5a7b15c4 100755
--- a/common/params.py
+++ b/common/params.py
@@ -63,6 +63,7 @@ class UnknownKeyName(Exception):
"IsUploadVideoOverCellularEnabled": TxType.PERSISTENT,
"IsDriverMonitoringEnabled": TxType.PERSISTENT,
"IsGeofenceEnabled": TxType.PERSISTENT,
+ "SpeedLimitOffset": TxType.PERSISTENT,
# written: visiond
# read: visiond, controlsd
"CalibrationParams": TxType.PERSISTENT,
@@ -74,6 +75,8 @@ class UnknownKeyName(Exception):
"DoUninstall": TxType.CLEAR_ON_MANAGER_START,
"ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
"IsUpdateAvailable": TxType.PERSISTENT,
+ "LongitudinalControl": TxType.PERSISTENT,
+ "LimitSetSpeed": TxType.PERSISTENT,
"RecordFront": TxType.PERSISTENT,
}
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 107c3d542c2036..424bd6158e43c1 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -83,7 +83,7 @@ def get_model_height_transform(camera_frame_from_road_frame, height):
# camera_frame_from_model_frame aka 'warp matrix'
# was: calibration.h/CalibrationTransform
-def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height):
+def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height):
vp = vp_from_ke(camera_frame_from_road_frame)
model_camera_from_model_frame = np.array([
diff --git a/installer/updater/updater b/installer/updater/updater
index a5defb4108b556..37f149f8bf62a2 100755
Binary files a/installer/updater/updater and b/installer/updater/updater differ
diff --git a/phonelibs/openblas/libopenblas.so b/phonelibs/openblas/libopenblas.so
new file mode 120000
index 00000000000000..7a792bc9071885
--- /dev/null
+++ b/phonelibs/openblas/libopenblas.so
@@ -0,0 +1 @@
+libopenblas_armv8p-r0.2.19.so
\ No newline at end of file
diff --git a/phonelibs/openblas/libopenblas_armv8p-r0.2.19.so b/phonelibs/openblas/libopenblas_armv8p-r0.2.19.so
new file mode 100755
index 00000000000000..ace58c8a13122f
Binary files /dev/null and b/phonelibs/openblas/libopenblas_armv8p-r0.2.19.so differ
diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt
index deb37d08655808..b623e2a25e2e11 100644
--- a/requirements_openpilot.txt
+++ b/requirements_openpilot.txt
@@ -5,7 +5,7 @@ libusb1==1.5.0
pycapnp==0.6.3
pyzmq==15.4.0
raven==5.23.0
-requests==2.10.0
+requests==2.20.0
setproctitle==1.1.10
simplejson==3.8.2
pyyaml==3.12
@@ -16,4 +16,5 @@ filterpy==1.2.4
smbus2==0.2.0
pyflakes==1.6.0
-e git+https://github.com/commaai/le_python.git@5eef8f5be5929d33973e1b10e686fa0cdcd6792f#egg=Logentries
+-e git+https://github.com/commaai/python-overpy.git@f86529af402d4642e1faeb146671c40284007323#egg=overpy
Flask==1.0.2
diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py
index d897a815f2cb03..1e7b233ddda491 100755
--- a/selfdrive/boardd/boardd.py
+++ b/selfdrive/boardd/boardd.py
@@ -71,9 +71,10 @@ def __parse_can_buffer(dat):
def can_send_many(arr):
snds = []
for addr, _, dat, alt in arr:
- snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
- snd = snd.ljust(0x10, '\x00')
- snds.append(snd)
+ if addr < 0x800: # only support 11 bit addr
+ snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
+ snd = snd.ljust(0x10, '\x00')
+ snds.append(snd)
while 1:
try:
handle.bulkWrite(3, ''.join(snds))
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index 1f0e36e0dc23ef..633ce4b8ae0458 100755
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -119,6 +119,7 @@ def get_params(candidate, fingerprint):
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint)
+ ret.openpilotLongitudinalControl = False
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
ret.steerLimitAlert = False
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index edb3115c79ee61..9ec1b9e5d0e12c 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -62,6 +62,7 @@ def get_params(candidate, fingerprint):
# Have to go passive if ASCM is online (ACC-enabled cars),
# or camera is on powertrain bus (LKA cars without ACC).
ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint)
+ ret.openpilotLongitudinalControl = ret.enableCamera
std_cargo = 136
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index f46343c18fc7d6..99d4a2214300c2 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -40,8 +40,6 @@ def get_can_signals(CP):
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
("GEAR", "GEARBOX", 0),
- ("BRAKE_ERROR_1", "STANDSTILL", 1),
- ("BRAKE_ERROR_2", "STANDSTILL", 1),
("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1),
("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0),
("BRAKE_PRESSED", "POWERTRAIN_DATA", 0),
@@ -63,7 +61,6 @@ def get_can_signals(CP):
("STEERING_SENSORS", 100),
("SCM_FEEDBACK", 10),
("GEARBOX", 100),
- ("STANDSTILL", 50),
("SEATBELT_STATUS", 10),
("CRUISE", 10),
("POWERTRAIN_DATA", 100),
@@ -84,9 +81,12 @@ def get_can_signals(CP):
checks += [("GAS_PEDAL_2", 100)]
else:
# Nidec signals.
- signals += [("CRUISE_SPEED_PCM", "CRUISE", 0),
+ signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
+ ("BRAKE_ERROR_2", "STANDSTILL", 1),
+ ("CRUISE_SPEED_PCM", "CRUISE", 0),
("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)]
- checks += [("CRUISE_PARAMS", 50)]
+ checks += [("CRUISE_PARAMS", 50),
+ ("STANDSTILL", 50)]
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
@@ -205,7 +205,10 @@ def update(self, cp, cp_cam):
self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 2, 3, 4, 6]
self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] != 0
self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3] # 3 is low speed lockout, not worth a warning
- self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2']
+ if self.CP.radarOffCan:
+ self.brake_error = 0
+ else:
+ self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2']
self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index e074900e59937e..e6c4ed4de1ef93 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -151,10 +151,13 @@ def get_params(candidate, fingerprint):
ret.safetyModel = car.CarParams.SafetyModels.hondaBosch
ret.enableCamera = True
ret.radarOffCan = True
+ ret.openpilotLongitudinalControl = False
else:
ret.safetyModel = car.CarParams.SafetyModels.honda
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
ret.enableGasInterceptor = 0x201 in fingerprint
+ ret.openpilotLongitudinalControl = ret.enableCamera
+
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
@@ -485,8 +488,8 @@ def update(self, c):
ret.buttonEvents = buttonEvents
# events
- # TODO: I don't like the way capnp does enums
- # These strings aren't checked at compile time
+ # TODO: event names aren't checked at compile time.
+ # Maybe there is a way to use capnp enums directly
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 96174ef09643e8..c525c0185e939b 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -163,6 +163,7 @@ def get_params(candidate, fingerprint):
ret.longPidDeadzoneV = [0.]
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
+ ret.openpilotLongitudinalControl = False
ret.steerLimitAlert = False
ret.stoppingControl = False
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index 5edbcbbd113f78..b7d60a4257bfee 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -47,6 +47,7 @@ def get_params(candidate, fingerprint):
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.noOutput
+ ret.openpilotLongitudinalControl = False
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 57e90339cc6ae7..4f3b53236acb18 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -186,6 +186,7 @@ def get_params(candidate, fingerprint):
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
+ ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu)
cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs)
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 617c8189cba23d..422faa02b65add 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.5.6-release"
+#define COMMA_VERSION "0.5.7-release"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 5a4b7ef4dda41a..0a96c233516499 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -2,13 +2,11 @@
import gc
import zmq
import json
-
from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.profiler import Profiler
from common.params import Params
-
import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
@@ -25,7 +23,7 @@
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.driver_monitor import DriverStatus
-from selfdrive.locationd.calibration_values import Calibration, Filter
+from selfdrive.locationd.calibration_helpers import Calibration, Filter
ThermalStatus = log.ThermalData.ThermalStatus
State = log.Live100Data.ControlState
@@ -121,14 +119,14 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
-def calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence):
+def calc_plan(CS, CP, VM, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence):
"""Calculate a longitudinal plan using MPC"""
# Slow down when based on driver monitoring or geofence
force_decel = driver_status.awareness < 0. or (geofence is not None and not geofence.in_geofence)
# Update planner
- plan_packet = PL.update(CS, LaC, LoC, v_cruise_kph, force_decel)
+ plan_packet = PL.update(CS, CP, VM, LaC, LoC, v_cruise_kph, force_decel)
plan = plan_packet.plan
plan_ts = plan_packet.logMonoTime
events += list(plan.events)
@@ -461,6 +459,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
+ params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
state = State.disabled
soft_disable_timer = 0
@@ -496,7 +495,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
prof.checkpoint("Sample")
# Define longitudinal plan (MPC)
- plan, plan_ts = calc_plan(CS, CP, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence)
+ plan, plan_ts = calc_plan(CS, CP, VM, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence)
prof.checkpoint("Plan")
if not passive:
@@ -512,7 +511,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
# Publish data
CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
- live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive)
+ live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
index 658ee1b85bbcfb..36b6f8b7e0b8d3 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
@@ -1,11 +1,19 @@
import os
+import platform
import subprocess
from cffi import FFI
mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
-subprocess.check_call(["make", "-j4"], cwd=mpc_dir)
+if platform.machine() == "x86_64":
+ try:
+ FFI().dlopen(os.path.join(mpc_dir, "libmpc1.so"))
+ except OSError:
+ # libmpc1.so is likely built for aarch64. cleaning...
+ subprocess.check_call(["make", "clean"], cwd=mpc_dir)
+
+subprocess.check_call(["make", "-j4"], cwd=mpc_dir)
def _get_libmpc(mpc_id):
libmpc_fn = os.path.join(mpc_dir, "libmpc%d.so" % mpc_id)
@@ -36,9 +44,7 @@ def _get_libmpc(mpc_id):
return (ffi, ffi.dlopen(libmpc_fn))
-
mpcs = [_get_libmpc(1), _get_libmpc(2)]
-
def get_libmpc(mpc_id):
return mpcs[mpc_id - 1]
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 33cb4f56f06d36..b986740113904d 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -6,6 +6,7 @@
from copy import copy
from cereal import log
from collections import defaultdict
+from common.params import Params
from common.realtime import sec_since_boot
from common.numpy_fast import interp
import selfdrive.messaging as messaging
@@ -19,6 +20,10 @@
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+# Max lateral acceleration, used to caclulate how much to slow down in turns
+A_Y_MAX = 2.0 # m/s^2
+NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS
+
_DT = 0.01 # 100Hz
_DT_MPC = 0.2 # 5Hz
MAX_SPEED_ERROR = 2.0
@@ -249,8 +254,10 @@ def __init__(self, CP, fcw_enabled):
context = zmq.Context()
self.CP = CP
self.poller = zmq.Poller()
+
self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller)
self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller)
+ self.live_map_data = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=self.poller)
if os.environ.get('GPS_PLANNER_ACTIVE', False):
self.gps_planner_plan = messaging.sub_sock(context, service_list['gpsPlannerPlan'].port, conflate=True, poller=self.poller, addr=GPS_PLANNER_ADDR)
@@ -293,8 +300,12 @@ def __init__(self, CP, fcw_enabled):
self.last_gps_planner_plan = None
self.gps_planner_active = False
+ self.last_live_map_data = None
self.perception_state = log.Live20Data.new_message()
+ self.params = Params()
+ self.v_speedlimit = NO_CURVATURE_SPEED
+
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
solutions = {'cruise': self.v_cruise}
@@ -327,7 +338,7 @@ def choose_solution(self, v_cruise_setpoint, enabled):
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
# this runs whenever we get a packet that can change the plan
- def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel):
+ def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
@@ -342,6 +353,8 @@ def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel):
l20 = messaging.recv_one(socket)
elif socket is self.gps_planner_plan:
gps_planner_plan = messaging.recv_one(socket)
+ elif socket is self.live_map_data:
+ self.last_live_map_data = messaging.recv_one(socket).liveMapData
if gps_planner_plan is not None:
self.last_gps_planner_plan = gps_planner_plan
@@ -381,9 +394,23 @@ def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel):
enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
+
+ if self.last_live_map_data:
+ self.v_speedlimit = NO_CURVATURE_SPEED
+
+ # Speed limit
+ if self.last_live_map_data.speedLimitValid:
+ speed_limit = self.last_live_map_data.speedLimit
+ set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
+
+ if set_speed_limit_active:
+ offset = float(self.params.get("SpeedLimitOffset"))
+ self.v_speedlimit = speed_limit + offset
+
+ v_cruise_setpoint = min([v_cruise_setpoint, self.v_speedlimit])
+
# Calculate speed for normal cruise control
if enabled:
-
accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
diff --git a/selfdrive/locationd/calibration_values.py b/selfdrive/locationd/calibration_helpers.py
similarity index 100%
rename from selfdrive/locationd/calibration_values.py
rename to selfdrive/locationd/calibration_helpers.py
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index 36d76aa59e7600..6c395efed20dc8 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -6,13 +6,13 @@
import json
import numpy as np
import selfdrive.messaging as messaging
-from selfdrive.locationd.calibration_values import Calibration, Filter
+from selfdrive.locationd.calibration_helpers import Calibration, Filter
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from common.params import Params
from common.ffi_wrapper import ffi_wrap
import common.transformations.orientation as orient
-from common.transformations.model import model_height, get_camera_frame_from_model_frame
+from common.transformations.model import model_height, get_camera_frame_from_model_frame, get_camera_frame_from_bigmodel_frame
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
eon_intrinsics, get_calib_from_vp, normalize, denormalize, H, W
@@ -208,13 +208,15 @@ def send_data(self, livecalibration):
calib = get_calib_from_vp(self.vp)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
ke = eon_intrinsics.dot(extrinsic_matrix)
- warp_matrix = get_camera_frame_from_model_frame(ke, model_height)
+ warp_matrix = get_camera_frame_from_model_frame(ke)
+ warp_matrix_big = get_camera_frame_from_bigmodel_frame(ke)
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100)
cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten())
+ cal_send.liveCalibration.warpMatrixBig = map(float, warp_matrix_big.flatten())
cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten())
livecalibration.send(cal_send.to_bytes())
diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd
index 881b9e84fea041..ce1e7ecd2f6fa5 100755
Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 6b64a4af77f224..90753644d3cdb6 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -42,7 +42,7 @@ def unblock_stdout():
if __name__ == "__main__":
if os.path.isfile("/init.qcom.rc") \
- and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 6):
+ and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 8):
# update continue.sh before updating NEOS
if os.path.isfile(os.path.join(BASEDIR, "scripts", "continue.sh")):
@@ -88,6 +88,7 @@ def unblock_stdout():
"controlsd": "selfdrive.controls.controlsd",
"radard": "selfdrive.controls.radard",
"ubloxd": "selfdrive.locationd.ubloxd",
+ "mapd": "selfdrive.mapd.mapd",
"loggerd": ("selfdrive/loggerd", ["./loggerd"]),
"logmessaged": "selfdrive.logmessaged",
"tombstoned": "selfdrive.tombstoned",
@@ -135,7 +136,8 @@ def get_running():
'visiond',
'proclogd',
'ubloxd',
- 'orbd'
+ 'orbd',
+ 'mapd',
]
def register_managed_process(name, desc, car_started=False):
@@ -474,6 +476,12 @@ def main():
params.put("IsDriverMonitoringEnabled", "1")
if params.get("IsGeofenceEnabled") is None:
params.put("IsGeofenceEnabled", "-1")
+ if params.get("SpeedLimitOffset") is None:
+ params.put("SpeedLimitOffset", "0")
+ if params.get("LongitudinalControl") is None:
+ params.put("LongitudinalControl", "0")
+ if params.get("LimitSetSpeed") is None:
+ params.put("LimitSetSpeed", "0")
# is this chffrplus?
if os.getenv("PASSIVE") is not None:
diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py
new file mode 100644
index 00000000000000..e69de29bb2d1d6
diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py
new file mode 100755
index 00000000000000..e12188590992fa
--- /dev/null
+++ b/selfdrive/mapd/mapd.py
@@ -0,0 +1,240 @@
+#!/usr/bin/env python
+
+# Add phonelibs openblas to LD_LIBRARY_PATH if import fails
+try:
+ from scipy import spatial
+except ImportError as e:
+ import os
+ import sys
+ from common.basedir import BASEDIR
+
+ openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/")
+ os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path
+
+ args = [sys.executable]
+ args.extend(sys.argv)
+ os.execv(sys.executable, args)
+
+import time
+import zmq
+import threading
+import numpy as np
+import overpy
+from collections import defaultdict
+
+from common.transformations.coordinates import geodetic2ecef
+from selfdrive.services import service_list
+import selfdrive.messaging as messaging
+from mapd_helpers import LOOKAHEAD_TIME, MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points
+
+
+OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter"
+OVERPASS_HEADERS = {
+ 'User-Agent': 'NEOS (comma.ai)'
+}
+
+last_gps = None
+query_lock = threading.Lock()
+last_query_result = None
+last_query_pos = None
+
+
+def build_way_query(lat, lon, radius=50):
+ """Builds a query to find all highways within a given radius around a point"""
+ pos = " (around:%f,%f,%f)" % (radius, lat, lon)
+ q = """(
+ way
+ """ + pos + """
+ [highway];
+ >;);out;
+ """
+ return q
+
+
+def query_thread():
+ global last_query_result, last_query_pos
+ api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.)
+
+ while True:
+ if last_gps is not None:
+ fix_ok = last_gps.flags & 1
+ if not fix_ok:
+ continue
+
+ if last_query_pos is not None:
+ cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude))
+ prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude))
+ dist = np.linalg.norm(cur_ecef - prev_ecef)
+ if dist < 1000:
+ continue
+
+ q = build_way_query(last_gps.latitude, last_gps.longitude, radius=2000)
+ try:
+ new_result = api.query(q)
+
+ # Build kd-tree
+ nodes = []
+ real_nodes = []
+ node_to_way = defaultdict(list)
+
+ for n in new_result.nodes:
+ nodes.append((float(n.lat), float(n.lon), 0))
+ real_nodes.append(n)
+
+ for way in new_result.ways:
+ for n in way.nodes:
+ node_to_way[n.id].append(way)
+
+ nodes = np.asarray(nodes)
+ nodes = geodetic2ecef(nodes)
+ tree = spatial.cKDTree(nodes)
+
+ query_lock.acquire()
+ last_query_result = new_result, tree, real_nodes, node_to_way
+ last_query_pos = last_gps
+ query_lock.release()
+
+ except Exception as e:
+ print e
+ query_lock.acquire()
+ last_query_result = None
+ query_lock.release()
+ time.sleep(1)
+
+
+def mapsd_thread():
+ global last_gps
+
+ context = zmq.Context()
+ gps_sock = messaging.sub_sock(context, service_list['gpsLocation'].port, conflate=True)
+ gps_external_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True)
+ map_data_sock = messaging.pub_sock(context, service_list['liveMapData'].port)
+
+ cur_way = None
+ curvature_valid = False
+ curvature = None
+ upcoming_curvature = 0.
+ dist_to_turn = 0.
+ road_points = None
+
+ xx = np.arange(0, MAPS_LOOKAHEAD_DISTANCE, 10)
+
+ while True:
+ gps = messaging.recv_one(gps_sock)
+ gps_ext = messaging.recv_one_or_none(gps_external_sock)
+
+ if gps_ext is not None:
+ gps = gps_ext.gpsLocationExternal
+ else:
+ gps = gps.gpsLocation
+
+ last_gps = gps
+
+ fix_ok = gps.flags & 1
+ if not fix_ok or last_query_result is None:
+ cur_way = None
+ curvature = None
+ curvature_valid = False
+ upcoming_curvature = 0.
+ dist_to_turn = 0.
+ road_points = None
+ else:
+ lat = gps.latitude
+ lon = gps.longitude
+ heading = gps.bearing
+ speed = gps.speed
+
+ query_lock.acquire()
+ cur_way = Way.closest(last_query_result, lat, lon, heading)
+ if cur_way is not None:
+ pnts, curvature_valid = cur_way.get_lookahead(last_query_result, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
+
+ xs = pnts[:, 0]
+ ys = pnts[:, 1]
+ road_points = map(float, xs), map(float, ys)
+
+ if speed < 10:
+ curvature_valid = False
+
+ # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
+ if curvature_valid:
+ # Compute the curvature for each point
+ with np.errstate(divide='ignore'):
+ circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
+ circles = np.asarray(circles)
+ radii = np.nan_to_num(circles[:, 2])
+ radii[radii < 10] = np.inf
+ curvature = 1. / radii
+
+ # Index of closest point
+ closest = np.argmin(np.linalg.norm(pnts, axis=1))
+ dist_to_closest = pnts[closest, 0] # We can use x distance here since it should be close
+
+ # Compute distance along path
+ dists = list()
+ dists.append(0)
+ for p, p_prev in zip(pnts, pnts[1:, :]):
+ dists.append(dists[-1] + np.linalg.norm(p - p_prev))
+ dists = np.asarray(dists)
+ dists = dists - dists[closest] + dist_to_closest
+
+ # TODO: Determine left or right turn
+ curvature = np.nan_to_num(curvature)
+ curvature_interp = np.interp(xx, dists[1:-1], curvature)
+ curvature_lookahead = curvature_interp[:int(speed * LOOKAHEAD_TIME / 10)]
+
+ # Outlier rejection
+ new_curvature = np.percentile(curvature_lookahead, 90)
+
+ k = 0.9
+ upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
+ in_turn_indices = curvature_interp > 0.8 * new_curvature
+ if np.any(in_turn_indices):
+ dist_to_turn = np.min(xx[in_turn_indices])
+ else:
+ dist_to_turn = 999
+ query_lock.release()
+
+ dat = messaging.new_message()
+ dat.init('liveMapData')
+
+ if last_gps is not None:
+ dat.liveMapData.lastGps = last_gps
+
+ if cur_way is not None:
+ dat.liveMapData.wayId = cur_way.id
+
+ # Seed limit
+ max_speed = cur_way.max_speed
+ if max_speed is not None:
+ dat.liveMapData.speedLimitValid = True
+ dat.liveMapData.speedLimit = max_speed
+
+ # Curvature
+ dat.liveMapData.curvatureValid = curvature_valid
+ dat.liveMapData.curvature = float(upcoming_curvature)
+ dat.liveMapData.distToTurn = float(dist_to_turn)
+ if road_points is not None:
+ dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
+ if curvature is not None:
+ dat.liveMapData.roadCurvatureX = map(float, xx)
+ dat.liveMapData.roadCurvature = map(float, curvature_interp)
+
+ map_data_sock.send(dat.to_bytes())
+
+
+def main(gctx=None):
+ main_thread = threading.Thread(target=mapsd_thread)
+ main_thread.daemon = True
+ main_thread.start()
+
+ q_thread = threading.Thread(target=query_thread)
+ q_thread.daemon = True
+ q_thread.start()
+
+ while True:
+ time.sleep(0.1)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/selfdrive/mapd/mapd_helpers.py b/selfdrive/mapd/mapd_helpers.py
new file mode 100644
index 00000000000000..f3c9c3445ad1a2
--- /dev/null
+++ b/selfdrive/mapd/mapd_helpers.py
@@ -0,0 +1,223 @@
+import math
+import numpy as np
+from datetime import datetime
+
+from selfdrive.config import Conversions as CV
+from common.transformations.coordinates import LocalCoord, geodetic2ecef
+
+LOOKAHEAD_TIME = 10.
+MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME
+
+
+def circle_through_points(p1, p2, p3):
+ """Fits a circle through three points
+ Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm"""
+ x1, y1, _ = p1
+ x2, y2, _ = p2
+ x3, y3, _ = p3
+
+ A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2
+ B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1)
+ C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2)
+ D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2)
+
+ return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
+
+
+def parse_speed_unit(max_speed):
+ """Converts a maxspeed string to m/s based on the unit present in the input.
+ OpenStreetMap defaults to kph if no unit is present. """
+
+ conversion = CV.KPH_TO_MS
+ if 'mph' in max_speed:
+ max_speed = max_speed.replace(' mph', '')
+ conversion = CV.MPH_TO_MS
+
+ return float(max_speed) * conversion
+
+
+class Way:
+ def __init__(self, way):
+ self.id = way.id
+ self.way = way
+
+ points = list()
+
+ for node in self.way.get_nodes(resolve_missing=False):
+ points.append((float(node.lat), float(node.lon), 0.))
+
+ self.points = np.asarray(points)
+
+ @classmethod
+ def closest(cls, query_results, lat, lon, heading):
+ results, tree, real_nodes, node_to_way = query_results
+
+ cur_pos = geodetic2ecef((lat, lon, 0))
+ nodes = tree.query_ball_point(cur_pos, 500)
+
+ # If no nodes within 500m, choose closest one
+ if not nodes:
+ nodes = [tree.query(cur_pos)[1]]
+
+ ways = []
+ for n in nodes:
+ real_node = real_nodes[n]
+ ways += node_to_way[real_node.id]
+ ways = set(ways)
+
+ closest_way = None
+ best_score = None
+ for way in ways:
+ way = Way(way)
+ points = way.points_in_car_frame(lat, lon, heading)
+
+ on_way = way.on_way(lat, lon, heading, points)
+ if not on_way:
+ continue
+
+ # Create mask of points in front and behind
+ x = points[:, 0]
+ y = points[:, 1]
+ angles = np.arctan2(y, x)
+ front = np.logical_and((-np.pi / 2) < angles,
+ angles < (np.pi / 2))
+ behind = np.logical_not(front)
+
+ dists = np.linalg.norm(points, axis=1)
+
+ # Get closest point behind the car
+ dists_behind = np.copy(dists)
+ dists_behind[front] = np.NaN
+ closest_behind = points[np.nanargmin(dists_behind)]
+
+ # Get closest point in front of the car
+ dists_front = np.copy(dists)
+ dists_front[behind] = np.NaN
+ closest_front = points[np.nanargmin(dists_front)]
+
+ # fit line: y = a*x + b
+ x1, y1, _ = closest_behind
+ x2, y2, _ = closest_front
+ a = (y2 - y1) / max((x2 - x1), 1e-5)
+ b = y1 - a * x1
+
+ # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error
+ # (A 20 degree heading offset results in an a of about 1/3)
+ score = abs(a) * 60. + abs(b)
+ if closest_way is None or score < best_score:
+ closest_way = way
+ best_score = score
+
+ return closest_way
+
+ def __str__(self):
+ return "%s %s" % (self.id, self.way.tags)
+
+ @property
+ def max_speed(self):
+ """Extracts the (conditional) speed limit from a way"""
+ if not self.way:
+ return None
+
+ tags = self.way.tags
+ max_speed = None
+
+ if 'maxspeed' in tags:
+ max_speed = parse_speed_unit(tags['maxspeed'])
+
+ if 'maxspeed:conditional' in tags:
+ max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ')
+ cond = cond[1:-1]
+
+ start, end = cond.split('-')
+ now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays
+ start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
+ end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
+
+ if start <= now <= end:
+ max_speed = parse_speed_unit(max_speed_cond)
+
+ return max_speed
+
+ def on_way(self, lat, lon, heading, points=None):
+ if points is None:
+ points = self.points_in_car_frame(lat, lon, heading)
+ x = points[:, 0]
+ return np.min(x) < 0. and np.max(x) > 0.
+
+ def closest_point(self, lat, lon, heading, points=None):
+ if points is None:
+ points = self.points_in_car_frame(lat, lon, heading)
+ i = np.argmin(np.linalg.norm(points, axis=1))
+ return points[i]
+
+ def distance_to_closest_node(self, lat, lon, heading, points=None):
+ if points is None:
+ points = self.points_in_car_frame(lat, lon, heading)
+ return np.min(np.linalg.norm(points, axis=1))
+
+ def points_in_car_frame(self, lat, lon, heading):
+ lc = LocalCoord.from_geodetic([lat, lon, 0.])
+
+ # Build rotation matrix
+ heading = math.radians(-heading + 90)
+ c, s = np.cos(heading), np.sin(heading)
+ rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
+
+ # Convert to local coordinates
+ points_carframe = lc.geodetic2ned(self.points).T
+
+ # Rotate with heading of car
+ points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T
+
+ return points_carframe
+
+ def next_way(self, query_results, lat, lon, heading, backwards=False):
+ results, tree, real_nodes, node_to_way = query_results
+
+ if backwards:
+ node = self.way.nodes[0]
+ else:
+ node = self.way.nodes[-1]
+
+ ways = node_to_way[node.id]
+
+ way = None
+ try:
+ # Simple heuristic to find next way
+ ways = [w for w in ways if w.id != self.id and w.tags['highway'] == self.way.tags['highway']]
+ if len(ways) == 1:
+ way = Way(ways[0])
+ except KeyError:
+ pass
+
+ return way
+
+ def get_lookahead(self, query_results, lat, lon, heading, lookahead):
+ pnts = None
+ way = self
+ valid = False
+
+ for i in range(5):
+ # Get new points and append to list
+ new_pnts = way.points_in_car_frame(lat, lon, heading)
+
+ if pnts is None:
+ pnts = new_pnts
+ else:
+ pnts = np.vstack([pnts, new_pnts])
+
+ # Check current lookahead distance
+ max_dist = np.linalg.norm(pnts[-1, :])
+ if max_dist > lookahead:
+ valid = True
+
+ if max_dist > 2 * lookahead:
+ break
+
+ # Find next way
+ way = way.next_way(query_results, lat, lon, heading)
+ if not way:
+ break
+
+ return pnts, valid
diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd
index 9c48a5f6ac02f3..d38869ce22075e 100755
Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ
diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord
index 84137a9b8c2af2..84194bb06c9be6 100755
Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ
diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py
index 4238273d97ec7d..7f39d310d7afd6 100755
--- a/selfdrive/thermald.py
+++ b/selfdrive/thermald.py
@@ -84,6 +84,7 @@ def set_eon_fan(val):
# max fan speed only allowed if battery is hot
_BAT_TEMP_THERSHOLD = 45.
+
def handle_fan(max_cpu_temp, bat_temp, fan_speed):
new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_cpu_temp)
new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_cpu_temp)
@@ -103,6 +104,23 @@ def handle_fan(max_cpu_temp, bat_temp, fan_speed):
return fan_speed
+
+def check_car_battery_voltage(should_start, health, charging_disabled):
+
+ # charging disallowed if:
+ # - there are health packets from panda, and;
+ # - 12V battery voltage is too low, and;
+ # - onroad isn't started
+ if charging_disabled and (health is None or health.health.voltage > 11500):
+ charging_disabled = False
+ os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
+ elif not charging_disabled and health is not None and health.health.voltage < 11000 and not should_start:
+ charging_disabled = True
+ os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')
+
+ return charging_disabled
+
+
class LocationStarter(object):
def __init__(self):
self.last_good_loc = 0
@@ -133,6 +151,7 @@ def update(self, started_ts, location):
cloudlog.event("location_start", location=location.to_dict() if location else None)
return location.speed*3.6 > 10
+
def thermald_thread():
setup_eon_fan()
@@ -156,6 +175,10 @@ def thermald_thread():
health_sock.RCVTIMEO = 1500
current_filter = FirstOrderFilter(0., CURRENT_TAU, 1.)
+ # Make sure charging is enabled
+ charging_disabled = False
+ os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled')
+
params = Params()
while 1:
@@ -182,7 +205,6 @@ def thermald_thread():
msg.thermal.usbOnline = bool(int(f.read()))
current_filter.update(msg.thermal.batteryCurrent / 1e6)
- msg.thermal.chargerDisabled = current_filter.x > 1.0 # if current is ? 1A out, then charger might be off
# TODO: add car battery voltage check
max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
@@ -268,6 +290,10 @@ def thermald_thread():
started_seen and (sec_since_boot() - off_ts) > 60:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
+ charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled)
+
+ msg.thermal.chargingDisabled = charging_disabled
+ msg.thermal.chargingError = current_filter.x > 1.0 # if current is > 1A out, then charger might be off
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9*(started_ts or 0))
diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c
index ce57dd46517e75..dddf15f7e7167e 100644
--- a/selfdrive/ui/ui.c
+++ b/selfdrive/ui/ui.c
@@ -87,6 +87,8 @@ const int alert_sizes[] = {
[ALERTSIZE_FULL] = vwp_h,
};
+const int SET_SPEED_NA = 255;
+
// TODO: this is also hardcoded in common/transformations/camera.py
const mat3 intrinsic_matrix = (mat3){{
910., 0., 582.,
@@ -224,9 +226,13 @@ typedef struct UIState {
int awake_timeout;
int volume_timeout;
+ int speed_lim_off_timeout;
+ int is_metric_timeout;
int status;
bool is_metric;
+ float speed_lim_off;
+ bool is_ego_over_limit;
bool passive;
char alert_type[64];
char alert_sound[64];
@@ -282,6 +288,26 @@ static void set_do_exit(int sig) {
do_exit = 1;
}
+static void read_speed_lim_off(UIState *s) {
+ char *speed_lim_off = NULL;
+ read_db_value(NULL, "SpeedLimitOffset", &speed_lim_off, NULL);
+ s->speed_lim_off = 0.;
+ if (speed_lim_off) {
+ s->speed_lim_off = strtod(speed_lim_off, NULL);
+ free(speed_lim_off);
+ }
+ s->speed_lim_off_timeout = 2 * 60; // 2Hz
+}
+
+static void read_is_metric(UIState *s) {
+ char *is_metric;
+ const int result = read_db_value(NULL, "IsMetric", &is_metric, NULL);
+ if (result == 0) {
+ s->is_metric = is_metric[0] == '1';
+ free(is_metric);
+ }
+ s->is_metric_timeout = 2 * 60; // 2Hz
+}
static const char frame_vertex_shader[] =
"attribute vec4 aPosition;\n"
@@ -530,12 +556,9 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
0.0, 0.0, 0.0, 1.0,
}};
- char *value;
- const int result = read_db_value(NULL, "IsMetric", &value, NULL);
- if (result == 0) {
- s->is_metric = value[0] == '1';
- free(value);
- }
+ read_speed_lim_off(s);
+ read_is_metric(s);
+ s->is_metric_timeout = 60; // offset so values isn't read together with limit offset
}
static void ui_draw_transformed_box(UIState *s, uint32_t color) {
@@ -915,40 +938,80 @@ static void ui_draw_vision_maxspeed(UIState *s) {
const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx;
int ui_viz_rw = scene->ui_viz_rw;
- float maxspeed = s->scene.v_cruise;
- const int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2));
- const int viz_maxspeed_y = (box_y + (bdr_s*1.5));
- const int viz_maxspeed_w = 180;
- const int viz_maxspeed_h = 202;
char maxspeed_str[32];
- bool is_cruise_set = (maxspeed != 0 && maxspeed != 255);
+ float maxspeed = s->scene.v_cruise;
+ int maxspeed_calc = maxspeed * 0.6225 + 0.5;
+ float speedlimit = s->scene.speedlimit;
+ int speedlim_calc = speedlimit * 2.2369363 + 0.5;
+ int speed_lim_off = s->speed_lim_off * 2.2369363 + 0.5;
+ if (s->is_metric) {
+ maxspeed_calc = maxspeed + 0.5;
+ speedlim_calc = speedlimit * 3.6 + 0.5;
+ speed_lim_off = s->speed_lim_off * 3.6 + 0.5;
+ }
+
+ bool is_cruise_set = (maxspeed != 0 && maxspeed != SET_SPEED_NA);
+ bool is_speedlim_valid = s->scene.speedlimit_valid;
+ bool is_set_over_limit = is_speedlim_valid && s->scene.engaged &&
+ is_cruise_set && maxspeed_calc > (speedlim_calc + speed_lim_off);
+ int viz_maxspeed_w = 184;
+ int viz_maxspeed_h = 202;
+ int viz_maxspeed_x = (ui_viz_rx + (bdr_s*2));
+ int viz_maxspeed_y = (box_y + (bdr_s*1.5));
+ int viz_maxspeed_xo = 180;
+ viz_maxspeed_w += viz_maxspeed_xo;
+ viz_maxspeed_x += viz_maxspeed_w - (viz_maxspeed_xo * 2);
+
+ // Draw Background
+ nvgBeginPath(s->vg);
+ nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 30);
+ if (is_set_over_limit) {
+ nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180));
+ } else {
+ nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 100));
+ }
+ nvgFill(s->vg);
+
+ // Draw Border
nvgBeginPath(s->vg);
nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20);
- nvgStrokeColor(s->vg, nvgRGBA(255,255,255,80));
- nvgStrokeWidth(s->vg, 6);
+ if (is_set_over_limit) {
+ nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255));
+ } else if (is_speedlim_valid && !s->is_ego_over_limit) {
+ nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255));
+ } else if (is_speedlim_valid && s->is_ego_over_limit) {
+ nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 20));
+ } else {
+ nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 100));
+ }
+ nvgStrokeWidth(s->vg, 10);
nvgStroke(s->vg);
+ // Draw "MAX" Text
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
nvgFontFace(s->vg, "sans-regular");
nvgFontSize(s->vg, 26*2.5);
- nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
- nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 148, "MAX", NULL);
+ if (is_cruise_set) {
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 200));
+ } else {
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100));
+ }
+ nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 148, "MAX", NULL);
- nvgFontFace(s->vg, "sans-semibold");
- nvgFontSize(s->vg, 52*2.5);
- nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
+ // Draw Speed Text
+ nvgFontFace(s->vg, "sans-bold");
+ nvgFontSize(s->vg, 48*2.5);
if (is_cruise_set) {
- if (s->is_metric) {
- snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(maxspeed + 0.5));
- } else {
- snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(maxspeed * 0.6225 + 0.5));
- }
- nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 242, maxspeed_str, NULL);
+ snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", maxspeed_calc);
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
+ nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, maxspeed_str, NULL);
} else {
+ nvgFontFace(s->vg, "sans-semibold");
nvgFontSize(s->vg, 42*2.5);
- nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, 242, "N/A", NULL);
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100));
+ nvgText(s->vg, viz_maxspeed_x+(viz_maxspeed_xo/2)+(viz_maxspeed_w/2), 242, "N/A", NULL);
}
}
@@ -957,69 +1020,84 @@ static void ui_draw_vision_speedlimit(UIState *s) {
int ui_viz_rx = scene->ui_viz_rx;
int ui_viz_rw = scene->ui_viz_rw;
- if (!s->scene.speedlimit_valid){
- return;
- }
-
+ char speedlim_str[32];
float speedlimit = s->scene.speedlimit;
+ int speedlim_calc = speedlimit * 2.2369363 + 0.5;
+ if (s->is_metric) {
+ speedlim_calc = speedlimit * 3.6 + 0.5;
+ }
- const int viz_maxspeed_w = 180;
- const int viz_maxspeed_h = 202;
-
- const int viz_event_w = 220;
- const int viz_event_x = ((ui_viz_rx + ui_viz_rw) - (viz_event_w + (bdr_s*2)));
-
- const int viz_maxspeed_x = viz_event_x + (viz_event_w-viz_maxspeed_w);
- const int viz_maxspeed_y = (footer_y + ((footer_h - viz_maxspeed_h) / 2)) - 20;
+ bool is_speedlim_valid = s->scene.speedlimit_valid;
+ float hysteresis_offset = 0.5;
+ if (s->is_ego_over_limit) {
+ hysteresis_offset = 0.0;
+ }
+ s->is_ego_over_limit = is_speedlim_valid && s->scene.v_ego > (speedlimit + s->speed_lim_off + hysteresis_offset);
+
+ int viz_speedlim_w = 180;
+ int viz_speedlim_h = 202;
+ int viz_speedlim_x = (ui_viz_rx + (bdr_s*2));
+ int viz_speedlim_y = (box_y + (bdr_s*1.5));
+ if (!is_speedlim_valid) {
+ viz_speedlim_w -= 5;
+ viz_speedlim_h -= 10;
+ viz_speedlim_x += 9;
+ viz_speedlim_y += 5;
+ }
+ int viz_speedlim_bdr = is_speedlim_valid ? 30 : 15;
- char maxspeed_str[32];
+ // Draw Background
+ nvgBeginPath(s->vg);
+ nvgRoundedRect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, viz_speedlim_bdr);
+ if (is_speedlim_valid && s->is_ego_over_limit) {
+ nvgFillColor(s->vg, nvgRGBA(218, 111, 37, 180));
+ } else if (is_speedlim_valid) {
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
+ } else {
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100));
+ }
+ nvgFill(s->vg);
- if (s->is_metric) {
+ // Draw Border
+ if (is_speedlim_valid) {
+ nvgStrokeWidth(s->vg, 10);
+ nvgStroke(s->vg);
nvgBeginPath(s->vg);
- nvgCircle(s->vg, viz_maxspeed_x + viz_maxspeed_w / 2, viz_maxspeed_y + viz_maxspeed_h / 2, 127);
- nvgFillColor(s->vg, nvgRGBA(195, 0, 0, 255));
- nvgFill(s->vg);
+ nvgRoundedRect(s->vg, viz_speedlim_x, viz_speedlim_y, viz_speedlim_w, viz_speedlim_h, 20);
+ if (s->is_ego_over_limit) {
+ nvgStrokeColor(s->vg, nvgRGBA(218, 111, 37, 255));
+ } else if (is_speedlim_valid) {
+ nvgStrokeColor(s->vg, nvgRGBA(255, 255, 255, 255));
+ }
+ }
- nvgBeginPath(s->vg);
- nvgCircle(s->vg, viz_maxspeed_x + viz_maxspeed_w / 2, viz_maxspeed_y + viz_maxspeed_h / 2, 100);
+ // Draw "Speed Limit" Text
+ nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
+ nvgFontFace(s->vg, "sans-semibold");
+ nvgFontSize(s->vg, 50);
+ nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255));
+ if (is_speedlim_valid && s->is_ego_over_limit) {
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
- nvgFill(s->vg);
+ }
+ nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 50 : 45), "SPEED", NULL);
+ nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2 + (is_speedlim_valid ? 6 : 0), viz_speedlim_y + (is_speedlim_valid ? 90 : 85), "LIMIT", NULL);
- nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
- nvgFontFace(s->vg, "sans-bold");
- nvgFontSize(s->vg, 130);
+ // Draw Speed Text
+ nvgFontFace(s->vg, "sans-bold");
+ nvgFontSize(s->vg, 48*2.5);
+ if (s->is_ego_over_limit) {
+ nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
+ } else {
nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255));
-
- snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(speedlimit * 3.6 + 0.5));
- nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 135, maxspeed_str, NULL);
+ }
+ if (is_speedlim_valid) {
+ snprintf(speedlim_str, sizeof(speedlim_str), "%d", speedlim_calc);
+ nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), speedlim_str, NULL);
} else {
- const int border = 10;
- nvgBeginPath(s->vg);
- nvgRoundedRect(s->vg, viz_maxspeed_x - border, viz_maxspeed_y - border, viz_maxspeed_w + 2 * border, viz_maxspeed_h + 2 * border, 30);
- nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
- nvgFill(s->vg);
-
- nvgBeginPath(s->vg);
- nvgRoundedRect(s->vg, viz_maxspeed_x, viz_maxspeed_y, viz_maxspeed_w, viz_maxspeed_h, 20);
- nvgStrokeColor(s->vg, nvgRGBA(0, 0, 0, 255));
- nvgStrokeWidth(s->vg, 8);
- nvgStroke(s->vg);
-
-
- nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
nvgFontFace(s->vg, "sans-semibold");
- nvgFontSize(s->vg, 50);
- nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255));
- nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 50, "SPEED", NULL);
- nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 90, "LIMIT", NULL);
-
- nvgFontFace(s->vg, "sans-bold");
- nvgFontSize(s->vg, 120);
- nvgFillColor(s->vg, nvgRGBA(0, 0, 0, 255));
-
- snprintf(maxspeed_str, sizeof(maxspeed_str), "%d", (int)(speedlimit * 2.2369363 + 0.5));
- nvgText(s->vg, viz_maxspeed_x+viz_maxspeed_w/2, viz_maxspeed_y + 170, maxspeed_str, NULL);
- }
+ nvgFontSize(s->vg, 42*2.5);
+ nvgText(s->vg, viz_speedlim_x+viz_speedlim_w/2, viz_speedlim_y + (is_speedlim_valid ? 170 : 165), "N/A", NULL);
+ }
}
static void ui_draw_vision_speed(UIState *s) {
@@ -1137,6 +1215,7 @@ static void ui_draw_vision_header(UIState *s) {
nvgFill(s->vg);
ui_draw_vision_maxspeed(s);
+ ui_draw_vision_speedlimit(s);
ui_draw_vision_speed(s);
ui_draw_vision_wheel(s);
}
@@ -1151,8 +1230,6 @@ static void ui_draw_vision_footer(UIState *s) {
// Driver Monitoring
ui_draw_vision_face(s);
-
- ui_draw_vision_speedlimit(s);
}
static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
@@ -1710,7 +1787,7 @@ static void ui_update(UIState *s) {
struct cereal_LiveMapData datad;
cereal_read_LiveMapData(&datad, eventd.liveMapData);
s->scene.speedlimit = datad.speedLimit;
- s->scene.speedlimit_valid = datad.valid;
+ s->scene.speedlimit_valid = datad.speedLimitValid;
}
capn_free(&ctx);
zmq_msg_close(&msg);
@@ -1975,6 +2052,18 @@ int main() {
set_volume(s, volume);
}
+ if (s->speed_lim_off_timeout > 0) {
+ s->speed_lim_off_timeout--;
+ } else {
+ read_speed_lim_off(s);
+ }
+
+ if (s->is_metric_timeout > 0) {
+ s->is_metric_timeout--;
+ } else {
+ read_is_metric(s);
+ }
+
pthread_mutex_unlock(&s->lock);
// the bg thread needs to be scheduled, so the main thread needs time without the lock
diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond
index 40259afc55c789..caf5de38ae68a7 100755
Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ