mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-02-18 22:13:51 +08:00
openpilot v0.5.7 release
This commit is contained in:
Vehicle Researcher
82
README.md
82
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 | 25mph<sup>1</sup>| 25mph | Nidec |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| GMC<sup>3</sup>| Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Holden<sup>3</sup> | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| 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 | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
|
||||
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Ridgeline 2017-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Lexus | RX Hybrid 2016-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-18<sup>4</sup>| All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Highlander 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Highlander Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius Prime 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
|
||||
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
|
||||
| Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec |
|
||||
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| GMC<sup>3</sup> | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| Holden<sup>3</sup> | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
|
||||
| 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 | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
|
||||
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Ridgeline 2017-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Lexus | RX Hybrid 2016-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-18<sup>4</sup> | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Highlander 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Highlander Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius Prime 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
|
||||
<sup>1</sup>[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)***
|
||||
<sup>2</sup>When 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)
|
||||
<sup>3</sup>[GM installation guide](https://zoneos.com/volt/).
|
||||
<sup>4</sup>It 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/).
|
||||
<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
|
||||
<sup>1</sup>[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)***
|
||||
<sup>2</sup>When 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)
|
||||
<sup>3</sup>[GM installation guide](https://zoneos.com/volt/).
|
||||
<sup>4</sup>It 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/).
|
||||
<sup>5</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
|
||||
<sup>6</sup>Open 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. <br />
|
||||
<sup>7</sup>Community built Giraffe, find more information here, [GM Giraffe](https://zoneos.com/shop/) <br />
|
||||
|
||||
|
||||
10
RELEASES.md
10
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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -63,6 +63,7 @@ keys = {
|
||||
"IsUploadVideoOverCellularEnabled": TxType.PERSISTENT,
|
||||
"IsDriverMonitoringEnabled": TxType.PERSISTENT,
|
||||
"IsGeofenceEnabled": TxType.PERSISTENT,
|
||||
"SpeedLimitOffset": TxType.PERSISTENT,
|
||||
# written: visiond
|
||||
# read: visiond, controlsd
|
||||
"CalibrationParams": TxType.PERSISTENT,
|
||||
@@ -74,6 +75,8 @@ keys = {
|
||||
"DoUninstall": TxType.CLEAR_ON_MANAGER_START,
|
||||
"ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
|
||||
"IsUpdateAvailable": TxType.PERSISTENT,
|
||||
"LongitudinalControl": TxType.PERSISTENT,
|
||||
"LimitSetSpeed": TxType.PERSISTENT,
|
||||
|
||||
"RecordFront": TxType.PERSISTENT,
|
||||
}
|
||||
|
||||
@@ -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([
|
||||
|
||||
Binary file not shown.
1
phonelibs/openblas/libopenblas.so
Symbolic link
1
phonelibs/openblas/libopenblas.so
Symbolic link
@@ -0,0 +1 @@
|
||||
libopenblas_armv8p-r0.2.19.so
|
||||
BIN
phonelibs/openblas/libopenblas_armv8p-r0.2.19.so
Executable file
BIN
phonelibs/openblas/libopenblas_armv8p-r0.2.19.so
Executable file
Binary file not shown.
@@ -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
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -119,6 +119,7 @@ class CarInterface(object):
|
||||
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
|
||||
|
||||
@@ -62,6 +62,7 @@ class CarInterface(object):
|
||||
# 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
|
||||
|
||||
|
||||
@@ -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 @@ class CarState(object):
|
||||
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
|
||||
|
||||
@@ -151,10 +151,13 @@ class CarInterface(object):
|
||||
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 @@ class CarInterface(object):
|
||||
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
|
||||
|
||||
@@ -163,6 +163,7 @@ class CarInterface(object):
|
||||
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
|
||||
|
||||
@@ -47,6 +47,7 @@ class CarInterface(object):
|
||||
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
|
||||
|
||||
@@ -186,6 +186,7 @@ class CarInterface(object):
|
||||
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)
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "0.5.6-release"
|
||||
#define COMMA_VERSION "0.5.7-release"
|
||||
|
||||
@@ -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.latcontrol import LatControl
|
||||
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
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -6,6 +6,7 @@ import numpy as np
|
||||
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.speed_smoother import speed_smoother
|
||||
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 @@ class Planner(object):
|
||||
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 @@ class Planner(object):
|
||||
|
||||
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 @@ class Planner(object):
|
||||
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 @@ class Planner(object):
|
||||
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 @@ class Planner(object):
|
||||
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])]
|
||||
|
||||
@@ -6,13 +6,13 @@ import copy
|
||||
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 @@ class Calibrator(object):
|
||||
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())
|
||||
|
||||
Binary file not shown.
@@ -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 @@ managed_processes = {
|
||||
"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 @@ car_started_processes = [
|
||||
'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:
|
||||
|
||||
0
selfdrive/mapd/__init__.py
Normal file
0
selfdrive/mapd/__init__.py
Normal file
240
selfdrive/mapd/mapd.py
Executable file
240
selfdrive/mapd/mapd.py
Executable file
@@ -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()
|
||||
223
selfdrive/mapd/mapd_helpers.py
Normal file
223
selfdrive/mapd/mapd_helpers.py
Normal file
@@ -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
|
||||
Binary file not shown.
Binary file not shown.
@@ -84,6 +84,7 @@ _FAN_SPEEDS = [0, 16384, 32768, 65535]
|
||||
# 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 @@ class LocationStarter(object):
|
||||
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))
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
nvgFontFace(s->vg, "sans-semibold");
|
||||
nvgFontSize(s->vg, 52*2.5);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 255));
|
||||
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);
|
||||
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);
|
||||
|
||||
// Draw Speed Text
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 48*2.5);
|
||||
if (is_cruise_set) {
|
||||
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;
|
||||
}
|
||||
|
||||
float speedlimit = s->scene.speedlimit;
|
||||
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);
|
||||
|
||||
const int viz_maxspeed_w = 180;
|
||||
const int viz_maxspeed_h = 202;
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
char maxspeed_str[32];
|
||||
|
||||
if (s->is_metric) {
|
||||
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);
|
||||
|
||||
nvgBeginPath(s->vg);
|
||||
nvgCircle(s->vg, viz_maxspeed_x + viz_maxspeed_w / 2, viz_maxspeed_y + viz_maxspeed_h / 2, 100);
|
||||
// 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));
|
||||
nvgFill(s->vg);
|
||||
|
||||
nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
|
||||
nvgFontFace(s->vg, "sans-bold");
|
||||
nvgFontSize(s->vg, 130);
|
||||
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);
|
||||
} 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);
|
||||
nvgFillColor(s->vg, nvgRGBA(255, 255, 255, 100));
|
||||
}
|
||||
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);
|
||||
// Draw Border
|
||||
if (is_speedlim_valid) {
|
||||
nvgStrokeWidth(s->vg, 10);
|
||||
nvgStroke(s->vg);
|
||||
nvgBeginPath(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));
|
||||
}
|
||||
}
|
||||
|
||||
// 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));
|
||||
}
|
||||
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);
|
||||
// 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));
|
||||
}
|
||||
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 {
|
||||
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
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user