diff --git a/README.md b/README.md index 6393503ed..88f8eda8a 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ Also, we have a several thousand people community on [slack](https://slack.comma - + @@ -51,54 +51,56 @@ 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 | -| Chevrolet3| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom7| -| Chevrolet3| Volt 2017-18 | 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 | +| 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/)
+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/). Community Maintained Cars ------ diff --git a/RELEASES.md b/RELEASES.md index eef675a30..216feb83c 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 a9e5738a4..84ebf24ae 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 b86fa68f5..dc6b7f9d5 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 2f25aa0d1..bf982f8ab 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 389b550ad..e643728e0 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 3981a341f..cc137d5a7 100755 --- a/common/params.py +++ b/common/params.py @@ -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, } diff --git a/common/transformations/model.py b/common/transformations/model.py index 107c3d542..424bd6158 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 a5defb410..898644280 100755 Binary files a/installer/updater/updater and b/installer/updater/updater differ diff --git a/opendbc/generator/honda/honda_insight_ex_2019_can.dbc b/opendbc/generator/honda/honda_insight_ex_2019_can.dbc new file mode 100644 index 000000000..ff988cf9d --- /dev/null +++ b/opendbc/generator/honda/honda_insight_ex_2019_can.dbc @@ -0,0 +1,44 @@ +CM_ "IMPORT _bosch_2018.dbc" + +BO_ 419 GEARBOX: 8 PCM + SG_ GEAR : 7|8@0+ (1,0) [0|255] "" EON + SG_ GEAR_SHIFTER : 29|6@0+ (1,0) [0|63] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + +BO_ 432 STANDSTILL: 7 VSA + SG_ BRAKE_ERROR_1 : 13|1@0+ (1,0) [0|1] "" EON + SG_ BRAKE_ERROR_2 : 12|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON + +BO_ 927 RADAR_HUD: 8 RADAR + SG_ ZEROS_BOH : 7|10@0+ (1,0) [0|127] "" BDY + SG_ CMBS_OFF : 12|1@0+ (1,0) [0|1] "" BDY + SG_ RESUME_INSTRUCTION : 21|1@0+ (1,0) [0|1] "" XXX + SG_ SET_TO_1 : 13|1@0+ (1,0) [0|1] "" BDY + SG_ ZEROS_BOH2 : 11|4@0+ (1,0) [0|1] "" XXX + SG_ APPLY_BRAKES_FOR_CANC : 23|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_ALERTS : 20|5@0+ (1,0) [0|1] "" BDY + SG_ SET_TO_0 : 22|1@0+ (1,0) [0|1] "" XXX + SG_ HUD_LEAD : 40|1@0+ (1,0) [0|1] "" XXX + SG_ SET_TO_64 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|255] "" XXX + SG_ ZEROS_BOH3 : 47|7@0+ (1,0) [0|127] "" XXX + SG_ ZEROS_BOH4 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 1029 DOORS_STATUS: 8 BDY + SG_ DOOR_OPEN_FL : 37|1@0+ (1,0) [0|1] "" EON + SG_ DOOR_OPEN_FR : 38|1@0+ (1,0) [0|1] "" EON + SG_ DOOR_OPEN_RL : 39|1@0+ (1,0) [0|1] "" EON + SG_ DOOR_OPEN_RR : 40|1@0+ (1,0) [0|1] "" EON + SG_ TRUNK_OPEN : 41|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + + VAL_ 419 GEAR 10 "R" 1 "D" 0 "P"; + VAL_ 419 GEAR_SHIFTER 32 "D" 16 "N" 8 "R" 4 "P" ; + +CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0"; diff --git a/opendbc/generator/toyota/_toyota_2017.dbc b/opendbc/generator/toyota/_toyota_2017.dbc index 294b948dd..7fb249cac 100644 --- a/opendbc/generator/toyota/_toyota_2017.dbc +++ b/opendbc/generator/toyota/_toyota_2017.dbc @@ -248,9 +248,9 @@ VAL_ 1553 UNITS 1 "km" 2 "miles"; VAL_ 1556 TURN_SIGNALS 3 "none" 2 "right" 1 "left"; VAL_ 1161 TSGN1 1 "speed sign" 0 "none"; VAL_ 1161 TSGN2 1 "speed sign" 0 "none"; -VAL_ 1161 SPLSGN2 15 "conditional" 0 "none"; +VAL_ 1161 SPLSGN2 15 "conditional blank" 5 "rain" 0 "none"; VAL_ 1162 TSGN3 0 "none" 1 "speed sign" 2 "0 unlimited" 7 "unlimited" 16 "highway" 17 "no highway" 18 "motorway" 19 "no motorway" 20 "in city" 21 "outside city" 22 "pedestrian area" 23 "no pedestrian area" 65 "no overtaking left" 66 "no overtaking right" 67 "overtaking allowed again" 65 "no overtake" 129 "no entry"; -VAL_ 1162 SPLSGN3 15 "conditional" 0 "none"; +VAL_ 1162 SPLSGN3 15 "conditional blank" 5 "rain" 0 "none"; CM_ "CHFFR_METRIC 37 STEER_ANGLE STEER_ANGLE 0.36 180"; diff --git a/opendbc/gm_global_a_powertrain.dbc b/opendbc/gm_global_a_powertrain.dbc index 27f181dc0..1342c49f7 100644 --- a/opendbc/gm_global_a_powertrain.dbc +++ b/opendbc/gm_global_a_powertrain.dbc @@ -180,6 +180,7 @@ BO_ 880 ASCMActiveCruiseControlStatus: 6 K124_ASCM SG_ ACCGapLevel : 21|2@0+ (1,0) [0|0] "" NEO SG_ ACCResumeButton : 1|1@0+ (1,0) [0|0] "" NEO SG_ ACCCmdActive : 23|1@0+ (1,0) [0|0] "" NEO + SG_ FCWAlert : 41|2@0+ (1,0) [0|3] "" XXX BO_ 1001 ECMVehicleSpeed: 8 K20_ECM SG_ VehicleSpeed : 7|16@0+ (0.01,0) [0|0] "mph" NEO diff --git a/opendbc/honda_insight_ex_2019_can_generated.dbc b/opendbc/honda_insight_ex_2019_can_generated.dbc new file mode 100644 index 000000000..00050aa8d --- /dev/null +++ b/opendbc/honda_insight_ex_2019_can_generated.dbc @@ -0,0 +1,311 @@ +CM_ "AUTOGENERATED FILE, DO NOT EDIT" + + +CM_ "Imported file _bosch_2018.dbc starts here" +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BU_: EBCM EON CAM RADAR PCM EPS VSA SCM BDY XXX EPB + +BO_ 228 STEERING_CONTROL: 5 EON + SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS + SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS + SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS + SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS + SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS + SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS + +BO_ 232 BRAKE_HOLD: 7 XXX + SG_ XMISSION_SPEED : 7|14@0- (1,0) [1|0] "" XXX + SG_ COMPUTER_BRAKE : 39|16@0+ (1,0) [0|0] "" XXX + SG_ COMPUTER_BRAKE_REQUEST : 29|1@0+ (1,0) [0|0] "" XXX + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" XXX + +BO_ 304 GAS_PEDAL_2: 8 PCM + SG_ ENGINE_TORQUE_ESTIMATE : 7|16@0- (1,0) [-1000|1000] "Nm" EON + SG_ ENGINE_TORQUE_REQUEST : 23|16@0- (1,0) [-1000|1000] "Nm" EON + SG_ CAR_GAS : 39|8@0+ (1,0) [0|255] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 330 STEERING_SENSORS: 8 EPS + SG_ STEER_ANGLE : 7|16@0- (-0.1,0) [-500|500] "deg" EON + SG_ STEER_ANGLE_RATE : 23|16@0- (-1,0) [-3000|3000] "deg/s" EON + SG_ STEER_ANGLE_OFFSET : 39|8@0- (-0.1,0) [-128|127] "deg" EON + SG_ STEER_WHEEL_ANGLE : 47|16@0- (-0.1,0) [-500|500] "deg" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 344 ENGINE_DATA: 8 PCM + SG_ XMISSION_SPEED : 7|16@0+ (0.01,0) [0|250] "kph" EON + SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON + SG_ XMISSION_SPEED2 : 39|16@0+ (0.01,0) [0|250] "kph" EON + SG_ ODOMETER : 55|8@0+ (10,0) [0|2550] "m" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 380 POWERTRAIN_DATA: 8 PCM + SG_ PEDAL_GAS : 7|8@0+ (1,0) [0|255] "" EON + SG_ ENGINE_RPM : 23|16@0+ (1,0) [0|15000] "rpm" EON + SG_ GAS_PRESSED : 39|1@0+ (1,0) [0|1] "" EON + SG_ ACC_STATUS : 38|1@0+ (1,0) [0|1] "" EON + SG_ BOH_17C : 37|5@0+ (1,0) [0|1] "" EON + SG_ BRAKE_SWITCH : 32|1@0+ (1,0) [0|1] "" EON + SG_ BOH2_17C : 47|10@0+ (1,0) [0|1] "" EON + SG_ BRAKE_PRESSED : 53|1@0+ (1,0) [0|1] "" EON + SG_ BOH3_17C : 52|5@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 399 STEER_STATUS: 7 EPS + SG_ STEER_TORQUE_SENSOR : 7|16@0- (1,0) [-31000|31000] "tbd" EON + SG_ STEER_TORQUE_MOTOR : 23|16@0- (1,0) [-31000|31000] "tbd" EON + SG_ STEER_STATUS : 39|4@0+ (1,0) [0|15] "" EON + SG_ STEER_CONTROL_ACTIVE : 35|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON + +BO_ 420 VSA_STATUS: 8 VSA + SG_ ESP_DISABLED : 28|1@0+ (1,0) [0|1] "" EON + SG_ USER_BRAKE : 7|16@0+ (0.015625,-1.609375) [0|1000] "" EON + SG_ BRAKE_HOLD_ACTIVE : 46|1@0+ (1,0) [0|1] "" EON + SG_ BRAKE_HOLD_ENABLED : 45|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 450 EPB_STATUS: 8 EPB + SG_ EPB_ACTIVE : 3|1@0+ (1,0) [0|1] "" EON + SG_ EPB_STATE : 29|2@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 464 WHEEL_SPEEDS: 8 VSA + SG_ WHEEL_SPEED_FL : 7|15@0+ (0.01,0) [0|250] "kph" EON + SG_ WHEEL_SPEED_FR : 8|15@0+ (0.01,0) [0|250] "kph" EON + SG_ WHEEL_SPEED_RL : 25|15@0+ (0.01,0) [0|250] "kph" EON + SG_ WHEEL_SPEED_RR : 42|15@0+ (0.01,0) [0|250] "kph" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + +BO_ 479 ACC_CONTROL: 8 EON + SG_ SET_TO_1 : 20|5@0+ (1,0) [0|1] "" PCM + SG_ CONTROL_ON : 23|3@0+ (1,0) [0|5] "" XXX + SG_ RELATED_TO_GAS : 7|7@0+ (1,0) [0|69] "" XXX + SG_ GAS_COMMAND : 0|9@0+ (1,0) [0|1] "" PCM + SG_ GAS_BRAKE : 31|14@0- (1,0) [0|1] "" XXX + SG_ ZEROS_BOH : 33|18@0+ (1,0) [100|100] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 495 ACC_CONTROL_ON: 8 XXX + SG_ SET_TO_75 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_30 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ ZEROS_BOH : 23|8@0+ (1,0) [0|255] "" XXX + SG_ ZEROS_BOH2 : 47|16@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_FF : 15|8@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_3 : 6|7@0+ (1,0) [0|4095] "" XXX + SG_ CONTROL_ON : 7|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + +BO_ 545 XXX_16: 6 SCM + SG_ ECON_ON : 23|1@0+ (1,0) [0|1] "" XXX + SG_ DRIVE_MODE : 37|2@0+ (1,0) [0|3] "" XXX + SG_ COUNTER : 45|2@0+ (1,0) [0|3] "" BDY + SG_ CHECKSUM : 43|4@0+ (1,0) [0|15] "" BDY + +BO_ 597 ROUGH_WHEEL_SPEED: 8 VSA + SG_ WHEEL_SPEED_FL : 7|8@0+ (1,0) [0|255] "mph" EON + SG_ WHEEL_SPEED_FR : 15|8@0+ (1,0) [0|255] "mph" EON + SG_ WHEEL_SPEED_RL : 23|8@0+ (1,0) [0|255] "mph" EON + SG_ WHEEL_SPEED_RR : 31|8@0+ (1,0) [0|255] "mph" EON + SG_ SET_TO_X55 : 39|8@0+ (1,0) [0|255] "" XXX + SG_ SET_TO_X55_2 : 47|8@0+ (1,0) [0|255] "" EON + SG_ LONG_COUNTER : 55|8@0+ (1,0) [0|255] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + + BO_ 662 SCM_BUTTONS: 4 SCM + SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON + SG_ CRUISE_SETTING : 3|2@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 29|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 27|4@0+ (1,0) [0|15] "" EON + +BO_ 773 SEATBELT_STATUS: 7 BDY + SG_ SEATBELT_DRIVER_LAMP : 7|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_PASS_UNLATCHED : 10|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_PASS_LATCHED : 11|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_DRIVER_UNLATCHED : 12|1@0+ (1,0) [0|1] "" EON + SG_ SEATBELT_DRIVER_LATCHED : 13|1@0+ (1,0) [0|1] "" EON + SG_ PASS_AIRBAG_OFF : 14|1@0+ (1,0) [0|1] "" EON + SG_ PASS_AIRBAG_ON : 15|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 51|4@0+ (1,0) [0|3] "" EON + +BO_ 777 CAR_SPEED: 8 PCM + SG_ ROUGH_CAR_SPEED : 23|8@0+ (1,0) [0|255] "" XXX + SG_ CAR_SPEED : 7|16@0+ (1,0) [0|65535] "" XXX + SG_ ROUGH_CAR_SPEED_3 : 39|16@0+ (1,0) [0|65535] "" XXX + SG_ ROUGH_CAR_SPEED_2 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 780 ACC_HUD: 8 ADAS + SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "" BDY + SG_ DTC_MODE : 39|1@0+ (1,0) [0|1] "" BDY + SG_ BOH : 38|1@0+ (1,0) [0|1] "" BDY + SG_ FCM_PROBLEM : 34|1@0+ (1,0) [0|1] "" BDY + SG_ RADAR_OBSTRUCTED : 33|1@0+ (1,0) [0|1] "" BDY + SG_ ENABLE_MINI_CAR : 32|1@0+ (1,0) [0|1] "" BDY + SG_ BOH_3 : 43|1@0+ (1,0) [0|3] "" BDY + SG_ BOH_4 : 42|1@0+ (1,0) [0|3] "" BDY + SG_ BOH_5 : 41|1@0+ (1,0) [0|3] "" BDY + SG_ CRUISE_CONTROL_LABEL : 40|1@0+ (1,0) [0|3] "" BDY + SG_ ZEROS_BOH : 7|24@0+ (0.002759506,0) [0|100] "m/s" BDY + SG_ FCM_OFF : 35|1@0+ (1,0) [0|1] "" BDY + SG_ SET_TO_1 : 36|1@0+ (1,0) [0|1] "" XXX + SG_ HUD_DISTANCE : 47|2@0+ (1,0) [0|3] "" BDY + SG_ HUD_LEAD : 45|2@0+ (1,0) [0|3] "" BDY + SG_ ACC_PROBLEM : 37|1@0+ (1,0) [0|1] "" BDY + SG_ ACC_ON : 52|1@0+ (1,0) [0|1] "" XXX + SG_ BOH_6 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ SET_TO_X3 : 55|2@0+ (1,0) [0|3] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 804 CRUISE: 8 PCM + SG_ TRIP_FUEL_CONSUMED : 23|16@0+ (1,0) [0|255] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + +BO_ 806 SCM_FEEDBACK: 8 SCM + SG_ DRIVERS_DOOR_OPEN : 17|1@0+ (1,0) [0|1] "" XXX + SG_ MAIN_ON : 28|1@0+ (1,0) [0|1] "" EON + SG_ RIGHT_BLINKER : 27|1@0+ (1,0) [0|1] "" EON + SG_ LEFT_BLINKER : 26|1@0+ (1,0) [0|1] "" EON + SG_ CMBS_STATES : 22|2@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 829 LKAS_HUD: 5 ADAS + SG_ CAM_TEMP_HIGH : 7|1@0+ (1,0) [0|255] "" BDY + SG_ SET_ME_X41 : 6|7@0+ (1,0) [0|127] "" BDY + SG_ BOH : 6|7@0+ (1,0) [0|127] "" BDY + SG_ DASHED_LANES : 14|1@0+ (1,0) [0|1] "" BDY + SG_ DTC : 13|1@0+ (1,0) [0|1] "" BDY + SG_ LKAS_PROBLEM : 12|1@0+ (1,0) [0|1] "" BDY + SG_ LKAS_OFF : 11|1@0+ (1,0) [0|1] "" BDY + SG_ SOLID_LANES : 10|1@0+ (1,0) [0|1] "" BDY + SG_ LDW_RIGHT : 9|1@0+ (1,0) [0|1] "" BDY + SG_ STEERING_REQUIRED : 8|1@0+ (1,0) [0|1] "" BDY + SG_ BOH : 23|2@0+ (1,0) [0|4] "" BDY + SG_ LDW_PROBLEM : 21|1@0+ (1,0) [0|1] "" BDY + SG_ BEEP : 17|2@0+ (1,0) [0|1] "" BDY + SG_ LDW_ON : 28|1@0+ (1,0) [0|1] "" BDY + SG_ LDW_OFF : 27|1@0+ (1,0) [0|1] "" BDY + SG_ CLEAN_WINDSHIELD : 26|1@0+ (1,0) [0|1] "" BDY + SG_ SET_ME_X48 : 31|8@0+ (1,0) [0|255] "" BDY + SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" BDY + SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" BDY + +BO_ 862 CAMERA_MESSAGES: 8 CAM + SG_ ZEROS_BOH : 7|50@0+ (1,0) [0|127] "" BDY + SG_ AUTO_HIGHBEAMS_ACTIVE : 53|1@0+ (1,0) [0|1] "" XXX + SG_ HIGHBEAMS_ON : 52|1@0+ (1,0) [0|1] "" XXX + SG_ ZEROS_BOH_2 : 51|4@0+ (1,0) [0|15] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 884 STALK_STATUS: 8 XXX + SG_ AUTO_HEADLIGHTS : 46|1@0+ (1,0) [0|1] "" EON + SG_ HIGH_BEAM_HOLD : 47|1@0+ (1,0) [0|1] "" EON + SG_ HIGH_BEAM_FLASH : 45|1@0+ (1,0) [0|1] "" EON + SG_ HEADLIGHTS_ON : 54|1@0+ (1,0) [0|1] "" EON + SG_ WIPER_SWITCH : 53|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + +BO_ 891 STALK_STATUS_2: 8 XXX + SG_ WIPERS : 17|2@0+ (1,0) [0|3] "" EON + SG_ LOW_BEAMS : 35|1@0+ (1,0) [0|1] "" XXX + SG_ HIGH_BEAMS : 34|1@0+ (1,0) [0|1] "" XXX + SG_ PARK_LIGHTS : 36|1@0+ (1,0) [0|1] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + +CM_ "honda_insight_ex_2019_can.dbc starts here" + + +BO_ 419 GEARBOX: 8 PCM + SG_ GEAR : 7|8@0+ (1,0) [0|255] "" EON + SG_ GEAR_SHIFTER : 29|6@0+ (1,0) [0|63] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON + +BO_ 432 STANDSTILL: 7 VSA + SG_ BRAKE_ERROR_1 : 13|1@0+ (1,0) [0|1] "" EON + SG_ BRAKE_ERROR_2 : 12|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 53|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 51|4@0+ (1,0) [0|15] "" EON + +BO_ 927 RADAR_HUD: 8 RADAR + SG_ ZEROS_BOH : 7|10@0+ (1,0) [0|127] "" BDY + SG_ CMBS_OFF : 12|1@0+ (1,0) [0|1] "" BDY + SG_ RESUME_INSTRUCTION : 21|1@0+ (1,0) [0|1] "" XXX + SG_ SET_TO_1 : 13|1@0+ (1,0) [0|1] "" BDY + SG_ ZEROS_BOH2 : 11|4@0+ (1,0) [0|1] "" XXX + SG_ APPLY_BRAKES_FOR_CANC : 23|1@0+ (1,0) [0|1] "" XXX + SG_ ACC_ALERTS : 20|5@0+ (1,0) [0|1] "" BDY + SG_ SET_TO_0 : 22|1@0+ (1,0) [0|1] "" XXX + SG_ HUD_LEAD : 40|1@0+ (1,0) [0|1] "" XXX + SG_ SET_TO_64 : 31|8@0+ (1,0) [0|255] "" XXX + SG_ LEAD_DISTANCE : 39|8@0+ (1,0) [0|255] "" XXX + SG_ ZEROS_BOH3 : 47|7@0+ (1,0) [0|127] "" XXX + SG_ ZEROS_BOH4 : 55|8@0+ (1,0) [0|255] "" XXX + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX + +BO_ 1029 DOORS_STATUS: 8 BDY + SG_ DOOR_OPEN_FL : 37|1@0+ (1,0) [0|1] "" EON + SG_ DOOR_OPEN_FR : 38|1@0+ (1,0) [0|1] "" EON + SG_ DOOR_OPEN_RL : 39|1@0+ (1,0) [0|1] "" EON + SG_ DOOR_OPEN_RR : 40|1@0+ (1,0) [0|1] "" EON + SG_ TRUNK_OPEN : 41|1@0+ (1,0) [0|1] "" EON + SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON + SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON + + VAL_ 419 GEAR 10 "R" 1 "D" 0 "P"; + VAL_ 419 GEAR_SHIFTER 32 "D" 16 "N" 8 "R" 4 "P" ; + +CM_ "CHFFR_METRIC 330 STEER_ANGLE STEER_ANGLE 0.36 180; CHFFR_METRIC 380 ENGINE_RPM ENGINE_RPM 1 0; CHFFR_METRIC 804 ENGINE_TEMPERATURE ENGINE_TEMPERATURE 1 0"; diff --git a/panda/VERSION b/panda/VERSION index 301779b03..02e8c95da 100644 --- a/panda/VERSION +++ b/panda/VERSION @@ -1 +1 @@ -v1.1.7 \ No newline at end of file +v1.1.8 \ No newline at end of file diff --git a/panda/board/main.c b/panda/board/main.c index 8bb6a0cee..b1033bb73 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -589,6 +589,8 @@ int main() { uint64_t marker = 0; #define CURRENT_THRESHOLD 0xF00 #define CLICKS 8 + // Enough clicks to ensure that enumeration happened. Should be longer than bootup time of the device connected to EON + #define CLICKS_BOOTUP 30 #endif for (cnt=0;;cnt++) { @@ -615,8 +617,8 @@ int main() { } break; case USB_POWER_CDP: - // been CLICKS clicks since we switched to CDP - if ((cnt-marker) >= CLICKS) { + // been CLICKS_BOOTUP clicks since we switched to CDP + if ((cnt-marker) >= CLICKS_BOOTUP ) { // measure current draw, if positive and no enumeration, switch to DCP if (!is_enumerated && current < CURRENT_THRESHOLD) { puts("USBP: no enumeration with current draw, switching to DCP mode\n"); diff --git a/phonelibs/openblas/libopenblas.so b/phonelibs/openblas/libopenblas.so new file mode 120000 index 000000000..7a792bc90 --- /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 000000000..ace58c8a1 Binary files /dev/null and b/phonelibs/openblas/libopenblas_armv8p-r0.2.19.so differ diff --git a/pyextra/overpy-0.4-py2.7.egg-info/PKG-INFO b/pyextra/overpy-0.4-py2.7.egg-info/PKG-INFO deleted file mode 100644 index 96d494a14..000000000 --- a/pyextra/overpy-0.4-py2.7.egg-info/PKG-INFO +++ /dev/null @@ -1,123 +0,0 @@ -Metadata-Version: 1.1 -Name: overpy -Version: 0.4 -Summary: Python Wrapper to access the OpenStreepMap Overpass API -Home-page: https://github.com/DinoTools/python-overpy -Author: PhiBo (DinoTools) -Author-email: UNKNOWN -License: MIT -Description: Python Overpass Wrapper - ======================= - - A Python Wrapper to access the Overpass API. - - Have a look at the `documentation`_ to find additional information. - - .. image:: https://pypip.in/version/overpy/badge.svg - :target: https://pypi.python.org/pypi/overpy/ - :alt: Latest Version - - .. image:: https://pypip.in/license/overpy/badge.svg - :target: https://pypi.python.org/pypi/overpy/ - :alt: License - - .. image:: https://travis-ci.org/DinoTools/python-overpy.svg?branch=master - :target: https://travis-ci.org/DinoTools/python-overpy - - .. image:: https://coveralls.io/repos/DinoTools/python-overpy/badge.png?branch=master - :target: https://coveralls.io/r/DinoTools/python-overpy?branch=master - - Features - -------- - - * Query Overpass API - * Parse JSON and XML response data - * Additional helper functions - - Install - ------- - - **Requirements:** - - Supported Python versions: - - * Python 2.7 - * Python >= 3.2 - * PyPy and PyPy3 - - **Install:** - - .. code-block:: console - - $ pip install overpy - - Examples - -------- - - Additional examples can be found in the `documentation`_ and in the *examples* directory. - - .. code-block:: python - - import overpy - - api = overpy.Overpass() - - # fetch all ways and nodes - result = api.query(""" - way(50.746,7.154,50.748,7.157) ["highway"]; - (._;>;); - out body; - """) - - for way in result.ways: - print("Name: %s" % way.tags.get("name", "n/a")) - print(" Highway: %s" % way.tags.get("highway", "n/a")) - print(" Nodes:") - for node in way.nodes: - print(" Lat: %f, Lon: %f" % (node.lat, node.lon)) - - - Helper - ~~~~~~ - - Helper methods are available to provide easy access to often used requests. - - .. code-block:: python - - import overpy.helper - - # 3600062594 is the OSM id of Chemnitz and is the bounding box for the request - street = overpy.helper.get_street( - "Straße der Nationen", - "3600062594" - ) - - # this finds an intersection between Straße der Nationen and Carolastraße in Chemnitz - intersection = overpy.helper.get_intersection( - "Straße der Nationen", - "Carolastraße", - "3600062594" - ) - - - License - ------- - - Published under the MIT (see LICENSE for more information) - - .. _`documentation`: http://python-overpy.readthedocs.org/ - -Keywords: OverPy Overpass OSM OpenStreetMap -Platform: UNKNOWN -Classifier: Development Status :: 4 - Beta -Classifier: License :: OSI Approved :: MIT License -Classifier: Operating System :: OS Independent -Classifier: Programming Language :: Python -Classifier: Programming Language :: Python :: 2.7 -Classifier: Programming Language :: Python :: 3 -Classifier: Programming Language :: Python :: 3.2 -Classifier: Programming Language :: Python :: 3.3 -Classifier: Programming Language :: Python :: 3.4 -Classifier: Programming Language :: Python :: 3.5 -Classifier: Programming Language :: Python :: Implementation :: CPython -Classifier: Programming Language :: Python :: Implementation :: PyPy diff --git a/pyextra/overpy-0.4-py2.7.egg-info/SOURCES.txt b/pyextra/overpy-0.4-py2.7.egg-info/SOURCES.txt deleted file mode 100644 index 66bf17f7c..000000000 --- a/pyextra/overpy-0.4-py2.7.egg-info/SOURCES.txt +++ /dev/null @@ -1,61 +0,0 @@ -CHANGELOG.rst -LICENSE -MANIFEST.in -README.rst -setup.cfg -setup.py -docs/make.bat -docs/source/api.rst -docs/source/changelog.rst -docs/source/conf.py -docs/source/contributing.rst -docs/source/example.rst -docs/source/index.rst -docs/source/introduction.rst -examples/get_areas.py -examples/get_nodes.py -examples/get_ways.py -overpy/__about__.py -overpy/__init__.py -overpy/exception.py -overpy/helper.py -overpy.egg-info/PKG-INFO -overpy.egg-info/SOURCES.txt -overpy.egg-info/dependency_links.txt -overpy.egg-info/not-zip-safe -overpy.egg-info/top_level.txt -tests/__init__.py -tests/base_class.py -tests/test_exception.py -tests/test_json.py -tests/test_request.py -tests/test_result.py -tests/test_result_way.py -tests/test_xml.py -tests/json/area-01.json -tests/json/node-01.json -tests/json/relation-01.json -tests/json/relation-02.json -tests/json/relation-03.json -tests/json/relation-04.json -tests/json/result-expand-01.json -tests/json/result-expand-02.json -tests/json/result-way-01.json -tests/json/result-way-02.json -tests/json/result-way-03.json -tests/json/way-01.json -tests/json/way-02.json -tests/json/way-03.json -tests/json/way-04.json -tests/response/bad-request-encoding.html -tests/response/bad-request.html -tests/xml/area-01.xml -tests/xml/node-01.xml -tests/xml/relation-01.xml -tests/xml/relation-02.xml -tests/xml/relation-03.xml -tests/xml/relation-04.xml -tests/xml/way-01.xml -tests/xml/way-02.xml -tests/xml/way-03.xml -tests/xml/way-04.xml \ No newline at end of file diff --git a/pyextra/overpy-0.4-py2.7.egg-info/dependency_links.txt b/pyextra/overpy-0.4-py2.7.egg-info/dependency_links.txt deleted file mode 100644 index 8b1378917..000000000 --- a/pyextra/overpy-0.4-py2.7.egg-info/dependency_links.txt +++ /dev/null @@ -1 +0,0 @@ - diff --git a/pyextra/overpy-0.4-py2.7.egg-info/installed-files.txt b/pyextra/overpy-0.4-py2.7.egg-info/installed-files.txt deleted file mode 100644 index 0cd05ada1..000000000 --- a/pyextra/overpy-0.4-py2.7.egg-info/installed-files.txt +++ /dev/null @@ -1,56 +0,0 @@ -../overpy/__about__.py -../overpy/__about__.pyc -../overpy/__init__.py -../overpy/__init__.pyc -../overpy/exception.py -../overpy/exception.pyc -../overpy/helper.py -../overpy/helper.pyc -../tests/__init__.py -../tests/__init__.pyc -../tests/base_class.py -../tests/base_class.pyc -../tests/json/area-01.json -../tests/json/node-01.json -../tests/json/relation-01.json -../tests/json/relation-02.json -../tests/json/relation-03.json -../tests/json/relation-04.json -../tests/json/result-expand-01.json -../tests/json/result-expand-02.json -../tests/json/result-way-01.json -../tests/json/result-way-02.json -../tests/json/result-way-03.json -../tests/json/way-01.json -../tests/json/way-02.json -../tests/json/way-03.json -../tests/json/way-04.json -../tests/response/bad-request-encoding.html -../tests/response/bad-request.html -../tests/test_exception.py -../tests/test_exception.pyc -../tests/test_json.py -../tests/test_json.pyc -../tests/test_request.py -../tests/test_request.pyc -../tests/test_result.py -../tests/test_result.pyc -../tests/test_result_way.py -../tests/test_result_way.pyc -../tests/test_xml.py -../tests/test_xml.pyc -../tests/xml/area-01.xml -../tests/xml/node-01.xml -../tests/xml/relation-01.xml -../tests/xml/relation-02.xml -../tests/xml/relation-03.xml -../tests/xml/relation-04.xml -../tests/xml/way-01.xml -../tests/xml/way-02.xml -../tests/xml/way-03.xml -../tests/xml/way-04.xml -PKG-INFO -SOURCES.txt -dependency_links.txt -not-zip-safe -top_level.txt diff --git a/pyextra/overpy-0.4-py2.7.egg-info/not-zip-safe b/pyextra/overpy-0.4-py2.7.egg-info/not-zip-safe deleted file mode 100644 index 8b1378917..000000000 --- a/pyextra/overpy-0.4-py2.7.egg-info/not-zip-safe +++ /dev/null @@ -1 +0,0 @@ - diff --git a/pyextra/overpy-0.4-py2.7.egg-info/top_level.txt b/pyextra/overpy-0.4-py2.7.egg-info/top_level.txt deleted file mode 100644 index 4611d9bb6..000000000 --- a/pyextra/overpy-0.4-py2.7.egg-info/top_level.txt +++ /dev/null @@ -1,2 +0,0 @@ -overpy -tests diff --git a/pyextra/overpy/__init__.py b/pyextra/overpy/__init__.py index 8e038bda9..2836080ab 100644 --- a/pyextra/overpy/__init__.py +++ b/pyextra/overpy/__init__.py @@ -5,6 +5,8 @@ from xml.sax import handler, make_parser import json import re import sys +import time +import requests from overpy import exception from overpy.__about__ import ( @@ -18,12 +20,15 @@ PY3 = sys.version_info[0] == 3 XML_PARSER_DOM = 1 XML_PARSER_SAX = 2 -if PY2: - from urllib2 import urlopen - from urllib2 import HTTPError -elif PY3: - from urllib.request import urlopen - from urllib.error import HTTPError +# Try to convert some common attributes +# http://wiki.openstreetmap.org/wiki/Elements#Common_attributes +GLOBAL_ATTRIBUTE_MODIFIERS = { + "changeset": int, + "timestamp": lambda ts: datetime.strptime(ts, "%Y-%m-%dT%H:%M:%SZ"), + "uid": int, + "version": int, + "visible": lambda v: v.lower() == "true" +} def is_valid_type(element, cls): @@ -41,11 +46,16 @@ def is_valid_type(element, cls): class Overpass(object): """ Class to access the Overpass API + + :cvar default_max_retry_count: Global max number of retries (Default: 0) + :cvar default_retry_timeout: Global time to wait between tries (Default: 1.0s) """ + default_max_retry_count = 0 default_read_chunk_size = 4096 + default_retry_timeout = 1.0 default_url = "http://overpass-api.de/api/interpreter" - def __init__(self, read_chunk_size=None, url=None, xml_parser=XML_PARSER_SAX): + def __init__(self, read_chunk_size=None, url=None, xml_parser=XML_PARSER_SAX, max_retry_count=None, retry_timeout=None, timeout=5.0, headers=None): """ :param read_chunk_size: Max size of each chunk read from the server response :type read_chunk_size: Integer @@ -53,6 +63,14 @@ class Overpass(object): :type url: str :param xml_parser: The xml parser to use :type xml_parser: Integer + :param max_retry_count: Max number of retries (Default: default_max_retry_count) + :type max_retry_count: Integer + :param retry_timeout: Time to wait between tries (Default: default_retry_timeout) + :type retry_timeout: float + :param timeout: HTTP request timeout + :type timeout: float + :param headers: HTTP request headers + :type headers: dict """ self.url = self.default_url if url is not None: @@ -63,7 +81,34 @@ class Overpass(object): if read_chunk_size is None: read_chunk_size = self.default_read_chunk_size self.read_chunk_size = read_chunk_size + + if max_retry_count is None: + max_retry_count = self.default_max_retry_count + self.max_retry_count = max_retry_count + + if retry_timeout is None: + retry_timeout = self.default_retry_timeout + self.retry_timeout = retry_timeout + self.xml_parser = xml_parser + self.timeout = timeout + self.headers = headers + + def _handle_remark_msg(self, msg): + """ + Try to parse the message provided with the remark tag or element. + + :param str msg: The message + :raises overpy.exception.OverpassRuntimeError: If message starts with 'runtime error:' + :raises overpy.exception.OverpassRuntimeRemark: If message starts with 'runtime remark:' + :raises overpy.exception.OverpassUnknownError: If we are unable to identify the error + """ + msg = msg.strip() + if msg.startswith("runtime error:"): + raise exception.OverpassRuntimeError(msg=msg) + elif msg.startswith("runtime remark:"): + raise exception.OverpassRuntimeRemark(msg=msg) + raise exception.OverpassUnknownError(msg=msg) def query(self, query): """ @@ -76,56 +121,79 @@ class Overpass(object): if not isinstance(query, bytes): query = query.encode("utf-8") - try: - f = urlopen(self.url, query) - except HTTPError as e: - f = e + retry_num = 0 + retry_exceptions = [] + do_retry = True if self.max_retry_count > 0 else False + while retry_num <= self.max_retry_count: + if retry_num > 0: + time.sleep(self.retry_timeout) + retry_num += 1 - response = f.read(self.read_chunk_size) - while True: - data = f.read(self.read_chunk_size) - if len(data) == 0: - break - response = response + data - f.close() + try: + if self.headers is not None: + r = requests.post(self.url, query, timeout=self.timeout, headers=self.headers) + else: + r = requests.post(self.url, query, timeout=self.timeout) + response = r.content + except (requests.exceptions.BaseHTTPError, requests.exceptions.RequestException) as e: + if not do_retry: + raise e + retry_exceptions.append(e) + continue - if f.code == 200: - if PY2: - http_info = f.info() - content_type = http_info.getheader("content-type") - else: - content_type = f.getheader("Content-Type") + if r.status_code == 200: + content_type = r.headers["Content-Type"] - if content_type == "application/json": - return self.parse_json(response) + if content_type == "application/json": + return self.parse_json(response) - if content_type == "application/osm3s+xml": - return self.parse_xml(response) + if content_type == "application/osm3s+xml": + return self.parse_xml(response) - raise exception.OverpassUnknownContentType(content_type) + e = exception.OverpassUnknownContentType(content_type) + if not do_retry: + raise e + retry_exceptions.append(e) + continue + elif r.status_code == 400: + msgs = [] + for msg in self._regex_extract_error_msg.finditer(response): + tmp = self._regex_remove_tag.sub(b"", msg.group("msg")) + try: + tmp = tmp.decode("utf-8") + except UnicodeDecodeError: + tmp = repr(tmp) + msgs.append(tmp) - if f.code == 400: - msgs = [] - for msg in self._regex_extract_error_msg.finditer(response): - tmp = self._regex_remove_tag.sub(b"", msg.group("msg")) - try: - tmp = tmp.decode("utf-8") - except UnicodeDecodeError: - tmp = repr(tmp) - msgs.append(tmp) + e = exception.OverpassBadRequest( + query, + msgs=msgs + ) + if not do_retry: + raise e + retry_exceptions.append(e) + continue + elif r.status_code == 429: + e = exception.OverpassTooManyRequests + if not do_retry: + raise e + retry_exceptions.append(e) + continue + elif r.status_code == 504: + e = exception.OverpassGatewayTimeout + if not do_retry: + raise e + retry_exceptions.append(e) + continue - raise exception.OverpassBadRequest( - query, - msgs=msgs - ) + # No valid response code + e = exception.OverpassUnknownHTTPStatusCode(r.status_code) + if not do_retry: + raise e + retry_exceptions.append(e) + continue - if f.code == 429: - raise exception.OverpassTooManyRequests - - if f.code == 504: - raise exception.OverpassGatewayTimeout - - raise exception.OverpassUnknownHTTPStatusCode(f.code) + raise exception.MaxRetriesReached(retry_count=retry_num, exceptions=retry_exceptions) def parse_json(self, data, encoding="utf-8"): """ @@ -139,8 +207,11 @@ class Overpass(object): :rtype: overpy.Result """ if isinstance(data, bytes): - data = data.decode(encoding) + data = data.decode(encoding) + data = json.loads(data, parse_float=Decimal) + if "remark" in data: + self._handle_remark_msg(msg=data.get("remark")) return Result.from_json(data, api=self) def parse_xml(self, data, encoding="utf-8", parser=None): @@ -155,13 +226,16 @@ class Overpass(object): """ if parser is None: parser = self.xml_parser - if isinstance(data, bytes): data = data.decode(encoding) if PY2 and not isinstance(data, str): # Python 2.x: Convert unicode strings data = data.encode(encoding) + m = re.compile("(?P[^<>]*)").search(data) + if m: + self._handle_remark_msg(m.group("msg")) + return Result.from_xml(data, api=self, parser=parser) @@ -279,23 +353,39 @@ class Result(object): return result @classmethod - def from_xml(cls, data, api=None, parser=XML_PARSER_SAX): + def from_xml(cls, data, api=None, parser=None): """ - Create a new instance and load data from xml object. + Create a new instance and load data from xml data or object. + + .. note:: + If parser is set to None, the functions tries to find the best parse. + By default the SAX parser is chosen if a string is provided as data. + The parser is set to DOM if an xml.etree.ElementTree.Element is provided as data value. :param data: Root element - :type data: xml.etree.ElementTree.Element - :param api: + :type data: str | xml.etree.ElementTree.Element + :param api: The instance to query additional information if required. :type api: Overpass - :param parser: Specify the parser to use(DOM or SAX) - :type parser: Integer + :param parser: Specify the parser to use(DOM or SAX)(Default: None = autodetect, defaults to SAX) + :type parser: Integer | None :return: New instance of Result object :rtype: Result """ + if parser is None: + if isinstance(data, str): + parser = XML_PARSER_SAX + else: + parser = XML_PARSER_DOM + result = cls(api=api) if parser == XML_PARSER_DOM: import xml.etree.ElementTree as ET - root = ET.fromstring(data) + if isinstance(data, str): + root = ET.fromstring(data) + elif isinstance(data, ET.Element): + root = data + else: + raise exception.OverPyException("Unable to detect data type.") for elem_cls in [Node, Way, Relation, Area]: for child in root: @@ -522,17 +612,10 @@ class Element(object): """ self._result = result - # Try to convert some common attributes - # http://wiki.openstreetmap.org/wiki/Elements#Common_attributes - self._attribute_modifiers = { - "changeset": int, - "timestamp": lambda ts: datetime.strptime(ts, "%Y-%m-%dT%H:%M:%SZ"), - "uid": int, - "version": int, - "visible": lambda v: v.lower() == "true" - } self.attributes = attributes - for n, m in self._attribute_modifiers.items(): + # ToDo: Add option to modify attribute modifiers + attribute_modifiers = dict(GLOBAL_ATTRIBUTE_MODIFIERS.items()) + for n, m in attribute_modifiers.items(): if n in self.attributes: self.attributes[n] = m(self.attributes[n]) self.id = None diff --git a/pyextra/overpy/exception.py b/pyextra/overpy/exception.py index 7179d246a..3d8416a12 100644 --- a/pyextra/overpy/exception.py +++ b/pyextra/overpy/exception.py @@ -37,6 +37,18 @@ class ElementDataWrongType(OverPyException): ) +class MaxRetriesReached(OverPyException): + """ + Raised if max retries reached and the Overpass server didn't respond with a result. + """ + def __init__(self, retry_count, exceptions): + self.exceptions = exceptions + self.retry_count = retry_count + + def __str__(self): + return "Unable get any result from the Overpass API server after %d retries." % self.retry_count + + class OverpassBadRequest(OverPyException): """ Raised if the Overpass API service returns a syntax error. @@ -62,6 +74,29 @@ class OverpassBadRequest(OverPyException): return "\n".join(tmp_msgs) +class OverpassError(OverPyException): + """ + Base exception to report errors if the response returns a remark tag or element. + + .. note:: + If you are not sure which of the subexceptions you should use, use this one and try to parse the message. + + For more information have a look at https://github.com/DinoTools/python-overpy/issues/62 + + :param str msg: The message from the remark tag or element + """ + def __init__(self, msg=None): + #: The message from the remark tag or element + self.msg = msg + + def __str__(self): + if self.msg is None: + return "No error message provided" + if not isinstance(self.msg, str): + return str(self.msg) + return self.msg + + class OverpassGatewayTimeout(OverPyException): """ Raised if load of the Overpass API service is too high and it can't handle the request. @@ -70,6 +105,22 @@ class OverpassGatewayTimeout(OverPyException): OverPyException.__init__(self, "Server load too high") +class OverpassRuntimeError(OverpassError): + """ + Raised if the server returns a remark-tag(xml) or remark element(json) with a message starting with + 'runtime error:'. + """ + pass + + +class OverpassRuntimeRemark(OverpassError): + """ + Raised if the server returns a remark-tag(xml) or remark element(json) with a message starting with + 'runtime remark:'. + """ + pass + + class OverpassTooManyRequests(OverPyException): """ Raised if the Overpass API service returns a 429 status code. @@ -94,6 +145,13 @@ class OverpassUnknownContentType(OverPyException): return "Unknown content type: %s" % self.content_type +class OverpassUnknownError(OverpassError): + """ + Raised if the server returns a remark-tag(xml) or remark element(json) and we are unable to find any reason. + """ + pass + + class OverpassUnknownHTTPStatusCode(OverPyException): """ Raised if the returned HTTP status code isn't handled by OverPy. diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt index deb37d086..b623e2a25 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 d897a815f..1e7b233dd 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 1f0e36e0d..633ce4b8a 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -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 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 5d86c0605..fd90ffd4d 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -10,7 +10,7 @@ from selfdrive.can.packer import CANPacker class CarControllerParams(): def __init__(self, car_fingerprint): - if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): self.STEER_MAX = 300 self.STEER_STEP = 2 # how often we update the steer cmd self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) @@ -104,7 +104,7 @@ class CarController(object): self.apply_steer_last = apply_steer idx = (frame / P.STEER_STEP) % 4 - if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): can_sends.append(gmcan.create_steering_control(self.packer_pt, canbus.powertrain, apply_steer, idx, lkas_enabled)) if self.car_fingerprint == CAR.CADILLAC_CT6: @@ -113,7 +113,7 @@ class CarController(object): ### GAS/BRAKE ### - if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): # no output if not enabled, but keep sending keepalive messages # treat pedals as one final_pedal = actuators.gas - actuators.brake diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 94aa09c95..5b665a741 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -31,11 +31,11 @@ def get_powertrain_can_parser(CP, canbus): ("LKATorqueDeliveredStatus", "PSCMStatus", 0), ] - if CP.carFingerprint in (CAR.VOLT, CAR.MALIBU): + if CP.carFingerprint == CAR.VOLT: signals += [ ("RegenPaddle", "EBCMRegenPaddle", 0), ] - if CP.carFingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if CP.carFingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): signals += [ ("TractionControlOn", "ESPStatus", 0), ("EPBClosed", "EPBStatus", 0), @@ -120,13 +120,13 @@ class CarState(object): self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 - if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'] self.acc_active = False self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] - if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU): + if self.car_fingerprint == CAR.VOLT: self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) else: self.regen_pressed = False diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index f421f73dc..9ec1b9e5d 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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 @@ -97,6 +98,24 @@ class CarInterface(object): ret.steerRatio = 15.7 ret.steerRatioRear = 0. + elif candidate == CAR.ACADIA: + ret.minEnableSpeed = -1 # engage speed is decided by pcm + ret.mass = 4353. * CV.LB_TO_KG + std_cargo + ret.safetyModel = car.CarParams.SafetyModels.gm + ret.wheelbase = 2.86 + ret.steerRatio = 14.4 #end to end is 13.46 + ret.steerRatioRear = 0. + ret.centerToFront = ret.wheelbase * 0.4 + + elif candidate == CAR.CADILLAC_ATS: + ret.minEnableSpeed = 18 * CV.MPH_TO_MS + ret.mass = 1601 + std_cargo + ret.safetyModel = car.CarParams.SafetyModels.gm + ret.wheelbase = 2.78 + ret.steerRatio = 15.3 + ret.steerRatioRear = 0. + ret.centerToFront = ret.wheelbase * 0.49 + elif candidate == CAR.CADILLAC_CT6: # engage speed is decided by pcm ret.minEnableSpeed = -1 @@ -266,7 +285,7 @@ class CarInterface(object): if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if self.CS.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not self.CS.gear_shifter_valid: diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 56c2c7ca4..6af0d3f8f 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -22,7 +22,7 @@ LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS def create_radard_can_parser(canbus, car_fingerprint): dbc_f = DBC[car_fingerprint]['radar'] - if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): # C1A-ARS3-A by Continental radar_targets = range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS) signals = zip(['FLRRNumValidTargets', diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 021c02ebe..17260fe27 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -6,8 +6,10 @@ AudibleAlert = car.CarControl.HUDControl.AudibleAlert class CAR: HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" VOLT = "CHEVROLET VOLT PREMIER 2017" + CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018" MALIBU = "CHEVROLET MALIBU PREMIER 2017" + ACADIA = "GMC ACADIA DENALI 2018" class CruiseButtons: UNPRESS = 1 @@ -37,7 +39,7 @@ AUDIO_HUD = { def is_eps_status_ok(eps_status, car_fingerprint): valid_eps_status = [] - if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA): + if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): valid_eps_status += [0, 1] elif car_fingerprint == CAR.CADILLAC_CT6: valid_eps_status += [0, 1, 4, 5, 6] @@ -69,6 +71,11 @@ FINGERPRINTS = { { 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 }], + CAR.CADILLAC_ATS: [ + # Cadillac ATS Coupe Premium Performance 3.6L RWD w/ ACC 2018 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 + }], CAR.CADILLAC_CT6: [{ 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 336: 1, 338: 6, 340: 6, 352: 5, 354: 5, 356: 8, 368: 3, 372: 5, 381: 8, 386: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 458: 5, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 800: 6, 801: 8, 804: 3, 810: 8, 832: 8, 833: 8, 834: 8, 835: 6, 836: 5, 837: 8, 838: 8, 839: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 884: 8, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1934: 7, 2016: 8, 2024: 8 }], @@ -77,6 +84,11 @@ FINGERPRINTS = { { 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8, }], + CAR.ACADIA: [ + # Acadia Denali w/ /ACC 2018 + { + 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 + }], } STEER_THRESHOLD = 1.0 @@ -85,12 +97,16 @@ STOCK_CONTROL_MSGS = { CAR.HOLDEN_ASTRA: [384, 715], CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" CAR.MALIBU: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" - CAR.CADILLAC_CT6: [], # Cadillac does not require ASCMs to be disconnected + CAR.ACADIA: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" + CAR.CADILLAC_ATS: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" + CAR.CADILLAC_CT6: [], # CT6 does not require ASCMs to be disconnected } DBC = { CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'), } diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f46343c18..99d4a2214 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 @@ 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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e074900e5..e6c4ed4de 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 96174ef09..c525c0185 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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 diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 5edbcbbd1..b7d60a425 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 57e90339c..4f3b53236 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index d4929e7bc..ac67fbc66 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -108,6 +108,10 @@ FINGERPRINTS = { }], CAR.LEXUS_RXH: [{ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8 + }, + # RX450HL + { + 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.CHR: [{ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 740: 5, 800: 8, 810: 2, 812: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8 diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 617c8189c..422faa02b 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 5a4b7ef4d..0a96c2335 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.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 diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index 658ee1b85..36b6f8b7e 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 33cb4f56f..b98674011 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -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])] 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 36d76aa59..6c395efed 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -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()) diff --git a/selfdrive/loggerd/loggerd b/selfdrive/loggerd/loggerd index 881b9e84f..ce1e7ecd2 100755 Binary files a/selfdrive/loggerd/loggerd and b/selfdrive/loggerd/loggerd differ diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 6b64a4af7..90753644d 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 @@ 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: diff --git a/selfdrive/mapd/__init__.py b/selfdrive/mapd/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/mapd/mapd.py b/selfdrive/mapd/mapd.py new file mode 100755 index 000000000..e1a9595e8 --- /dev/null +++ b/selfdrive/mapd/mapd.py @@ -0,0 +1,276 @@ +#!/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 os +import sys +import time +import zmq +import threading +import numpy as np +import overpy +from collections import defaultdict + +from common.params import Params +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 +import selfdrive.crash as crash +from selfdrive.version import version, dirty + + +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 setup_thread_excepthook(): + """ + Workaround for `sys.excepthook` thread bug from: + http://bugs.python.org/issue1230540 + Call once from the main thread before creating any threads. + Source: https://stackoverflow.com/a/31622038 + """ + init_original = threading.Thread.__init__ + + def init(self, *args, **kwargs): + init_original(self, *args, **kwargs) + run_original = self.run + + def run_with_except_hook(*args2, **kwargs2): + try: + run_original(*args2, **kwargs2) + except Exception: + sys.excepthook(*sys.exc_info()) + + self.run = run_with_except_hook + + threading.Thread.__init__ = init + + +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][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; + >;);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: + time.sleep(1) + 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() + + +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, cur_way) + 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 and pnts.shape[0] > 3: + # 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): + params = Params() + dongle_id = params.get("DongleId") + crash.bind_user(id=dongle_id) + crash.bind_extra(version=version, dirty=dirty, is_eon=True) + crash.install() + + setup_thread_excepthook() + 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 000000000..6d84fbd7d --- /dev/null +++ b/selfdrive/mapd/mapd_helpers.py @@ -0,0 +1,229 @@ +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, prev_way=None): + 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) + + # Prefer same type of road + if prev_way is not None: + if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''): + score *= 0.5 + + 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 9c48a5f6a..d38869ce2 100755 Binary files a/selfdrive/sensord/gpsd and b/selfdrive/sensord/gpsd differ diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord index 84137a9b8..84194bb06 100755 Binary files a/selfdrive/sensord/sensord and b/selfdrive/sensord/sensord differ diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 4238273d9..7f39d310d 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -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)) diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index ce57dd465..dddf15f7e 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); - - 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 diff --git a/selfdrive/visiond/visiond b/selfdrive/visiond/visiond index 40259afc5..caf5de38a 100755 Binary files a/selfdrive/visiond/visiond and b/selfdrive/visiond/visiond differ