openpilot v0.5.9 release

pull/533/head
Vehicle Researcher 6 years ago
parent fdb04d9f69
commit 0207a97040
  1. 10
      README.md
  2. 13
      RELEASES.md
  3. BIN
      apk/ai.comma.plus.frame.apk
  4. BIN
      apk/ai.comma.plus.offroad.apk
  5. 49
      cereal/log.capnp
  6. 1
      common/params.py
  7. 10
      common/transformations/camera.py
  8. BIN
      models/posenet.dlc
  9. 4
      requirements_openpilot.txt
  10. 4
      selfdrive/car/chrysler/interface.py
  11. 4
      selfdrive/car/ford/interface.py
  12. 50
      selfdrive/car/gm/carcontroller.py
  13. 28
      selfdrive/car/gm/carstate.py
  14. 30
      selfdrive/car/gm/interface.py
  15. 16
      selfdrive/car/gm/values.py
  16. 4
      selfdrive/car/honda/carcontroller.py
  17. 16
      selfdrive/car/honda/carstate.py
  18. 32
      selfdrive/car/honda/interface.py
  19. 2
      selfdrive/car/hyundai/carcontroller.py
  20. 12
      selfdrive/car/hyundai/carstate.py
  21. 18
      selfdrive/car/hyundai/interface.py
  22. 12
      selfdrive/car/hyundai/values.py
  23. 4
      selfdrive/car/mock/interface.py
  24. 18
      selfdrive/car/toyota/carcontroller.py
  25. 10
      selfdrive/car/toyota/carstate.py
  26. 52
      selfdrive/car/toyota/interface.py
  27. 12
      selfdrive/car/toyota/toyotacan.py
  28. 15
      selfdrive/car/toyota/values.py
  29. 2
      selfdrive/common/version.h
  30. 159
      selfdrive/controls/controlsd.py
  31. 82
      selfdrive/controls/lib/latcontrol.py
  32. 2
      selfdrive/controls/lib/longcontrol.py
  33. 65
      selfdrive/controls/lib/model_parser.py
  34. 180
      selfdrive/controls/lib/pathplanner.py
  35. 336
      selfdrive/controls/lib/planner.py
  36. 69
      selfdrive/controls/plannerd.py
  37. 24
      selfdrive/controls/radard.py
  38. 14
      selfdrive/debug/dump.py
  39. 218
      selfdrive/locationd/calibrationd.py
  40. 2
      selfdrive/locationd/ublox.py
  41. 1
      selfdrive/locationd/ubloxd.py
  42. BIN
      selfdrive/loggerd/loggerd
  43. 2
      selfdrive/loggerd/uploader.py
  44. 5
      selfdrive/manager.py
  45. 105
      selfdrive/mapd/default_speeds.json
  46. 454
      selfdrive/mapd/default_speeds_by_region.json
  47. 209
      selfdrive/mapd/default_speeds_generator.py
  48. 33
      selfdrive/mapd/mapd.py
  49. 168
      selfdrive/mapd/mapd_helpers.py
  50. BIN
      selfdrive/sensord/gpsd
  51. BIN
      selfdrive/sensord/sensord
  52. 3
      selfdrive/service_list.yaml
  53. 4
      selfdrive/test/tests/plant/test_longitudinal.py
  54. 4
      selfdrive/visiond/build_from_src.mk
  55. 114
      selfdrive/visiond/visiond.cc

@ -62,6 +62,7 @@ Supported Cars
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------| | ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
| Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec | | Acura | ILX 2016-17 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec |
| Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec | | Acura | RDX 2018 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Buick<sup>3</sup> | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>| | Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>| | Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>| | Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>7</sup>|
@ -81,19 +82,20 @@ Supported Cars
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>| | Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>| | Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>| | Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>| | Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>| | Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Lexus | RX Hybrid 2016-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota | | Lexus | RX Hybrid 2016-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota | | Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | C-HR 2017-18<sup>4</sup> | All | Yes | Stock | 0mph | 0mph | Toyota | | Toyota | C-HR 2017-18<sup>4</sup> | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota | | Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Highlander 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota | | Toyota | Highlander 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Highlander Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota | | Toyota | Highlander Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota | | Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota | | Toyota | Prius 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Prius Prime 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota | | Toyota | Prius Prime 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota | | Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota | | Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota | | Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
<sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)*** <sup>1</sup>[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma.ai](https://comma.ai)***
@ -166,7 +168,7 @@ Directory structure
├── sensord # IMU / GPS interface code ├── sensord # IMU / GPS interface code
├── test # Car simulator running code through virtual maneuvers ├── test # Car simulator running code through virtual maneuvers
├── ui # The UI ├── ui # The UI
└── visiond # Embedded vision pipeline └── visiond # Vision pipeline
To understand how the services interact, see `selfdrive/service_list.yaml` To understand how the services interact, see `selfdrive/service_list.yaml`

@ -1,10 +1,21 @@
Version 0.5.9 (2019-02-10)
========================
* Improve calibration using a dedicated neural network
* Abstract planner in its own process to remove lags in controls process
* Improve speed limits with country/region defaults by road type
* Reduce mapd data usage with gzip thanks to eFiniLan
* Zip log files in the background to reduce disk usage
* Kia Optima support thanks to emmertex!
* Buick Regal 2018 support thanks to HOYS!
* Comma pedal support for Toyota thanks to wocsor! Note: tuning needed and not maintained by comma
Version 0.5.8 (2019-01-17) Version 0.5.8 (2019-01-17)
======================== ========================
* Open sourced visiond * Open sourced visiond
* Auto-slowdown for upcoming turns * Auto-slowdown for upcoming turns
* Chrysler/Jeep/Fiat support thanks to adhintz! * Chrysler/Jeep/Fiat support thanks to adhintz!
* Honda Civic 2019 support thanks to csouers! * Honda Civic 2019 support thanks to csouers!
* Improved use of car display in Toyota thanks to arne182! * Improve use of car display in Toyota thanks to arne182!
* No data upload when connected to Android or iOS hotspots and "Enable Upload Over Cellular" setting is off * No data upload when connected to Android or iOS hotspots and "Enable Upload Over Cellular" setting is off
* EON stops charging when 12V battery drops below 11.8V * EON stops charging when 12V battery drops below 11.8V

Binary file not shown.

Binary file not shown.

@ -374,6 +374,7 @@ struct Live100Data {
l20MonoTimeDEPRECATED @17 :UInt64; l20MonoTimeDEPRECATED @17 :UInt64;
mdMonoTimeDEPRECATED @18 :UInt64; mdMonoTimeDEPRECATED @18 :UInt64;
planMonoTime @28 :UInt64; planMonoTime @28 :UInt64;
pathPlanMonoTime @50 :UInt64;
state @31 :ControlState; state @31 :ControlState;
vEgo @0 :Float32; vEgo @0 :Float32;
@ -401,6 +402,7 @@ struct Live100Data {
cumLagMs @15 :Float32; cumLagMs @15 :Float32;
startMonoTime @48 :UInt64; startMonoTime @48 :UInt64;
mapValid @49 :Bool; mapValid @49 :Bool;
forceDecel @51 :Bool;
enabled @19 :Bool; enabled @19 :Bool;
active @36 :Bool; active @36 :Bool;
@ -546,12 +548,12 @@ struct Plan {
events @13 :List(Car.CarEvent); events @13 :List(Car.CarEvent);
# lateral, 3rd order polynomial # lateral, 3rd order polynomial
lateralValid @0 :Bool; lateralValidDEPRECATED @0 :Bool;
dPoly @1 :List(Float32); dPolyDEPRECATED @1 :List(Float32);
laneWidth @11 :Float32; laneWidthDEPRECATED @11 :Float32;
# longitudinal # longitudinal
longitudinalValid @2 :Bool; longitudinalValidDEPRECATED @2 :Bool;
vCruise @16 :Float32; vCruise @16 :Float32;
aCruise @17 :Float32; aCruise @17 :Float32;
vTarget @3 :Float32; vTarget @3 :Float32;
@ -560,10 +562,14 @@ struct Plan {
aTargetMinDEPRECATED @4 :Float32; aTargetMinDEPRECATED @4 :Float32;
aTargetMaxDEPRECATED @5 :Float32; aTargetMaxDEPRECATED @5 :Float32;
aTarget @18 :Float32; aTarget @18 :Float32;
vStart @26 :Float32;
aStart @27 :Float32;
jerkFactor @6 :Float32; jerkFactor @6 :Float32;
hasLead @7 :Bool; hasLead @7 :Bool;
hasLeftLane @23 :Bool; hasLeftLaneDEPRECATED @23 :Bool;
hasRightLane @24 :Bool; hasRightLaneDEPRECATED @24 :Bool;
fcw @8 :Bool; fcw @8 :Bool;
longitudinalPlanSource @15 :LongitudinalPlanSource; longitudinalPlanSource @15 :LongitudinalPlanSource;
@ -577,6 +583,7 @@ struct Plan {
decelForTurn @22 :Bool; decelForTurn @22 :Bool;
mapValid @25 :Bool; mapValid @25 :Bool;
struct GpsTrajectory { struct GpsTrajectory {
x @0 :List(Float32); x @0 :List(Float32);
y @1 :List(Float32); y @1 :List(Float32);
@ -590,6 +597,21 @@ struct Plan {
} }
} }
struct PathPlan {
laneWidth @0 :Float32;
dPoly @1 :List(Float32);
cPoly @2 :List(Float32);
cProb @3 :Float32;
lPoly @4 :List(Float32);
lProb @5 :Float32;
rPoly @6 :List(Float32);
rProb @7 :Float32;
angleSteers @8 :Float32;
valid @9 :Bool;
}
struct LiveLocationData { struct LiveLocationData {
status @0 :UInt8; status @0 :UInt8;
@ -1276,6 +1298,7 @@ struct UbloxGnss {
carrierPhaseStdev @10 :Float32; carrierPhaseStdev @10 :Float32;
# doppler standard deviation in Hz # doppler standard deviation in Hz
dopplerStdev @11 :Float32; dopplerStdev @11 :Float32;
sigId @12 :UInt8;
struct TrackingStatus { struct TrackingStatus {
# pseudorange valid # pseudorange valid
@ -1584,6 +1607,11 @@ struct LiveParametersData {
struct LiveMapData { struct LiveMapData {
speedLimitValid @0 :Bool; speedLimitValid @0 :Bool;
speedLimit @1 :Float32; speedLimit @1 :Float32;
speedAdvisoryValid @12 :Bool;
speedAdvisory @13 :Float32;
speedLimitAheadValid @14 :Bool;
speedLimitAhead @15 :Float32;
speedLimitAheadDistance @16 :Float32;
curvatureValid @2 :Bool; curvatureValid @2 :Bool;
curvature @3 :Float32; curvature @3 :Float32;
wayId @4 :UInt64; wayId @4 :UInt64;
@ -1603,6 +1631,13 @@ struct CameraOdometry {
rotStd @3 :List(Float32); # std rad/s in device frame rotStd @3 :List(Float32); # std rad/s in device frame
} }
struct KalmanOdometry {
trans @0 :List(Float32); # m/s in device frame
rot @1 :List(Float32); # rad/s in device frame
transStd @2 :List(Float32); # std m/s in device frame
rotStd @3 :List(Float32); # std rad/s in device frame
}
struct Event { struct Event {
# in nanoseconds? # in nanoseconds?
logMonoTime @0 :UInt64; logMonoTime @0 :UInt64;
@ -1671,5 +1706,7 @@ struct Event {
liveParameters @61 :LiveParametersData; liveParameters @61 :LiveParametersData;
liveMapData @62 :LiveMapData; liveMapData @62 :LiveMapData;
cameraOdometry @63 :CameraOdometry; cameraOdometry @63 :CameraOdometry;
pathPlan @64 :PathPlan;
kalmanOdometry @65 :KalmanOdometry;
} }
} }

@ -67,6 +67,7 @@ keys = {
# written: visiond # written: visiond
# read: visiond, controlsd # read: visiond, controlsd
"CalibrationParams": TxType.PERSISTENT, "CalibrationParams": TxType.PERSISTENT,
"ControlsParams": TxType.PERSISTENT,
# written: controlsd # written: controlsd
# read: radard # read: radard
"CarParams": TxType.CLEAR_ON_CAR_START, "CarParams": TxType.CLEAR_ON_CAR_START,

@ -29,21 +29,13 @@ view_frame_from_device_frame = device_frame_from_view_frame.T
def get_calib_from_vp(vp): def get_calib_from_vp(vp):
vp_norm = normalize(vp) vp_norm = normalize(vp)
yaw_calib = np.arctan(vp_norm[0]) yaw_calib = np.arctan(vp_norm[0])
pitch_calib = np.arctan(vp_norm[1]*np.cos(yaw_calib)) pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
# TODO should be, this but written
# to be compatible with meshcalib and
# get_view_frame_from_road_fram
#pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
roll_calib = 0 roll_calib = 0
return roll_calib, pitch_calib, yaw_calib return roll_calib, pitch_calib, yaw_calib
# aka 'extrinsic_matrix' # aka 'extrinsic_matrix'
# road : x->forward, y -> left, z->up # road : x->forward, y -> left, z->up
def get_view_frame_from_road_frame(roll, pitch, yaw, height): def get_view_frame_from_road_frame(roll, pitch, yaw, height):
# TODO
# calibration pitch is currently defined
# opposite to pitch in device frame
pitch = -pitch
device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
view_from_road = view_frame_from_device_frame.dot(device_from_road) view_from_road = view_frame_from_device_frame.dot(device_from_road)
return np.hstack((view_from_road, [[0], [height], [0]])) return np.hstack((view_from_road, [[0], [height], [0]]))

Binary file not shown.

@ -15,7 +15,7 @@ cffi==1.11.5
contextlib2==0.5.4 contextlib2==0.5.4
crc16==0.1.1 crc16==0.1.1
crcmod==1.7 crcmod==1.7
cryptography==1.4 cryptography==2.1.4
cycler==0.10.0 cycler==0.10.0
decorator==4.0.10 decorator==4.0.10
docopt==0.6.2 docopt==0.6.2
@ -31,7 +31,7 @@ nose==1.3.7
numpy==1.11.1 numpy==1.11.1
pause==0.1.2 pause==0.1.2
py==1.4.31 py==1.4.31
pyOpenSSL==16.0.0 pyOpenSSL==17.5.0
pyasn1-modules==0.0.8 pyasn1-modules==0.0.8
pyasn1==0.1.9 pyasn1==0.1.9
pycapnp==0.6.3 pycapnp==0.6.3

@ -1,6 +1,6 @@
#!/usr/bin/env python #!/usr/bin/env python
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from cereal import car, log from cereal import car
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -256,7 +256,7 @@ class CarInterface(object):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c, perception_state=log.Live20Data.new_message()): def apply(self, c):
if (self.CS.frame == -1): if (self.CS.frame == -1):
return False # if we haven't seen a frame 220, then do not update. return False # if we haven't seen a frame 220, then do not update.

@ -1,6 +1,6 @@
#!/usr/bin/env python #!/usr/bin/env python
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from cereal import car, log from cereal import car
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
@ -210,7 +210,7 @@ class CarInterface(object):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c, perception_state=log.Live20Data.new_message()): def apply(self, c):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators, self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.visualAlert, c.cruiseControl.cancel) c.hudControl.visualAlert, c.cruiseControl.cancel)

@ -4,22 +4,24 @@ from selfdrive.config import Conversions as CV
from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.gm import gmcan from selfdrive.car.gm import gmcan
from selfdrive.car.gm.values import CAR, DBC from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS
from selfdrive.can.packer import CANPacker from selfdrive.can.packer import CANPacker
class CarControllerParams(): class CarControllerParams():
def __init__(self, car_fingerprint): def __init__(self, car_fingerprint):
if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if car_fingerprint in SUPERCRUISE_CARS:
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)
self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero
elif car_fingerprint == CAR.CADILLAC_CT6:
self.STEER_MAX = 150 self.STEER_MAX = 150
self.STEER_STEP = 1 # how often we update the steer cmd self.STEER_STEP = 1 # how often we update the steer cmd
self.STEER_DELTA_UP = 2 # 0.75s time to peak torque self.STEER_DELTA_UP = 2 # 0.75s time to peak torque
self.STEER_DELTA_DOWN = 5 # 0.3s from peak torque to zero self.STEER_DELTA_DOWN = 5 # 0.3s from peak torque to zero
self.MIN_STEER_SPEED = -1. # can steer down to zero
else:
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)
self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero
self.MIN_STEER_SPEED = 3.
self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily
@ -94,7 +96,7 @@ class CarController(object):
### STEER ### ### STEER ###
if (frame % P.STEER_STEP) == 0: if (frame % P.STEER_STEP) == 0:
lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > 3. lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED
if lkas_enabled: if lkas_enabled:
apply_steer = actuators.steer * P.STEER_MAX apply_steer = actuators.steer * P.STEER_MAX
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
@ -104,16 +106,16 @@ class CarController(object):
self.apply_steer_last = apply_steer self.apply_steer_last = apply_steer
idx = (frame / P.STEER_STEP) % 4 idx = (frame / P.STEER_STEP) % 4
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if self.car_fingerprint in SUPERCRUISE_CARS:
can_sends.append(gmcan.create_steering_control(self.packer_pt,
canbus.powertrain, apply_steer, idx, lkas_enabled))
if self.car_fingerprint == CAR.CADILLAC_CT6:
can_sends += gmcan.create_steering_control_ct6(self.packer_pt, can_sends += gmcan.create_steering_control_ct6(self.packer_pt,
canbus, apply_steer, CS.v_ego, idx, lkas_enabled) canbus, apply_steer, CS.v_ego, idx, lkas_enabled)
else:
can_sends.append(gmcan.create_steering_control(self.packer_pt,
canbus.powertrain, apply_steer, idx, lkas_enabled))
### GAS/BRAKE ### ### GAS/BRAKE ###
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if self.car_fingerprint not in SUPERCRUISE_CARS:
# no output if not enabled, but keep sending keepalive messages # no output if not enabled, but keep sending keepalive messages
# treat pedals as one # treat pedals as one
final_pedal = actuators.gas - actuators.brake final_pedal = actuators.gas - actuators.brake
@ -164,17 +166,17 @@ class CarController(object):
if frame % P.ADAS_KEEPALIVE_STEP == 0: if frame % P.ADAS_KEEPALIVE_STEP == 0:
can_sends += gmcan.create_adas_keepalive(canbus.powertrain) can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
# Show green icon when LKA torque is applied, and # Show green icon when LKA torque is applied, and
# alarming orange icon when approaching torque limit. # alarming orange icon when approaching torque limit.
# If not sent again, LKA icon disappears in about 5 seconds. # If not sent again, LKA icon disappears in about 5 seconds.
# Conveniently, sending camera message periodically also works as a keepalive. # Conveniently, sending camera message periodically also works as a keepalive.
lka_active = CS.lkas_status == 1 lka_active = CS.lkas_status == 1
lka_critical = lka_active and abs(actuators.steer) > 0.9 lka_critical = lka_active and abs(actuators.steer) > 0.9
lka_icon_status = (lka_active, lka_critical) lka_icon_status = (lka_active, lka_critical)
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
or lka_icon_status != self.lka_icon_status_last: or lka_icon_status != self.lka_icon_status_last:
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical)) can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
self.lka_icon_status_last = lka_icon_status self.lka_icon_status_last = lka_icon_status
# Send chimes # Send chimes
if self.chime != chime: if self.chime != chime:

@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.can.parser import CANParser from selfdrive.can.parser import CANParser
from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \ from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \
CruiseButtons, is_eps_status_ok, \ CruiseButtons, is_eps_status_ok, \
STEER_THRESHOLD STEER_THRESHOLD, SUPERCRUISE_CARS
def get_powertrain_can_parser(CP, canbus): def get_powertrain_can_parser(CP, canbus):
# this function generates lists for signal, messages and initial values # this function generates lists for signal, messages and initial values
@ -35,17 +35,17 @@ def get_powertrain_can_parser(CP, canbus):
signals += [ signals += [
("RegenPaddle", "EBCMRegenPaddle", 0), ("RegenPaddle", "EBCMRegenPaddle", 0),
] ]
if CP.carFingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if CP.carFingerprint in SUPERCRUISE_CARS:
signals += [
("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0)
]
else:
signals += [ signals += [
("TractionControlOn", "ESPStatus", 0), ("TractionControlOn", "ESPStatus", 0),
("EPBClosed", "EPBStatus", 0), ("EPBClosed", "EPBStatus", 0),
("CruiseMainOn", "ECMEngineStatus", 0), ("CruiseMainOn", "ECMEngineStatus", 0),
("CruiseState", "AcceleratorPedal2", 0), ("CruiseState", "AcceleratorPedal2", 0),
] ]
if CP.carFingerprint == CAR.CADILLAC_CT6:
signals += [
("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0)
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
@ -121,7 +121,14 @@ class CarState(object):
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False
self.main_on = False
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
self.esp_disabled = False
self.regen_pressed = False
self.pcm_acc_status = int(self.acc_active)
else:
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'] self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
self.acc_active = False self.acc_active = False
@ -131,13 +138,6 @@ class CarState(object):
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else: else:
self.regen_pressed = False self.regen_pressed = False
if self.car_fingerprint == CAR.CADILLAC_CT6:
self.park_brake = False
self.main_on = False
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
self.esp_disabled = False
self.regen_pressed = False
self.pcm_acc_status = int(self.acc_active)
# Brake pedal's potentiometer returns near-zero reading # Brake pedal's potentiometer returns near-zero reading
# even when pedal is not pressed. # even when pedal is not pressed.

@ -1,10 +1,10 @@
#!/usr/bin/env python #!/usr/bin/env python
from cereal import car, log from cereal import car
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, SUPERCRUISE_CARS
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
try: try:
@ -107,6 +107,15 @@ class CarInterface(object):
ret.steerRatioRear = 0. ret.steerRatioRear = 0.
ret.centerToFront = ret.wheelbase * 0.4 ret.centerToFront = ret.wheelbase * 0.4
elif candidate == CAR.BUICK_REGAL:
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.mass = 3779. * CV.LB_TO_KG + std_cargo # (3849+3708)/2
ret.safetyModel = car.CarParams.SafetyModels.gm
ret.wheelbase = 2.83 #111.4 inches in meters
ret.steerRatio = 14.4 # guess for tourx
ret.steerRatioRear = 0.
ret.centerToFront = ret.wheelbase * 0.4 # guess for tourx
elif candidate == CAR.CADILLAC_ATS: elif candidate == CAR.CADILLAC_ATS:
ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.mass = 1601 + std_cargo ret.mass = 1601 + std_cargo
@ -285,7 +294,13 @@ class CarInterface(object):
if ret.seatbeltUnlatched: if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if self.CS.car_fingerprint in SUPERCRUISE_CARS:
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
if self.CS.brake_error: if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid: if not self.CS.gear_shifter_valid:
@ -318,13 +333,6 @@ class CarInterface(object):
if b.type == "cancel" and b.pressed: if b.type == "cancel" and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE])) events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
if self.CS.car_fingerprint == CAR.CADILLAC_CT6:
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
ret.events = events ret.events = events
# update previous brake/gas pressed # update previous brake/gas pressed
@ -337,7 +345,7 @@ class CarInterface(object):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c, perception_state=log.Live20Data.new_message()): def apply(self, c):
hud_v_cruise = c.hudControl.setSpeed hud_v_cruise = c.hudControl.setSpeed
if hud_v_cruise > 70: if hud_v_cruise > 70:
hud_v_cruise = 0 hud_v_cruise = 0

@ -10,6 +10,9 @@ class CAR:
CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018" CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018"
MALIBU = "CHEVROLET MALIBU PREMIER 2017" MALIBU = "CHEVROLET MALIBU PREMIER 2017"
ACADIA = "GMC ACADIA DENALI 2018" ACADIA = "GMC ACADIA DENALI 2018"
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
SUPERCRUISE_CARS = [CAR.CADILLAC_CT6]
class CruiseButtons: class CruiseButtons:
UNPRESS = 1 UNPRESS = 1
@ -39,10 +42,10 @@ AUDIO_HUD = {
def is_eps_status_ok(eps_status, car_fingerprint): def is_eps_status_ok(eps_status, car_fingerprint):
valid_eps_status = [] valid_eps_status = []
if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): if car_fingerprint in SUPERCRUISE_CARS:
valid_eps_status += [0, 1]
elif car_fingerprint == CAR.CADILLAC_CT6:
valid_eps_status += [0, 1, 4, 5, 6] valid_eps_status += [0, 1, 4, 5, 6]
else:
valid_eps_status += [0, 1]
return eps_status in valid_eps_status return eps_status in valid_eps_status
def parse_gear_shifter(can_gear): def parse_gear_shifter(can_gear):
@ -71,6 +74,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 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.BUICK_REGAL : [
# Regal TourX Essence w/ ACC 2018
{
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8
}],
CAR.CADILLAC_ATS: [ CAR.CADILLAC_ATS: [
# Cadillac ATS Coupe Premium Performance 3.6L RWD w/ ACC 2018 # Cadillac ATS Coupe Premium Performance 3.6L RWD w/ ACC 2018
{ {
@ -99,6 +107,7 @@ STOCK_CONTROL_MSGS = {
CAR.MALIBU: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" CAR.MALIBU: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
CAR.ACADIA: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" CAR.ACADIA: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
CAR.CADILLAC_ATS: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" CAR.CADILLAC_ATS: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
CAR.BUICK_REGAL: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
CAR.CADILLAC_CT6: [], # CT6 does not require ASCMs to be disconnected CAR.CADILLAC_CT6: [], # CT6 does not require ASCMs to be disconnected
} }
@ -108,5 +117,6 @@ DBC = {
CAR.MALIBU: 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.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_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
CAR.BUICK_REGAL: 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'), CAR.CADILLAC_CT6: dbc_dict('cadillac_ct6_powertrain', 'cadillac_ct6_object', chassis_dbc='cadillac_ct6_chassis'),
} }

@ -86,7 +86,7 @@ class CarController(object):
def update(self, sendcan, enabled, CS, frame, actuators, \ def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime): hud_alert, snd_beep, snd_chime):
""" Controls thread """ """ Controls thread """
@ -121,7 +121,7 @@ class CarController(object):
# For lateral control-only, send chimes as a beep since we don't send 0x1fa # For lateral control-only, send chimes as a beep since we don't send 0x1fa
if CS.CP.radarOffCan: if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep is not 0 else snd_chime snd_beep = snd_beep if snd_beep != 0 else snd_chime
#print chime, alert_id, hud_alert #print chime, alert_id, hud_alert
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)

@ -48,6 +48,7 @@ def get_can_signals(CP):
("ESP_DISABLED", "VSA_STATUS", 1), ("ESP_DISABLED", "VSA_STATUS", 1),
("HUD_LEAD", "ACC_HUD", 0), ("HUD_LEAD", "ACC_HUD", 0),
("USER_BRAKE", "VSA_STATUS", 0), ("USER_BRAKE", "VSA_STATUS", 0),
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
("STEER_STATUS", "STEER_STATUS", 5), ("STEER_STATUS", "STEER_STATUS", 5),
("GEAR_SHIFTER", "GEARBOX", 0), ("GEAR_SHIFTER", "GEARBOX", 0),
("PEDAL_GAS", "POWERTRAIN_DATA", 0), ("PEDAL_GAS", "POWERTRAIN_DATA", 0),
@ -76,7 +77,6 @@ def get_can_signals(CP):
signals += [("CAR_GAS", "GAS_PEDAL_2", 0), signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
("MAIN_ON", "SCM_FEEDBACK", 0), ("MAIN_ON", "SCM_FEEDBACK", 0),
("EPB_STATE", "EPB_STATUS", 0), ("EPB_STATE", "EPB_STATUS", 0),
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
("CRUISE_SPEED", "ACC_HUD", 0)] ("CRUISE_SPEED", "ACC_HUD", 0)]
checks += [("GAS_PEDAL_2", 100)] checks += [("GAS_PEDAL_2", 100)]
else: else:
@ -101,8 +101,7 @@ def get_can_signals(CP):
if CP.carFingerprint == CAR.CIVIC: if CP.carFingerprint == CAR.CIVIC:
signals += [("CAR_GAS", "GAS_PEDAL_2", 0), signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
("MAIN_ON", "SCM_FEEDBACK", 0), ("MAIN_ON", "SCM_FEEDBACK", 0),
("EPB_STATE", "EPB_STATUS", 0), ("EPB_STATE", "EPB_STATUS", 0)]
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)]
elif CP.carFingerprint == CAR.ACURA_ILX: elif CP.carFingerprint == CAR.ACURA_ILX:
signals += [("CAR_GAS", "GAS_PEDAL_2", 0), signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
("MAIN_ON", "SCM_BUTTONS", 0)] ("MAIN_ON", "SCM_BUTTONS", 0)]
@ -110,8 +109,7 @@ def get_can_signals(CP):
signals += [("MAIN_ON", "SCM_BUTTONS", 0)] signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
elif CP.carFingerprint == CAR.ODYSSEY: elif CP.carFingerprint == CAR.ODYSSEY:
signals += [("MAIN_ON", "SCM_FEEDBACK", 0), signals += [("MAIN_ON", "SCM_FEEDBACK", 0),
("EPB_STATE", "EPB_STATUS", 0), ("EPB_STATE", "EPB_STATUS", 0)]
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)]
checks += [("EPB_STATUS", 50)] checks += [("EPB_STATUS", 50)]
elif CP.carFingerprint == CAR.PILOT: elif CP.carFingerprint == CAR.PILOT:
signals += [("MAIN_ON", "SCM_BUTTONS", 0), signals += [("MAIN_ON", "SCM_BUTTONS", 0),
@ -248,14 +246,13 @@ class CarState(object):
self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER']
self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH): if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH):
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
else: else:
self.park_brake = 0 # TODO self.park_brake = 0 # TODO
self.brake_hold = 0 # TODO
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
@ -303,8 +300,9 @@ class CarState(object):
self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD'] self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
# gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
# TODO: this should be ok for all cars. Verify it.
if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE): if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
if self.user_brake > 0.05: if self.user_brake > 0.05:
self.brake_pressed = 1 self.brake_pressed = 1

@ -1,7 +1,7 @@
#!/usr/bin/env python #!/usr/bin/env python
import os import os
import numpy as np import numpy as np
from cereal import car, log from cereal import car
from common.numpy_fast import clip, interp from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
@ -575,7 +575,7 @@ class CarInterface(object):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c, perception_state=log.Live20Data.new_message()): def apply(self, c):
if c.hudControl.speedVisible: if c.hudControl.speedVisible:
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
else: else:
@ -584,19 +584,19 @@ class CarInterface(object):
hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw] hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw]
snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw] snd_beep, snd_chime = AUDIO_HUD[c.hudControl.audibleAlert.raw]
pcm_accel = int(clip(c.cruiseControl.accelOverride,0,1)*0xc6) pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \ self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators, \ c.actuators,
c.cruiseControl.speedOverride, \ c.cruiseControl.speedOverride,
c.cruiseControl.override, \ c.cruiseControl.override,
c.cruiseControl.cancel, \ c.cruiseControl.cancel,
pcm_accel, \ pcm_accel,
perception_state.radarErrors, \ hud_v_cruise,
hud_v_cruise, c.hudControl.lanesVisible, \ c.hudControl.lanesVisible,
hud_show_car = c.hudControl.leadVisible, \ hud_show_car=c.hudControl.leadVisible,
hud_alert = hud_alert, \ hud_alert=hud_alert,
snd_beep = snd_beep, \ snd_beep=snd_beep,
snd_chime = snd_chime) snd_chime=snd_chime)
self.frame += 1 self.frame += 1

@ -10,7 +10,7 @@ from selfdrive.can.packer import CANPacker
# Steer torque limits # Steer torque limits
class SteerLimitParams: class SteerLimitParams:
STEER_MAX = 250 # 409 is the max STEER_MAX = 255 # 409 is the max, 255 is stock
STEER_DELTA_UP = 3 STEER_DELTA_UP = 3
STEER_DELTA_DOWN = 7 STEER_DELTA_DOWN = 7
STEER_DRIVER_ALLOWANCE = 50 STEER_DRIVER_ALLOWANCE = 50

@ -50,6 +50,7 @@ def get_can_parser(CP):
("CF_Clu_InhibitR", "CLU15", 0), ("CF_Clu_InhibitR", "CLU15", 0),
("CF_Lvr_Gear","LVR12",0), ("CF_Lvr_Gear","LVR12",0),
("CUR_GR", "TCU12",0),
("ACCEnable", "TCS13", 0), ("ACCEnable", "TCS13", 0),
("ACC_REQ", "TCS13", 0), ("ACC_REQ", "TCS13", 0),
@ -234,6 +235,17 @@ class CarState(object):
else: else:
self.gear_shifter_cluster = "unknown" self.gear_shifter_cluster = "unknown"
# Gear Selecton via TCU12
gear2 = cp.vl["TCU12"]["CUR_GR"]
if gear2 == 0:
self.gear_tcu = "park"
elif gear2 == 14:
self.gear_tcu = "reverse"
elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently
self.gear_tcu = "drive"
else:
self.gear_tcu = "unknown"
# save the entire LKAS11 and CLU11 # save the entire LKAS11 and CLU11
self.lkas11 = cp_cam.vl["LKAS11"] self.lkas11 = cp_cam.vl["LKAS11"]
self.clu11 = cp.vl["CLU11"] self.clu11 = cp.vl["CLU11"]

@ -1,11 +1,11 @@
#!/usr/bin/env python #!/usr/bin/env python
from cereal import car, log from cereal import car
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser
from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES
try: try:
from selfdrive.car.hyundai.carcontroller import CarController from selfdrive.car.hyundai.carcontroller import CarController
@ -113,6 +113,14 @@ class CarInterface(object):
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.16], [0.01]] ret.steerKpV, ret.steerKiV = [[0.16], [0.01]]
ret.minSteerSpeed = 35 * CV.MPH_TO_MS ret.minSteerSpeed = 35 * CV.MPH_TO_MS
elif candidate == CAR.KIA_OPTIMA:
ret.steerKf = 0.00005
ret.mass = 3558 * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_STINGER: elif candidate == CAR.KIA_STINGER:
ret.steerKf = 0.00005 ret.steerKf = 0.00005
ret.steerRateCost = 0.5 ret.steerRateCost = 0.5
@ -192,8 +200,10 @@ class CarInterface(object):
ret.wheelSpeeds.rr = self.CS.v_wheel_rr ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter # gear shifter
if self.CP.carFingerprint == CAR.ELANTRA: if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
ret.gearShifter = self.CS.gear_shifter_cluster ret.gearShifter = self.CS.gear_shifter_cluster
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
ret.gearShifter = self.CS.gear_tcu
else: else:
ret.gearShifter = self.CS.gear_shifter ret.gearShifter = self.CS.gear_shifter
@ -300,7 +310,7 @@ class CarInterface(object):
return ret.as_reader() return ret.as_reader()
def apply(self, c, perception_state=log.Live20Data.new_message()): def apply(self, c):
hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert) hud_alert = get_hud_alerts(c.hudControl.visualAlert, c.hudControl.audibleAlert)

@ -13,6 +13,7 @@ def get_hud_alerts(visual_alert, audible_alert):
class CAR: class CAR:
ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017" ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
GENESIS = "HYUNDAI GENESIS 2018" GENESIS = "HYUNDAI GENESIS 2018"
KIA_OPTIMA = "KIA OPTIMA SX 2019"
KIA_SORENTO = "KIA SORENTO GT LINE 2018" KIA_SORENTO = "KIA SORENTO GT LINE 2018"
KIA_STINGER = "KIA STINGER GT2 2018" KIA_STINGER = "KIA STINGER GT2 2018"
SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019" SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019"
@ -33,6 +34,9 @@ FINGERPRINTS = {
{ {
67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4
}], }],
CAR.KIA_OPTIMA: [{
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
CAR.KIA_SORENTO: [{ CAR.KIA_SORENTO: [{
67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1
}], }],
@ -53,12 +57,18 @@ CAMERA_MSGS = [832, 1156, 1191, 1342]
CHECKSUM = { CHECKSUM = {
"crc8": [CAR.SANTA_FE], "crc8": [CAR.SANTA_FE],
"6B": [CAR.KIA_SORENTO, CAR.GENESIS], "6B": [CAR.KIA_SORENTO, CAR.GENESIS],
"7B": [CAR.KIA_STINGER, CAR.ELANTRA], "7B": [CAR.KIA_STINGER, CAR.ELANTRA, CAR.KIA_OPTIMA],
}
FEATURES = {
"use_cluster_gears": [CAR.ELANTRA], # Use Cluster for Gear Selection, rather than Transmission
"use_tcu_gears": [CAR.KIA_OPTIMA], # Use TCU Message for Gear Selection
} }
DBC = { DBC = {
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS: dbc_dict('hyundai_kia_generic', None), CAR.GENESIS: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None), CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None),

@ -1,6 +1,6 @@
#!/usr/bin/env python #!/usr/bin/env python
import zmq import zmq
from cereal import car, log from cereal import car
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.services import service_list from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
@ -118,6 +118,6 @@ class CarInterface(object):
return ret.as_reader() return ret.as_reader()
def apply(self, c, perception_state=log.Live20Data.new_message()): def apply(self, c):
# in mock no carcontrols # in mock no carcontrols
return False return False

@ -5,7 +5,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \ create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \ create_ipas_steer_command, create_accel_command, \
create_fcw_command create_fcw_command, create_gas_command
from selfdrive.car.toyota.values import ECU, STATIC_MSGS from selfdrive.car.toyota.values import ECU, STATIC_MSGS
from selfdrive.can.packer import CANPacker from selfdrive.can.packer import CANPacker
@ -129,7 +129,16 @@ class CarController(object):
# *** compute control surfaces *** # *** compute control surfaces ***
# gas and brake # gas and brake
apply_accel = actuators.gas - actuators.brake
apply_gas = clip(actuators.gas, 0., 1.)
if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value
# +0.06 offset to reduce ABS pump usage when OP is engaged
apply_accel = 0.06 - actuators.brake
else:
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
@ -213,6 +222,11 @@ class CarController(object):
else: else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera: if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS: for addr in TARGET_IDS:
can_sends.append(create_video_target(frame/10, addr)) can_sends.append(create_video_target(frame/10, addr))

@ -62,6 +62,11 @@ def get_can_parser(CP):
if CP.carFingerprint == CAR.PRIUS: if CP.carFingerprint == CAR.PRIUS:
signals += [("STATE", "AUTOPARK_STATUS", 0)] signals += [("STATE", "AUTOPARK_STATUS", 0)]
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
checks.append(("GAS_SENSOR", 50))
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@ -112,7 +117,10 @@ class CarState(object):
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] if self.CP.enableGasInterceptor:
self.pedal_gas = cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS']
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']

@ -1,6 +1,6 @@
#!/usr/bin/env python #!/usr/bin/env python
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
from cereal import car, log from cereal import car
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -61,7 +61,7 @@ class CarInterface(object):
ret.safetyModel = car.CarParams.SafetyModels.toyota ret.safetyModel = car.CarParams.SafetyModels.toyota
# pedal # pedal
ret.enableCruise = True ret.enableCruise = not ret.enableGasInterceptor
# FIXME: hardcoding honda civic 2016 touring params so they can be used to # FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars # scale unknown params for other cars
@ -77,6 +77,7 @@ class CarInterface(object):
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
if candidate == CAR.PRIUS: if candidate == CAR.PRIUS:
stop_and_go = True
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.70 ret.wheelbase = 2.70
ret.steerRatio = 15.00 # unknown end-to-end spec ret.steerRatio = 15.00 # unknown end-to-end spec
@ -88,6 +89,7 @@ class CarInterface(object):
ret.steerActuatorDelay = 0.25 ret.steerActuatorDelay = 0.25
elif candidate in [CAR.RAV4, CAR.RAV4H]: elif candidate in [CAR.RAV4, CAR.RAV4H]:
stop_and_go = True if (candidate in CAR.RAV4H) else False
ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.65 ret.wheelbase = 2.65
ret.steerRatio = 16.30 # 14.5 is spec end-to-end ret.steerRatio = 16.30 # 14.5 is spec end-to-end
@ -97,6 +99,7 @@ class CarInterface(object):
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
elif candidate == CAR.COROLLA: elif candidate == CAR.COROLLA:
stop_and_go = False
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.70 ret.wheelbase = 2.70
ret.steerRatio = 17.8 ret.steerRatio = 17.8
@ -106,6 +109,7 @@ class CarInterface(object):
ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
elif candidate == CAR.LEXUS_RXH: elif candidate == CAR.LEXUS_RXH:
stop_and_go = True
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.79 ret.wheelbase = 2.79
ret.steerRatio = 16. # 14.8 is spec end-to-end ret.steerRatio = 16. # 14.8 is spec end-to-end
@ -115,6 +119,7 @@ class CarInterface(object):
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
elif candidate in [CAR.CHR, CAR.CHRH]: elif candidate in [CAR.CHR, CAR.CHRH]:
stop_and_go = True
ret.safetyParam = 100 ret.safetyParam = 100
ret.wheelbase = 2.63906 ret.wheelbase = 2.63906
ret.steerRatio = 13.6 ret.steerRatio = 13.6
@ -124,6 +129,7 @@ class CarInterface(object):
ret.steerKf = 0.00006 ret.steerKf = 0.00006
elif candidate in [CAR.CAMRY, CAR.CAMRYH]: elif candidate in [CAR.CAMRY, CAR.CAMRYH]:
stop_and_go = True
ret.safetyParam = 100 ret.safetyParam = 100
ret.wheelbase = 2.82448 ret.wheelbase = 2.82448
ret.steerRatio = 13.7 ret.steerRatio = 13.7
@ -133,6 +139,7 @@ class CarInterface(object):
ret.steerKf = 0.00006 ret.steerKf = 0.00006
elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]: elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
stop_and_go = True
ret.safetyParam = 100 ret.safetyParam = 100
ret.wheelbase = 2.78 ret.wheelbase = 2.78
ret.steerRatio = 16.0 ret.steerRatio = 16.0
@ -147,14 +154,12 @@ class CarInterface(object):
ret.longPidDeadzoneBP = [0., 9.] ret.longPidDeadzoneBP = [0., 9.]
ret.longPidDeadzoneV = [0., .15] ret.longPidDeadzoneV = [0., .15]
#detect the Pedal address
ret.enableGasInterceptor = 0x201 in fingerprint
# min speed to enable ACC. if car can do stop and go, then set enabling speed # min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter. # to a negative value, so it won't matter.
# hybrid models can't do stop and go even though the stock ACC can't ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS
if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.CHR,
CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.HIGHLANDERH, CAR.HIGHLANDER]:
ret.minEnableSpeed = -1.
elif candidate in [CAR.RAV4, CAR.COROLLA]: # TODO: hack ICE to do stop and go
ret.minEnableSpeed = 19. * CV.MPH_TO_MS
centerToRear = ret.wheelbase - ret.centerToFront centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for # TODO: get actual value, for now starting with reasonable value for
@ -178,8 +183,6 @@ class CarInterface(object):
# steer, gas, brake limitations VS speed # steer, gas, brake limitations VS speed
ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph
ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph
ret.gasMaxBP = [0.]
ret.gasMaxV = [0.5]
ret.brakeMaxBP = [5., 20.] ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8] ret.brakeMaxV = [1., 0.8]
@ -190,15 +193,24 @@ class CarInterface(object):
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu)
cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs) cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs)
cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
ret.steerLimitAlert = False ret.steerLimitAlert = False
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKiBP = [0., 35.]
ret.stoppingControl = False ret.stoppingControl = False
ret.startAccel = 0.0 ret.startAccel = 0.0
ret.longitudinalKpBP = [0., 5., 35.] if ret.enableGasInterceptor:
ret.longitudinalKpV = [3.6, 2.4, 1.5] ret.gasMaxBP = [0., 9., 35]
ret.longitudinalKiBP = [0., 35.] ret.gasMaxV = [0.2, 0.5, 0.7]
ret.longitudinalKiV = [0.54, 0.36] ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiV = [0.18, 0.12]
else:
ret.gasMaxBP = [0.]
ret.gasMaxV = [0.5]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
ret.longitudinalKiV = [0.54, 0.36]
return ret return ret
@ -234,7 +246,11 @@ class CarInterface(object):
# gas pedal # gas pedal
ret.gas = self.CS.car_gas ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 0 if self.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press
ret.gasPressed = self.CS.pedal_gas > 15
else:
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal # brake pedal
ret.brake = self.CS.user_brake ret.brake = self.CS.user_brake
@ -253,9 +269,11 @@ class CarInterface(object):
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0. ret.cruiseState.speedOffset = 0.
if self.CP.carFingerprint in [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER]:
if self.CP.carFingerprint in [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER] or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without # ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command # receiving any special command
# also if interceptor is detected
ret.cruiseState.standstill = False ret.cruiseState.standstill = False
else: else:
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
@ -346,7 +364,7 @@ class CarInterface(object):
# pass in a car.CarControl # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz
def apply(self, c, perception_state=log.Live20Data.new_message()): def apply(self, c):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert, c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,

@ -78,6 +78,18 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
} }
return packer.make_can_msg("ACC_CONTROL", 0, values) return packer.make_can_msg("ACC_CONTROL", 0, values)
def create_gas_command(packer, gas_amount):
"""Creates a CAN message for the Pedal DBC GAS_COMMAND."""
enable = gas_amount > 0.001
values = {"ENABLE": enable}
if enable:
values["GAS_COMMAND"] = gas_amount * 255.
values["GAS_COMMAND2"] = gas_amount * 255.
return packer.make_can_msg("GAS_COMMAND", 0, values)
def create_fcw_command(packer, fcw): def create_fcw_command(packer, fcw):
values = { values = {

@ -79,7 +79,7 @@ def check_ecu_msgs(fingerprint, ecu):
FINGERPRINTS = { FINGERPRINTS = {
CAR.RAV4: [{ CAR.RAV4: [{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}], }],
CAR.RAV4H: [{ CAR.RAV4H: [{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
@ -89,22 +89,23 @@ FINGERPRINTS = {
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
}], }],
CAR.PRIUS: [{ CAR.PRIUS: [{
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: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 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: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 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: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}, },
# Prius Prime # Prius Prime
{ {
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: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 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, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 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, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}, },
# Taiwanese Prius Prime # Taiwanese Prius Prime
{ {
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: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 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, 974: 8, 975: 5, 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, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 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, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 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, 974: 8, 975: 5, 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, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 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, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}], }],
#Corolla w/ added Pedal Support (512L and 513L)
CAR.COROLLA: [{ CAR.COROLLA: [{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}, },
# Corolla LE 2017 # Corolla LE 2017 w/ added Pedal Support (512L and 513L)
{ {
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
}], }],
CAR.LEXUS_RXH: [{ 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 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

@ -1 +1 @@
#define COMMA_VERSION "0.5.8-release" #define COMMA_VERSION "0.5.9-release"

@ -11,7 +11,6 @@ import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.services import service_list from selfdrive.services import service_list
from selfdrive.car.car_helpers import get_car from selfdrive.car.car_helpers import get_car
from selfdrive.controls.lib.planner import Planner
from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \ from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
get_events, \ get_events, \
create_event, \ create_event, \
@ -23,6 +22,7 @@ from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.driver_monitor import DriverStatus from selfdrive.controls.lib.driver_monitor import DriverStatus
from selfdrive.controls.lib.planner import _DT_MPC
from selfdrive.locationd.calibration_helpers import Calibration, Filter from selfdrive.locationd.calibration_helpers import Calibration, Filter
ThermalStatus = log.ThermalData.ThermalStatus ThermalStatus = log.ThermalData.ThermalStatus
@ -39,9 +39,9 @@ def isEnabled(state):
return (isActive(state) or state == State.preEnabled) return (isActive(state) or state == State.preEnabled)
def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_location, def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor,
poller, cal_status, cal_perc, overtemp, free_space, low_battery, poller, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, geofence, state, mismatch_counter, params): driver_status, state, mismatch_counter, params, plan, path_plan):
"""Receive data from sockets and create events for battery, temperature and disk space""" """Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events # Update carstate from CAN and create events
@ -54,7 +54,6 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
cal = None cal = None
hh = None hh = None
dm = None dm = None
gps = None
for socket, event in poller.poll(0): for socket, event in poller.poll(0):
if socket is thermal: if socket is thermal:
@ -65,8 +64,10 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
hh = messaging.recv_one(socket) hh = messaging.recv_one(socket)
elif socket is driver_monitor: elif socket is driver_monitor:
dm = messaging.recv_one(socket) dm = messaging.recv_one(socket)
elif socket is gps_location: elif socket is plan_sock:
gps = messaging.recv_one(socket) plan = messaging.recv_one(socket)
elif socket is path_plan_sock:
path_plan = messaging.recv_one(socket)
if td is not None: if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red overtemp = td.thermal.thermalStatus >= ThermalStatus.red
@ -110,32 +111,7 @@ def data_sample(CI, CC, thermal, calibration, health, driver_monitor, gps_locati
if dm is not None: if dm is not None:
driver_status.get_pose(dm.driverMonitoring, params) driver_status.get_pose(dm.driverMonitoring, params)
# Geofence return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan
if geofence is not None and gps is not None:
geofence.update_geofence_status(gps.gpsLocationExternal, params)
if geofence is not None and not geofence.in_geofence:
events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING]))
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
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, CP, VM, LaC, LoC, v_cruise_kph, force_decel)
plan = plan_packet.plan
plan_ts = plan_packet.logMonoTime
events += list(plan.events)
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
return plan, plan_ts
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
@ -225,8 +201,8 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc): driver_status, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc):
"""Given the state, this function returns an actuators packet""" """Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message() actuators = car.CarControl.Actuators.new_message()
@ -264,18 +240,25 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
AM.add(e, enabled, extra_text_2=extra_text) AM.add(e, enabled, extra_text_2=extra_text)
# Run angle offset learner at 20 Hz # Run angle offset learner at 20 Hz
if rk.frame % 5 == 2 and plan.lateralValid: if rk.frame % 5 == 2:
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset, angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle, path_plan.cPoly, path_plan.cProb, CS.steeringAngle,
CS.steeringPressed) CS.steeringPressed)
cur_time = sec_since_boot() # TODO: This won't work in replay
mpc_time = plan.l20MonoTime / 1e9
_DT = 0.01 # 100Hz
dt = min(cur_time - mpc_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
a_acc_sol = plan.aStart + (dt / _DT_MPC) * (plan.aTarget - plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
# Gas/Brake PID loop # Gas/Brake PID loop
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
CP, PL.lead_1)
# Steering PID loop and lateral MPC # Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL) CS.steeringPressed, CP, VM, path_plan)
# Send a "steering required alert" if saturation count has reached the limit # Send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert: if LaC.sat_flag and CP.steerLimitAlert:
@ -294,13 +277,15 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
AM.process_alerts(sec_since_boot()) AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, driver_status, angle_offset return actuators, v_cruise_kph, driver_status, angle_offset, v_acc_sol, a_acc_sol
def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, livempc, AM, driver_status, carcontrol, live100, AM, driver_status,
LaC, LoC, angle_offset, passive, start_time): LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc):
"""Send actuators and hud commands to the car, send live100 and MPC logging""" """Send actuators and hud commands to the car, send live100 and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
CC = car.CarControl.new_message() CC = car.CarControl.new_message()
@ -320,13 +305,15 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
CC.hudControl.speedVisible = isEnabled(state) CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state) CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = plan.hasLead CC.hudControl.leadVisible = plan.hasLead
CC.hudControl.rightLaneVisible = plan.hasRightLane CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5)
CC.hudControl.leftLaneVisible = plan.hasLeftLane CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5)
CC.hudControl.visualAlert = AM.visual_alert CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can # send car controls over can
CI.apply(CC, perception_state) CI.apply(CC)
force_decel = driver_status.awareness < 0.
# live100 # live100
dat = messaging.new_message() dat = messaging.new_message()
@ -343,6 +330,7 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
"driverMonitoringOn": bool(driver_status.monitor_on), "driverMonitoringOn": bool(driver_status.monitor_on),
"canMonoTimes": list(CS.canMonoTimes), "canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": plan_ts, "planMonoTime": plan_ts,
"pathPlanMonoTime": path_plan.logMonoTime,
"enabled": isEnabled(state), "enabled": isEnabled(state),
"active": isActive(state), "active": isActive(state),
"vEgo": CS.vEgo, "vEgo": CS.vEgo,
@ -362,8 +350,8 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
"upSteer": float(LaC.pid.p), "upSteer": float(LaC.pid.p),
"uiSteer": float(LaC.pid.i), "uiSteer": float(LaC.pid.i),
"ufSteer": float(LaC.pid.f), "ufSteer": float(LaC.pid.f),
"vTargetLead": float(plan.vTarget), "vTargetLead": float(v_acc),
"aTarget": float(plan.aTarget), "aTarget": float(a_acc),
"jerkFactor": float(plan.jerkFactor), "jerkFactor": float(plan.jerkFactor),
"angleOffset": float(angle_offset), "angleOffset": float(angle_offset),
"gpsPlannerActive": plan.gpsPlannerActive, "gpsPlannerActive": plan.gpsPlannerActive,
@ -372,6 +360,7 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
"cumLagMs": -rk.remaining * 1000., "cumLagMs": -rk.remaining * 1000.,
"startMonoTime": start_time, "startMonoTime": start_time,
"mapValid": plan.mapValid, "mapValid": plan.mapValid,
"forceDecel": bool(force_decel),
} }
live100.send(dat.to_bytes()) live100.send(dat.to_bytes())
@ -388,21 +377,13 @@ def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, ac
cc_send.carControl = CC cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes()) carcontrol.send(cc_send.to_bytes())
# send MPC when updated (20 Hz) if (rk.frame % 36000) == 0: # update angle offset every 6 minutes
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated: params.put("ControlsParams", json.dumps({'angle_offset': angle_offset}))
dat = messaging.new_message()
dat.init('liveMpc')
dat.liveMpc.x = list(LaC.mpc_solution[0].x)
dat.liveMpc.y = list(LaC.mpc_solution[0].y)
dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
dat.liveMpc.cost = LaC.mpc_solution[0].cost
livempc.send(dat.to_bytes())
return CC return CC
def controlsd_thread(gctx=None, rate=100, default_bias=0.): def controlsd_thread(gctx=None, rate=100):
gc.disable() gc.disable()
# start the loop # start the loop
@ -415,7 +396,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
live100 = messaging.pub_sock(context, service_list['live100'].port) live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port) carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
is_metric = params.get("IsMetric") == "1" is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0" passive = params.get("Passive") != "0"
@ -432,7 +412,8 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller) health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller) cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller)
driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller) driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller)
gps_location = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, conflate=True, poller=poller) plan_sock = messaging.sub_sock(context, service_list['plan'].port, conflate=True, poller=poller)
path_plan_sock = messaging.sub_sock(context, service_list['pathPlan'].port, conflate=True, poller=poller)
logcan = messaging.sub_sock(context, service_list['can'].port) logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message() CC = car.CarControl.new_message()
@ -449,11 +430,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
if passive: if passive:
CP.safetyModel = car.CarParams.SafetyModels.noOutput CP.safetyModel = car.CarParams.SafetyModels.noOutput
# Get FCW toggle from settings
fcw_enabled = params.get("IsFcwEnabled") == "1"
geofence = None
PL = Planner(CP, fcw_enabled)
LoC = LongControl(CP, CI.compute_gb) LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP) VM = VehicleModel(CP)
LaC = LatControl(CP) LaC = LatControl(CP)
@ -478,17 +454,19 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
mismatch_counter = 0 mismatch_counter = 0
low_battery = False low_battery = False
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) plan = messaging.new_message()
plan.init('plan')
path_plan = messaging.new_message()
path_plan.init('pathPlan')
# Read angle offset from previous drive, fallback to default rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
angle_offset = default_bias controls_params = params.get("ControlsParams")
calibration_params = params.get("CalibrationParams") # Read angle offset from previous drive
if calibration_params: if controls_params is not None:
try: controls_params = json.loads(controls_params)
calibration_params = json.loads(calibration_params) angle_offset = controls_params['angle_offset']
angle_offset = calibration_params["angle_offset2"] else:
except (ValueError, KeyError): angle_offset = 0.
pass
prof = Profiler(False) # off by default prof = Profiler(False) # off by default
@ -497,13 +475,21 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
prof.checkpoint("Ratekeeper", ignore=True) prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events # Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter = data_sample(CI, CC, thermal, cal, health, CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\
driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params) data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status,
state, mismatch_counter, params, plan, path_plan)
prof.checkpoint("Sample") prof.checkpoint("Sample")
# Define longitudinal plan (MPC) path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
plan, plan_ts = calc_plan(CS, CP, VM, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence) plan_age = (start_time - plan.logMonoTime) / 1e9
prof.checkpoint("Plan") if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
events += list(plan.plan.events)
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not passive: if not passive:
# update control state # update control state
@ -512,13 +498,16 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
prof.checkpoint("State transition") prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC) # Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, angle_offset = state_control(plan, CS, CP, state, events, v_cruise_kph, actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc) state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status,
LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
prof.checkpoint("State Control") prof.checkpoint("State Control")
# Publish data # Publish data
CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive, start_time) live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc)
prof.checkpoint("Sent") prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz rk.keep_time() # Run at 100Hz

@ -1,23 +1,11 @@
import math
import numpy as np
from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from common.numpy_fast import interp from common.numpy_fast import interp
from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
from cereal import car from cereal import car
_DT = 0.01 # 100Hz _DT = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz _DT_MPC = 0.05 # 20Hz
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
states[0].x = v_ego * delay
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
return states
def get_steer_max(CP, v_ego): def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV) return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
@ -28,75 +16,12 @@ class LatControl(object):
(CP.steerKiBP, CP.steerKiV), (CP.steerKiBP, CP.steerKiV),
k_f=CP.steerKf, pos_limit=1.0) k_f=CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0 self.last_cloudlog_t = 0.0
self.setup_mpc(CP.steerRateCost) self.angle_steers_des = 0.
def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost)
self.mpc_solution = libmpc_py.ffi.new("log_t *")
self.cur_state = libmpc_py.ffi.new("state_t *")
self.mpc_updated = False
self.mpc_nans = False
self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
self.cur_state[0].psi = 0.0
self.cur_state[0].delta = 0.0
self.last_mpc_ts = 0.0
self.angle_steers_des = 0.0
self.angle_steers_des_mpc = 0.0
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
def reset(self): def reset(self):
self.pid.reset() self.pid.reset()
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL): def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan):
cur_time = sec_since_boot()
self.mpc_updated = False
# TODO: this creates issues in replay when rewinding time: mpc won't run
if self.last_mpc_ts < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
self.angle_steers_des_prev = self.angle_steers_des_mpc
curvature_factor = VM.curvature_factor(v_ego)
l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width)
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
else:
delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio
self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset)
self.angle_steers_des_time = cur_time
self.mpc_updated = True
# Check for infeasable MPC solution
self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if self.mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if v_ego < 0.3 or not active: if v_ego < 0.3 or not active:
output_steer = 0.0 output_steer = 0.0
self.pid.reset() self.pid.reset()
@ -105,7 +30,8 @@ class LatControl(object):
# constant for 0.05s. # constant for 0.05s.
#dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps #dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev) #self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
self.angle_steers_des = self.angle_steers_des_mpc self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
steers_max = get_steer_max(CP, v_ego) steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max self.pid.neg_limit = -steers_max

@ -71,7 +71,7 @@ class LongControl(object):
self.pid.reset() self.pid.reset()
self.v_pid = v_pid self.v_pid = v_pid
def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, lead_1): def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP):
"""Update longitudinal control. This updates the state machine and runs a PID loop""" """Update longitudinal control. This updates the state machine and runs a PID loop"""
# Actuation limits # Actuation limits
gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)

@ -0,0 +1,65 @@
from common.numpy_fast import interp
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
CAMERA_OFFSET = 0.06 # m from center car to camera
class ModelParser(object):
def __init__(self):
self.d_poly = [0., 0., 0., 0.]
self.c_poly = [0., 0., 0., 0.]
self.c_prob = 0.
self.last_model = 0.
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
self._path_pinv = compute_path_pinv()
self.lane_width_estimate = 3.7
self.lane_width_certainty = 1.0
self.lane_width = 3.7
self.l_prob = 0.
self.r_prob = 0.
def update(self, v_ego, md):
if md is not None:
p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path
l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line
r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line
# only offset left and right lane lines; offsetting p_poly does not make sense
l_poly[3] += CAMERA_OFFSET
r_poly[3] += CAMERA_OFFSET
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
l_prob = md.model.leftLane.prob # left line prob
r_prob = md.model.rightLane.prob # right line prob
# Find current lanewidth
lr_prob = l_prob * r_prob
self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty)
current_lane_width = abs(l_poly[3] - r_poly[3])
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
lane_width_diff = abs(self.lane_width - current_lane_width)
lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0])
r_prob *= lane_r_prob
self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob
self.lead_var = md.model.lead.std**2
# compute target path
self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
self.r_poly = r_poly
self.r_prob = r_prob
self.l_poly = l_poly
self.l_prob = l_prob
self.p_poly = p_poly
self.p_prob = p_prob

@ -1,64 +1,122 @@
from common.numpy_fast import interp import zmq
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv import math
import numpy as np
from common.realtime import sec_since_boot
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.lateral_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.model_parser import ModelParser
import selfdrive.messaging as messaging
def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay):
states[0].x = v_ego * delay
states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay
return states
CAMERA_OFFSET = 0.06 # m from center car to camera
class PathPlanner(object): class PathPlanner(object):
def __init__(self): def __init__(self, CP):
self.d_poly = [0., 0., 0., 0.] self.MP = ModelParser()
self.c_poly = [0., 0., 0., 0.]
self.c_prob = 0. self.last_cloudlog_t = 0
self.last_model = 0.
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 context = zmq.Context()
self._path_pinv = compute_path_pinv() self.plan = messaging.pub_sock(context, service_list['pathPlan'].port)
self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
self.lane_width_estimate = 3.7
self.lane_width_certainty = 1.0 self.setup_mpc(CP.steerRateCost)
self.lane_width = 3.7 self.invalid_counter = 0
self.l_prob = 0.
self.r_prob = 0. def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
def update(self, v_ego, md): self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost)
if md is not None:
p_poly = model_polyfit(md.model.path.points, self._path_pinv) # predicted path self.mpc_solution = libmpc_py.ffi.new("log_t *")
l_poly = model_polyfit(md.model.leftLane.points, self._path_pinv) # left line self.cur_state = libmpc_py.ffi.new("state_t *")
r_poly = model_polyfit(md.model.rightLane.points, self._path_pinv) # right line self.cur_state[0].x = 0.0
self.cur_state[0].y = 0.0
# only offset left and right lane lines; offsetting p_poly does not make sense self.cur_state[0].psi = 0.0
l_poly[3] += CAMERA_OFFSET self.cur_state[0].delta = 0.0
r_poly[3] += CAMERA_OFFSET
self.angle_steers_des = 0.0
p_prob = 1. # model does not tell this probability yet, so set to 1 for now self.angle_steers_des_mpc = 0.0
l_prob = md.model.leftLane.prob # left line prob self.angle_steers_des_prev = 0.0
r_prob = md.model.rightLane.prob # right line prob self.angle_steers_des_time = 0.0
# Find current lanewidth def update(self, CP, VM, CS, md, live100):
lr_prob = l_prob * r_prob v_ego = CS.carState.vEgo
self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty) angle_steers = CS.carState.steeringAngle
current_lane_width = abs(l_poly[3] - r_poly[3]) active = live100.live100.active
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate) angle_offset = live100.live100.angleOffset
speed_lane_width = interp(v_ego, [0., 31.], [3., 3.8]) self.MP.update(v_ego, md)
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width # Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
lane_width_diff = abs(self.lane_width - current_lane_width) curvature_factor = VM.curvature_factor(v_ego)
lane_r_prob = interp(lane_width_diff, [0.3, 1.0], [1.0, 0.0])
l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly))
r_prob *= lane_r_prob r_poly = libmpc_py.ffi.new("double[4]", list(self.MP.r_poly))
p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly))
self.lead_dist = md.model.lead.dist
self.lead_prob = md.model.lead.prob # account for actuation delay
self.lead_var = md.model.lead.std**2 self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
# compute target path v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.d_poly, self.c_poly, self.c_prob = calc_desired_path( self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width) l_poly, r_poly, p_poly,
self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width)
self.r_poly = r_poly
self.r_prob = r_prob # reset to current steer angle if not active or overriding
if active:
self.l_poly = l_poly delta_desired = self.mpc_solution[0].delta[1]
self.l_prob = l_prob else:
delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio
self.p_poly = p_poly
self.p_prob = p_prob self.cur_state[0].delta = delta_desired
self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset)
# Check for infeasable MPC solution
mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")
if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge
self.invalid_counter += 1
else:
self.invalid_counter = 0
plan_valid = self.invalid_counter < 2
plan_send = messaging.new_message()
plan_send.init('pathPlan')
plan_send.pathPlan.laneWidth = float(self.MP.lane_width)
plan_send.pathPlan.dPoly = map(float, self.MP.d_poly)
plan_send.pathPlan.cPoly = map(float, self.MP.c_poly)
plan_send.pathPlan.cProb = float(self.MP.c_prob)
plan_send.pathPlan.lPoly = map(float, l_poly)
plan_send.pathPlan.lProb = float(self.MP.l_prob)
plan_send.pathPlan.rPoly = map(float, r_poly)
plan_send.pathPlan.rProb = float(self.MP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.valid = bool(plan_valid)
self.plan.send(plan_send.to_bytes())
dat = messaging.new_message()
dat.init('liveMpc')
dat.liveMpc.x = list(self.mpc_solution[0].x)
dat.liveMpc.y = list(self.mpc_solution[0].y)
dat.liveMpc.psi = list(self.mpc_solution[0].psi)
dat.liveMpc.delta = list(self.mpc_solution[0].delta)
dat.liveMpc.cost = self.mpc_solution[0].cost
self.livempc.send(dat.to_bytes())

@ -1,10 +1,7 @@
#!/usr/bin/env python #!/usr/bin/env python
import os
import zmq import zmq
import math import math
import numpy as np import numpy as np
from copy import copy
from cereal import log
from collections import defaultdict from collections import defaultdict
from common.params import Params from common.params import Params
from common.realtime import sec_since_boot from common.realtime import sec_since_boot
@ -14,23 +11,17 @@ from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.services import service_list from selfdrive.services import service_list
from selfdrive.controls.lib.drive_helpers import create_event, MPC_COST_LONG, EventTypes as ET from selfdrive.controls.lib.drive_helpers import create_event, MPC_COST_LONG, EventTypes as ET
from selfdrive.controls.lib.pathplanner import PathPlanner
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
from selfdrive.controls.lib.speed_smoother import speed_smoother from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU 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 = 1.85 # m/s^2
NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS
_DT = 0.01 # 100Hz
_DT_MPC = 0.2 # 5Hz _DT_MPC = 0.2 # 5Hz
MAX_SPEED_ERROR = 2.0 MAX_SPEED_ERROR = 2.0
AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
GPS_PLANNER_ADDR = "192.168.5.1"
# lookup tables VS speed to determine min and max accels in cruise # lookup tables VS speed to determine min and max accels in cruise
# make sure these accelerations are smaller than mpc limits # make sure these accelerations are smaller than mpc limits
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
@ -98,7 +89,7 @@ class FCWChecker(object):
# then limit ARel so that v_lead will get to zero in no sooner than t_decel. # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
# This helps underweighting ARel when v_lead is close to zero. # This helps underweighting ARel when v_lead is close to zero.
t_decel = 2. t_decel = 2.
a_rel = np.minimum(a_rel, v_lead/t_decel) a_rel = np.minimum(a_rel, v_lead / t_decel)
# delta of the quadratic equation to solve for ttc # delta of the quadratic equation to solve for ttc
delta = v_rel**2 + 2 * x_lead * a_rel delta = v_rel**2 + 2 * x_lead * a_rel
@ -186,6 +177,8 @@ class LongitudinalMpc(object):
self.cur_state[0].a_ego = a self.cur_state[0].a_ego = a
def update(self, CS, lead, v_cruise_setpoint): def update(self, CS, lead, v_cruise_setpoint):
v_ego = CS.carState.vEgo
# Setup current mpc state # Setup current mpc state
self.cur_state[0].x_ego = 0.0 self.cur_state[0].x_ego = 0.0
@ -194,7 +187,6 @@ class LongitudinalMpc(object):
v_lead = max(0.0, lead.vLead) v_lead = max(0.0, lead.vLead)
a_lead = lead.aLeadK a_lead = lead.aLeadK
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
v_lead = 0.0 v_lead = 0.0
a_lead = 0.0 a_lead = 0.0
@ -213,7 +205,7 @@ class LongitudinalMpc(object):
self.prev_lead_status = False self.prev_lead_status = False
# Fake a fast lead car, so mpc keeps running # Fake a fast lead car, so mpc keeps running
self.cur_state[0].x_l = 50.0 self.cur_state[0].x_l = 50.0
self.cur_state[0].v_l = CS.vEgo + 10.0 self.cur_state[0].v_l = v_ego + 10.0
a_lead = 0.0 a_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU self.a_lead_tau = _LEAD_ACCEL_TAU
@ -242,10 +234,10 @@ class LongitudinalMpc(object):
self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
self.cur_state[0].v_ego = CS.vEgo self.cur_state[0].v_ego = v_ego
self.cur_state[0].a_ego = 0.0 self.cur_state[0].a_ego = 0.0
self.v_mpc = CS.vEgo self.v_mpc = v_ego
self.a_mpc = CS.aEgo self.a_mpc = CS.carState.aEgo
self.prev_lead_status = False self.prev_lead_status = False
@ -255,38 +247,20 @@ class Planner(object):
self.CP = CP self.CP = CP
self.poller = zmq.Poller() 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)
else:
self.gps_planner_plan = None
self.plan = messaging.pub_sock(context, service_list['plan'].port) self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port) self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
self.last_md_ts = 0
self.last_l20_ts = 0
self.last_model = 0.
self.last_l20 = 0.
self.model_dead = True
self.radar_dead = True
self.radar_errors = [] self.radar_errors = []
self.PP = PathPlanner()
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc) self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc) self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
self.v_acc_start = 0.0 self.v_acc_start = 0.0
self.a_acc_start = 0.0 self.a_acc_start = 0.0
self.acc_start_time = sec_since_boot()
self.v_acc = 0.0 self.v_acc = 0.0
self.v_acc_sol = 0.0
self.v_acc_future = 0.0 self.v_acc_future = 0.0
self.a_acc = 0.0 self.a_acc = 0.0
self.a_acc_sol = 0.0
self.v_cruise = 0.0 self.v_cruise = 0.0
self.a_cruise = 0.0 self.a_cruise = 0.0
@ -298,11 +272,6 @@ class Planner(object):
self.fcw_checker = FCWChecker() self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled self.fcw_enabled = fcw_enabled
self.last_gps_planner_plan = None
self.gps_planner_active = False
self.last_live_map_data = None
self.perception_state = log.Live20Data.new_message()
self.params = Params() self.params = Params()
self.v_curvature = NO_CURVATURE_SPEED self.v_curvature = NO_CURVATURE_SPEED
self.v_speedlimit = NO_CURVATURE_SPEED self.v_speedlimit = NO_CURVATURE_SPEED
@ -319,12 +288,6 @@ class Planner(object):
slowest = min(solutions, key=solutions.get) slowest = min(solutions, key=solutions.get)
"""
print "D_SOL", solutions, slowest, self.v_acc_sol, self.a_acc_sol
print "D_V", self.mpc1.v_mpc, self.mpc2.v_mpc, self.v_cruise
print "D_A", self.mpc1.a_mpc, self.mpc2.a_mpc, self.a_cruise
"""
self.longitudinalPlanSource = slowest self.longitudinalPlanSource = slowest
# Choose lowest of MPC and cruise # Choose lowest of MPC and cruise
@ -340,201 +303,144 @@ class Planner(object):
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) 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, CP, VM, PP, live20, live100, md, live_map_data):
def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel): """Gets called when new live20 is available"""
cur_time = sec_since_boot() cur_time = live20.logMonoTime / 1e9
v_ego = CS.carState.vEgo
long_control_state = live100.live100.longControlState
v_cruise_kph = live100.live100.vCruise
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = None self.last_md_ts = md.logMonoTime
l20 = None
gps_planner_plan = None self.radar_errors = list(live20.live20.radarErrors)
for socket, event in self.poller.poll(0): self.lead_1 = live20.live20.leadOne
if socket is self.model: self.lead_2 = live20.live20.leadTwo
md = messaging.recv_one(socket)
elif socket is self.live20: enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
l20 = messaging.recv_one(socket) following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
elif socket is self.gps_planner_plan:
gps_planner_plan = messaging.recv_one(socket) self.v_speedlimit = NO_CURVATURE_SPEED
elif socket is self.live_map_data: self.v_curvature = NO_CURVATURE_SPEED
self.last_live_map_data = messaging.recv_one(socket).liveMapData self.map_valid = live_map_data.liveMapData.mapValid
if gps_planner_plan is not None: # Speed limit and curvature
self.last_gps_planner_plan = gps_planner_plan set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
if set_speed_limit_active:
if md is not None: if live_map_data.liveMapData.speedLimitValid:
self.last_md_ts = md.logMonoTime speed_limit = live_map_data.liveMapData.speedLimit
self.last_model = cur_time offset = float(self.params.get("SpeedLimitOffset"))
self.model_dead = False self.v_speedlimit = speed_limit + offset
self.PP.update(CS.vEgo, md) if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
if self.last_gps_planner_plan is not None: a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
plan = self.last_gps_planner_plan.gpsPlannerPlan v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
self.gps_planner_active = plan.valid self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
if plan.valid:
self.PP.d_poly = plan.poly self.decel_for_turn = bool(self.v_curvature < min([v_cruise_setpoint, self.v_speedlimit, v_ego + 1.]))
self.PP.p_poly = plan.poly v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
self.PP.c_poly = plan.poly
self.PP.l_prob = 0.0 # Calculate speed for normal cruise control
self.PP.r_prob = 0.0 if enabled:
self.PP.c_prob = 1.0 accel_limits = map(float, calc_cruise_accel_limits(v_ego, following))
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
if l20 is not None: accel_limits = limit_accel_in_turns(v_ego, CS.carState.steeringAngle, accel_limits, self.CP)
self.perception_state = copy(l20.live20)
self.last_l20_ts = l20.logMonoTime if force_slow_decel:
self.last_l20 = cur_time # if required so, force a smooth deceleration
self.radar_dead = False accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
self.radar_errors = list(l20.live20.radarErrors) accel_limits[0] = min(accel_limits[0], accel_limits[1])
self.v_acc_start = self.v_acc_sol # Change accel limits based on time remaining to turn
self.a_acc_start = self.a_acc_sol if self.decel_for_turn:
self.acc_start_time = cur_time time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn)
self.lead_1 = l20.live20.leadOne accel_limits[0] = max(accel_limits[0], required_decel)
self.lead_2 = l20.live20.leadTwo
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping) v_cruise_setpoint,
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 accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0],
if self.last_live_map_data: _DT_MPC)
self.v_speedlimit = NO_CURVATURE_SPEED # cruise speed can't be negative even is user is distracted
self.v_curvature = NO_CURVATURE_SPEED self.v_cruise = max(self.v_cruise, 0.)
self.map_valid = self.last_live_map_data.mapValid else:
starting = long_control_state == LongCtrlState.starting
# Speed limit a_ego = min(CS.carState.aEgo, 0.0)
if self.last_live_map_data.speedLimitValid: reset_speed = MIN_CAN_SPEED if starting else v_ego
speed_limit = self.last_live_map_data.speedLimit reset_accel = self.CP.startAccel if starting else a_ego
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None self.v_acc = reset_speed
self.a_acc = reset_accel
if set_speed_limit_active: self.v_acc_start = reset_speed
offset = float(self.params.get("SpeedLimitOffset")) self.a_acc_start = reset_accel
self.v_speedlimit = speed_limit + offset self.v_cruise = reset_speed
self.a_cruise = reset_accel
# Curvature
if self.last_live_map_data.curvatureValid: self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
curvature = abs(self.last_live_map_data.curvature) self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
v_curvature = math.sqrt(A_Y_MAX / max(1e-4, curvature))
self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature) self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
# leave 1m/s margin on vEgo to asses if turn is limiting our speed.
self.decel_for_turn = bool(self.v_curvature < min([v_cruise_setpoint, self.v_speedlimit, CS.vEgo + 1.])) self.choose_solution(v_cruise_setpoint, enabled)
v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
# determine fcw
# Calculate speed for normal cruise control if self.mpc1.new_lead:
if enabled: self.fcw_checker.reset_lead(cur_time)
accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP) self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
if force_slow_decel: self.lead_1.fcw, blinkers) and not CS.carState.brakePressed
# if required so, force a smooth deceleration if self.fcw:
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL) cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
# Change accel limits based on time remaining to turn
if self.decel_for_turn:
time_to_turn = max(1.0, self.last_live_map_data.distToTurn / max(self.v_cruise, 1.))
required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0],
_DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5:
self.model_dead = True
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
# **** send the plan **** # **** send the plan ****
plan_send = messaging.new_message() plan_send = messaging.new_message()
plan_send.init('plan') plan_send.init('plan')
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = [] events = []
if self.model_dead: if model_dead:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.radar_dead or 'commIssue' in self.radar_errors:
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if 'fault' in self.radar_errors: if 'fault' in self.radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if LaC.mpc_solution[0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(cur_time - self.acc_start_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol + self.a_acc_start) / 2.0
plan_send.plan.events = events plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = self.last_l20_ts plan_send.plan.l20MonoTime = live20.logMonoTime
# lateral plan
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan # longitudal plan
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vCruise = self.v_cruise plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vTarget = self.v_acc_sol plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aTarget = self.a_acc_sol plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.hasLeftLane = bool(self.PP.l_prob > 0.5)
plan_send.plan.hasRightLane = bool(self.PP.r_prob > 0.5)
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.gpsPlannerActive = self.gps_planner_active
plan_send.plan.vCurvature = self.v_curvature plan_send.plan.vCurvature = self.v_curvature
plan_send.plan.decelForTurn = self.decel_for_turn plan_send.plan.decelForTurn = self.decel_for_turn
plan_send.plan.mapValid = self.map_valid plan_send.plan.mapValid = self.map_valid
# Send out fcw # Send out fcw
fcw = self.fcw and (self.fcw_enabled or LoC.long_control_state != LongCtrlState.off) fcw = self.fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes()) self.plan.send(plan_send.to_bytes())
return plan_send
# Interpolate 0.05 seconds and save as starting point for next iteration
dt = 0.05 # s
a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc - self.a_acc_start)
v_acc_sol = self.v_acc_start + dt * (a_acc_sol + self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol

@ -0,0 +1,69 @@
#!/usr/bin/env python
import zmq
from cereal import car
from common.params import Params
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from selfdrive.controls.lib.planner import Planner
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.pathplanner import PathPlanner
import selfdrive.messaging as messaging
def plannerd_thread():
context = zmq.Context()
params = Params()
# Get FCW toggle from settings
fcw_enabled = params.get("IsFcwEnabled") == "1"
cloudlog.info("plannerd is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("plannerd got CarParams: %s", CP.carName)
PL = Planner(CP, fcw_enabled)
PP = PathPlanner(CP)
VM = VehicleModel(CP)
poller = zmq.Poller()
car_state_sock = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller)
live100_sock = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller)
model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller)
car_state = messaging.new_message()
car_state.init('carState')
live100 = messaging.new_message()
live100.init('live100')
model = messaging.new_message()
model.init('model')
live20 = messaging.new_message()
live20.init('live20')
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
while True:
for socket, event in poller.poll():
if socket is live100_sock:
live100 = messaging.recv_one(socket)
elif socket is car_state_sock:
car_state = messaging.recv_one(socket)
elif socket is model_sock:
model = messaging.recv_one(socket)
PP.update(CP, VM, car_state, model, live100)
elif socket is live_map_data_sock:
live_map_data = messaging.recv_one(socket)
elif socket is live20_sock:
live20 = messaging.recv_one(socket)
PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data)
def main(gctx=None):
plannerd_thread()
if __name__ == "__main__":
main()

@ -8,7 +8,7 @@ from fastcluster import linkage_vector
import selfdrive.messaging as messaging import selfdrive.messaging as messaging
from selfdrive.services import service_list from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
from selfdrive.controls.lib.pathplanner import PathPlanner from selfdrive.controls.lib.model_parser import ModelParser
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \ from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \
RDR_TO_LDR, NO_FUSION_SCORE RDR_TO_LDR, NO_FUSION_SCORE
from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.vehicle_model import VehicleModel
@ -66,7 +66,7 @@ def radard_thread(gctx=None):
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller) model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller) live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
PP = PathPlanner() MP = ModelParser()
RI = RadarInterface(CP) RI = RadarInterface(CP)
last_md_ts = 0 last_md_ts = 0
@ -134,26 +134,26 @@ def radard_thread(gctx=None):
last_md_ts = md.logMonoTime last_md_ts = md.logMonoTime
# *** get path prediction from the model *** # *** get path prediction from the model ***
PP.update(v_ego, md) MP.update(v_ego, md)
# run kalman filter only if prob is high enough # run kalman filter only if prob is high enough
if PP.lead_prob > 0.7: if MP.lead_prob > 0.7:
reading = speedSensorV.read(PP.lead_dist, covar=np.matrix(PP.lead_var)) reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var))
ekfv.update_scalar(reading) ekfv.update_scalar(reading)
ekfv.predict(tsv) ekfv.predict(tsv)
# When changing lanes the distance to the lead car can suddenly change, # When changing lanes the distance to the lead car can suddenly change,
# which makes the Kalman filter output large relative acceleration # which makes the Kalman filter output large relative acceleration
if mocked and abs(PP.lead_dist - ekfv.state[XV]) > 2.0: if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0:
ekfv.state[XV] = PP.lead_dist ekfv.state[XV] = MP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0. ekfv.state[SPEEDV] = 0.
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])), ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(MP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), False) float(ekfv.state[SPEEDV]), False)
else: else:
ekfv.state[XV] = PP.lead_dist ekfv.state[XV] = MP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init])) ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0. ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts: if VISION_POINT in ar_pts:
@ -162,7 +162,7 @@ def radard_thread(gctx=None):
# *** compute the likely path_y *** # *** compute the likely path_y ***
if (active and not steer_override) or mocked: if (active and not steer_override) or mocked:
# use path from model (always when mocking as steering is too noisy) # use path from model (always when mocking as steering is too noisy)
path_y = np.polyval(PP.d_poly, path_x) path_y = np.polyval(MP.d_poly, path_x)
else: else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset # use path from steer, set angle_offset to 0 it does not only report the physical offset
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0] path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]

@ -26,6 +26,7 @@ if __name__ == "__main__":
parser.add_argument('--proxy', action='store_true', help='republish on localhost') parser.add_argument('--proxy', action='store_true', help='republish on localhost')
parser.add_argument('--map', action='store_true') parser.add_argument('--map', action='store_true')
parser.add_argument('--addr', default='127.0.0.1') parser.add_argument('--addr', default='127.0.0.1')
parser.add_argument('--values', help='values to monitor (instead of entire event)')
parser.add_argument("socket", type=str, nargs='*', help="socket name") parser.add_argument("socket", type=str, nargs='*', help="socket name")
args = parser.parse_args() args = parser.parse_args()
@ -53,6 +54,10 @@ if __name__ == "__main__":
server_thread.start() server_thread.start()
print 'server running' print 'server running'
values = None
if args.values:
values = [s.strip().split(".") for s in args.values.split(",")]
while 1: while 1:
polld = poller.poll(timeout=1000) polld = poller.poll(timeout=1000)
for sock, mode in polld: for sock, mode in polld:
@ -79,5 +84,14 @@ if __name__ == "__main__":
print(json.loads(msg)) print(json.loads(msg))
elif args.dump_json: elif args.dump_json:
print json.dumps(evt.to_dict()) print json.dumps(evt.to_dict())
elif values:
print "logMonotime = {}".format(evt.logMonoTime)
for value in values:
if hasattr(evt, value[0]):
item = evt
for key in value:
item = getattr(item, key)
print "{} = {}".format(".".join(value), item)
print ""
else: else:
print evt print evt

@ -1,208 +1,77 @@
#!/usr/bin/env python #!/usr/bin/env python
import os import os
import zmq import zmq
import cv2
import copy import copy
import json import json
import numpy as np import numpy as np
import selfdrive.messaging as messaging import selfdrive.messaging as messaging
from selfdrive.locationd.calibration_helpers import Calibration, Filter from selfdrive.locationd.calibration_helpers import Calibration
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list from selfdrive.services import service_list
from common.params import Params 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, get_camera_frame_from_bigmodel_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, \ 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 eon_intrinsics, get_calib_from_vp, H, W
MPH_TO_MS = 0.44704
FRAMES_NEEDED = 120 # allow to update VP every so many frames MIN_SPEED_FILTER = 15 * MPH_TO_MS
VP_CYCLES_NEEDED = 2 MAX_YAW_RATE_FILTER = np.radians(2) # per second
CALIBRATION_CYCLES_NEEDED = FRAMES_NEEDED * VP_CYCLES_NEEDED INPUTS_NEEDED = 300 # allow to update VP every so many frames
DT = 0.1 # nominal time step of 10Hz (orbd_live runs slower on pc) INPUTS_WANTED = 600 # We want a little bit more than we need for stability
VP_RATE_LIM = 2. * DT # 2 px/s WRITE_CYCLES = 400 # write every 400 cycles
VP_INIT = np.array([W/2., H/2.]) VP_INIT = np.array([W/2., H/2.])
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
# big model is 864x288
VP_VALIDITY_CORNERS = np.array([[-150., -200.], [150., 200.]]) + VP_INIT
GRID_WEIGHT_INIT = 2e6
MAX_LINES = 500 # max lines to avoid over computation
HOOD_HEIGHT = H*3/4 # the part of image usually free from the car's hood
# These validity corners were chosen by looking at 1000
# and taking most extreme cases with some margin.
VP_VALIDITY_CORNERS = np.array([[W/2 - 150, 280], [W/2 + 150, 540]])
DEBUG = os.getenv("DEBUG") is not None DEBUG = os.getenv("DEBUG") is not None
# Wrap c code for slow grid incrementation
c_header = "\nvoid increment_grid(double *grid, double *lines, long long n);"
c_code = "#define H %d\n" % H
c_code += "#define W %d\n" % W
c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "get_vp.c")).read()
ffi, lib = ffi_wrap('get_vp', c_code, c_header)
def increment_grid_c(grid, lines, n):
lib.increment_grid(ffi.cast("double *", grid.ctypes.data),
ffi.cast("double *", lines.ctypes.data),
ffi.cast("long long", n))
def get_lines(p):
A = (p[:,0,1] - p[:,1,1])
B = (p[:,1,0] - p[:,0,0])
C = (p[:,0,0]*p[:,1,1] - p[:,1,0]*p[:,0,1])
return np.column_stack((A, B, -C))
def correct_pts(pts, rot_speeds, dt):
pts = np.hstack((pts, np.ones((pts.shape[0],1))))
cam_rot = dt * view_frame_from_device_frame.dot(rot_speeds)
rot = orient.rot_matrix(*cam_rot.T).T
pts_corrected = rot.dot(pts.T).T
pts_corrected[:,0] /= pts_corrected[:,2]
pts_corrected[:,1] /= pts_corrected[:,2]
return pts_corrected[:,:2]
def gaussian_kernel(sizex, sizey, stdx, stdy, dx, dy):
y, x = np.mgrid[-sizey:sizey+1, -sizex:sizex+1]
g = np.exp(-((x - dx)**2 / (2. * stdx**2) + (y - dy)**2 / (2. * stdy**2)))
return g / g.sum()
def gaussian_kernel_1d(kernel):
#creates separable gaussian filter
u,s,v = np.linalg.svd(kernel)
x = u[:,0]*np.sqrt(s[0])
y = np.sqrt(s[0])*v[0,:]
return x, y
def blur_image(img, kernel_x, kernel_y):
return cv2.sepFilter2D(img.astype(np.uint16), -1, kernel_x, kernel_y)
def is_calibration_valid(vp): def is_calibration_valid(vp):
return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \ return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \
vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1] vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1]
class Calibrator(object): class Calibrator(object):
def __init__(self, param_put=False): def __init__(self, param_put=False):
self.param_put = param_put self.param_put = param_put
self.speed = 0
self.vp_cycles = 0
self.angle_offset = 0.
self.yaw_rate = 0.
self.l100_last_updated = 0
self.prev_orbs = None
self.kernel = gaussian_kernel(11, 11, 2.35, 2.35, 0, 0)
self.kernel_x, self.kernel_y = gaussian_kernel_1d(self.kernel)
self.vp = copy.copy(VP_INIT) self.vp = copy.copy(VP_INIT)
self.vps = []
self.cal_status = Calibration.UNCALIBRATED self.cal_status = Calibration.UNCALIBRATED
self.frame_counter = 0 self.write_counter = 0
self.params = Params() self.params = Params()
calibration_params = self.params.get("CalibrationParams") calibration_params = self.params.get("CalibrationParams")
if calibration_params: if calibration_params:
try: try:
calibration_params = json.loads(calibration_params) calibration_params = json.loads(calibration_params)
self.vp = np.array(calibration_params["vanishing_point"]) self.vp = np.array(calibration_params["vanishing_point"])
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID self.vps = np.tile(self.vp, (calibration_params['valid_points'], 1)).tolist()
self.vp_cycles = VP_CYCLES_NEEDED self.update_status()
self.frame_counter = CALIBRATION_CYCLES_NEEDED
except Exception: except Exception:
cloudlog.exception("CalibrationParams file found but error encountered") cloudlog.exception("CalibrationParams file found but error encountered")
self.vp_unfilt = self.vp
self.orb_last_updated = 0.
self.reset_grid()
def reset_grid(self): def update_status(self):
if self.cal_status == Calibration.UNCALIBRATED: if len(self.vps) < INPUTS_NEEDED:
# empty grid
self.grid = np.zeros((H+1, W+1), dtype=np.float)
else:
# gaussian grid, centered at vp
self.grid = gaussian_kernel(W/2., H/2., 16, 16, self.vp[0] - W/2., self.vp[1] - H/2.) * GRID_WEIGHT_INIT
def rescale_grid(self):
self.grid *= 0.9
def update(self, uvs, yaw_rate, speed):
if len(uvs) < 10 or \
abs(yaw_rate) > Filter.MAX_YAW_RATE or \
speed < Filter.MIN_SPEED:
return
rot_speeds = np.array([0.,0.,-yaw_rate])
uvs[:,1,:] = denormalize(correct_pts(normalize(uvs[:,1,:]), rot_speeds, self.dt))
# exclude tracks where:
# - pixel movement was less than 10 pixels
# - tracks are in the "hood region"
good_tracks = np.all([np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10,
uvs[:,0,1] < HOOD_HEIGHT,
uvs[:,1,1] < HOOD_HEIGHT], axis = 0)
uvs = uvs[good_tracks]
if uvs.shape[0] > MAX_LINES:
uvs = uvs[np.random.choice(uvs.shape[0], MAX_LINES, replace=False), :]
lines = get_lines(uvs)
increment_grid_c(self.grid, lines, len(lines))
self.frame_counter += 1
if (self.frame_counter % FRAMES_NEEDED) == 0:
grid = blur_image(self.grid, self.kernel_x, self.kernel_y)
argmax_vp = np.unravel_index(np.argmax(grid), grid.shape)[::-1]
self.rescale_grid()
self.vp_unfilt = np.array(argmax_vp)
self.vp_cycles += 1
if (self.vp_cycles - VP_CYCLES_NEEDED) % 10 == 0: # update file every 10
# skip rate_lim before writing the file to avoid writing a lagged vp
if self.cal_status != Calibration.CALIBRATED:
self.vp = self.vp_unfilt
if self.param_put:
cal_params = {"vanishing_point": list(self.vp), "angle_offset2": self.angle_offset}
self.params.put("CalibrationParams", json.dumps(cal_params))
return self.vp_unfilt
def parse_orb_features(self, log):
matches = np.array(log.orbFeatures.matches)
n = len(log.orbFeatures.matches)
t = float(log.orbFeatures.timestampLastEof)*1e-9
if t == 0 or n == 0:
return t, np.zeros((0,2,2))
orbs = denormalize(np.column_stack((log.orbFeatures.xs,
log.orbFeatures.ys)))
if self.prev_orbs is not None:
valid_matches = (matches > -1) & (matches < len(self.prev_orbs))
tracks = np.hstack((orbs[valid_matches], self.prev_orbs[matches[valid_matches]])).reshape((-1,2,2))
else:
tracks = np.zeros((0,2,2))
self.prev_orbs = orbs
return t, tracks
def update_vp(self):
# rate limit to not move VP too fast
self.vp = np.clip(self.vp_unfilt, self.vp - VP_RATE_LIM, self.vp + VP_RATE_LIM)
if self.vp_cycles < VP_CYCLES_NEEDED:
self.cal_status = Calibration.UNCALIBRATED self.cal_status = Calibration.UNCALIBRATED
else: else:
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
def handle_orb_features(self, log): def handle_cam_odom(self, log):
self.update_vp() trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot
self.time_orb, frame_raw = self.parse_orb_features(log) if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
self.dt = min(self.time_orb - self.orb_last_updated, 1.) new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
self.orb_last_updated = self.time_orb new_vp = new_vp[:2]/new_vp[2]
if self.time_orb - self.l100_last_updated < 1: self.vps.append(new_vp)
return self.update(frame_raw, self.yaw_rate, self.speed) self.vps = self.vps[-INPUTS_WANTED:]
self.vp = np.mean(self.vps, axis=0)
def handle_live100(self, log): self.update_status()
self.l100_last_updated = self.time_orb self.write_counter += 1
self.speed = log.live100.vEgo if self.param_put and self.write_counter % WRITE_CYCLES == 0:
self.angle_offset = log.live100.angleOffset cal_params = {"vanishing_point": list(self.vp),
self.yaw_rate = log.live100.curvature * self.speed "valid_points": len(self.vps)}
self.params.put("CalibrationParams", json.dumps(cal_params))
def handle_debug(self): return new_vp
grid_blurred = blur_image(self.grid, self.kernel_x, self.kernel_y)
grid_grey = np.clip(grid_blurred/(0.1 + np.max(grid_blurred))*255, 0, 255)
grid_color = np.repeat(grid_grey[:,:,np.newaxis], 3, axis=2)
grid_color[:,:,0] = 0
cv2.circle(grid_color, tuple(self.vp.astype(np.int64)), 2, (255, 255, 0), -1)
cv2.imshow("debug", grid_color.astype(np.uint8))
cv2.waitKey(1)
def send_data(self, livecalibration): def send_data(self, livecalibration):
calib = get_calib_from_vp(self.vp) calib = get_calib_from_vp(self.vp)
@ -214,7 +83,7 @@ class Calibrator(object):
cal_send = messaging.new_message() cal_send = messaging.new_message()
cal_send.init('liveCalibration') cal_send.init('liveCalibration')
cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100) cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 / INPUTS_NEEDED, 100)
cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten()) cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten())
cal_send.liveCalibration.warpMatrixBig = map(float, warp_matrix_big.flatten()) cal_send.liveCalibration.warpMatrixBig = map(float, warp_matrix_big.flatten())
cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten()) cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten())
@ -225,24 +94,17 @@ class Calibrator(object):
def calibrationd_thread(gctx=None, addr="127.0.0.1"): def calibrationd_thread(gctx=None, addr="127.0.0.1"):
context = zmq.Context() context = zmq.Context()
live100 = messaging.sub_sock(context, service_list['live100'].port, addr=addr, conflate=True) cameraodometry = messaging.sub_sock(context, service_list['cameraOdometry'].port, addr=addr, conflate=True)
orbfeatures = messaging.sub_sock(context, service_list['orbFeatures'].port, addr=addr, conflate=True)
livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port) livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port)
calibrator = Calibrator(param_put=True)
calibrator = Calibrator(param_put = True)
# buffer with all the messages that still need to be input into the kalman # buffer with all the messages that still need to be input into the kalman
while 1: while 1:
of = messaging.recv_one(orbfeatures) co = messaging.recv_one(cameraodometry)
l100 = messaging.recv_one_or_none(live100)
new_vp = calibrator.handle_orb_features(of) new_vp = calibrator.handle_cam_odom(co)
if DEBUG and new_vp is not None: if DEBUG and new_vp is not None:
print 'got new vp', new_vp print 'got new vp', new_vp
if l100 is not None:
calibrator.handle_live100(l100)
if DEBUG:
calibrator.handle_debug()
calibrator.send_data(livecalibration) calibrator.send_data(livecalibration)
@ -250,6 +112,6 @@ def calibrationd_thread(gctx=None, addr="127.0.0.1"):
def main(gctx=None, addr="127.0.0.1"): def main(gctx=None, addr="127.0.0.1"):
calibrationd_thread(gctx, addr) calibrationd_thread(gctx, addr)
if __name__ == "__main__": if __name__ == "__main__":
main() main()

@ -459,7 +459,7 @@ msg_types = {
UBloxDescriptor('RXM_RAW', '<dHbBB3B', [ UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]' 'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
], 'numMeas', '<ddfBBBBHBBBBBB', [ ], 'numMeas', '<ddfBBBBHBBBBBB', [
'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'reserved2', 'freqId', 'locktime', 'cno', 'prMes', 'cpMes', 'doMes', 'gnssId', 'svId', 'sigId', 'freqId', 'locktime', 'cno',
'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3' 'prStdev', 'cpStdev', 'doStdev', 'trkStat', 'reserved3'
]), ]),
(CLASS_RXM, MSG_RXM_SFRB): (CLASS_RXM, MSG_RXM_SFRB):

@ -195,6 +195,7 @@ def gen_raw(msg):
'halfCycleSubtracted': trackingStatus_bools[3]} 'halfCycleSubtracted': trackingStatus_bools[3]}
measurements_parsed.append({ measurements_parsed.append({
'svId': m['svId'], 'svId': m['svId'],
'sigId': m['sigId'],
'pseudorange': m['prMes'], 'pseudorange': m['prMes'],
'carrierCycles': m['cpMes'], 'carrierCycles': m['cpMes'],
'doppler': m['doMes'], 'doppler': m['doMes'],

Binary file not shown.

@ -136,7 +136,7 @@ class Uploader(object):
def next_file_to_compress(self): def next_file_to_compress(self):
for name, key, fn in self.gen_upload_files(): for name, key, fn in self.gen_upload_files():
if name == "rlog": if name.endswith("log"):
return (key, fn, 0) return (key, fn, 0)
return None return None

@ -89,6 +89,7 @@ managed_processes = {
"thermald": "selfdrive.thermald", "thermald": "selfdrive.thermald",
"uploader": "selfdrive.loggerd.uploader", "uploader": "selfdrive.loggerd.uploader",
"controlsd": "selfdrive.controls.controlsd", "controlsd": "selfdrive.controls.controlsd",
"plannerd": "selfdrive.controls.plannerd",
"radard": "selfdrive.controls.radard", "radard": "selfdrive.controls.radard",
"ubloxd": "selfdrive.locationd.ubloxd", "ubloxd": "selfdrive.locationd.ubloxd",
"mapd": "selfdrive.mapd.mapd", "mapd": "selfdrive.mapd.mapd",
@ -104,7 +105,6 @@ managed_processes = {
"visiond": ("selfdrive/visiond", ["./visiond"]), "visiond": ("selfdrive/visiond", ["./visiond"]),
"sensord": ("selfdrive/sensord", ["./sensord"]), "sensord": ("selfdrive/sensord", ["./sensord"]),
"gpsd": ("selfdrive/sensord", ["./gpsd"]), "gpsd": ("selfdrive/sensord", ["./gpsd"]),
"orbd": ("selfdrive/orbd", ["./orbd_wrapper.sh"]),
"updated": "selfdrive.updated", "updated": "selfdrive.updated",
} }
android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame") android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame")
@ -132,6 +132,7 @@ persistent_processes = [
car_started_processes = [ car_started_processes = [
'controlsd', 'controlsd',
'plannerd',
'loggerd', 'loggerd',
'sensord', 'sensord',
'radard', 'radard',
@ -139,7 +140,6 @@ car_started_processes = [
'visiond', 'visiond',
'proclogd', 'proclogd',
'ubloxd', 'ubloxd',
'orbd',
'mapd', 'mapd',
] ]
@ -452,6 +452,7 @@ def main():
del managed_processes['proclogd'] del managed_processes['proclogd']
if os.getenv("NOCONTROL") is not None: if os.getenv("NOCONTROL") is not None:
del managed_processes['controlsd'] del managed_processes['controlsd']
del managed_processes['plannerd']
del managed_processes['radard'] del managed_processes['radard']
# support additional internal only extensions # support additional internal only extensions

@ -0,0 +1,105 @@
{
"_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits Special cases have been stripped",
"AR:urban": "40",
"AR:urban:primary": "60",
"AR:urban:secondary": "60",
"AR:rural": "110",
"AT:urban": "50",
"AT:rural": "100",
"AT:trunk": "100",
"AT:motorway": "130",
"BE:urban": "50",
"BE-VLG:rural": "70",
"BE-WAL:rural": "90",
"BE:trunk": "120",
"BE:motorway": "120",
"CH:urban[1]": "50",
"CH:rural": "80",
"CH:trunk": "100",
"CH:motorway": "120",
"CZ:pedestrian_zone": "20",
"CZ:living_street": "20",
"CZ:urban": "50",
"CZ:urban_trunk": "80",
"CZ:urban_motorway": "80",
"CZ:rural": "90",
"CZ:trunk": "110",
"CZ:motorway": "130",
"DK:urban": "50",
"DK:rural": "80",
"DK:motorway": "130",
"DE:living_street": "7",
"DE:urban": "50",
"DE:rural": "100",
"DE:trunk": "none",
"DE:motorway": "none",
"FI:urban": "50",
"FI:rural": "80",
"FI:trunk": "100",
"FI:motorway": "120",
"FR:urban": "50",
"FR:rural": "80",
"FR:trunk": "110",
"FR:motorway": "130",
"GR:urban": "50",
"GR:rural": "90",
"GR:trunk": "110",
"GR:motorway": "130",
"HU:urban": "50",
"HU:rural": "90",
"HU:trunk": "110",
"HU:motorway": "130",
"IT:urban": "50",
"IT:rural": "90",
"IT:trunk": "110",
"IT:motorway": "130",
"JP:national": "60",
"JP:motorway": "100",
"LT:living_street": "20",
"LT:urban": "50",
"LT:rural": "90",
"LT:trunk": "120",
"LT:motorway": "130",
"PL:living_street": "20",
"PL:urban": "50",
"PL:rural": "90",
"PL:trunk": "100",
"PL:motorway": "140",
"RO:urban": "50",
"RO:rural": "90",
"RO:trunk": "100",
"RO:motorway": "130",
"RU:living_street": "20",
"RU:urban": "60",
"RU:rural": "90",
"RU:motorway": "110",
"SK:urban": "50",
"SK:rural": "90",
"SK:trunk": "90",
"SK:motorway": "90",
"SI:urban": "50",
"SI:rural": "90",
"SI:trunk": "110",
"SI:motorway": "130",
"ES:living_street": "20",
"ES:urban": "50",
"ES:rural": "50",
"ES:trunk": "90",
"ES:motorway": "120",
"SE:urban": "50",
"SE:rural": "70",
"SE:trunk": "90",
"SE:motorway": "110",
"GB:nsl_restricted": "30 mph",
"GB:nsl_single": "60 mph",
"GB:nsl_dual": "70 mph",
"GB:motorway": "70 mph",
"UA:urban": "50",
"UA:rural": "90",
"UA:trunk": "110",
"UA:motorway": "130",
"UZ:living_street": "30",
"UZ:urban": "70",
"UZ:rural": "100",
"UZ:motorway": "110"
}

@ -0,0 +1,454 @@
{
"CA": {
"Default": [
{
"speed": "100",
"tags": {
"highway": "motorway"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk"
}
},
{
"speed": "80",
"tags": {
"highway": "primary"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "80",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "40",
"tags": {
"highway": "residential"
}
},
{
"speed": "40",
"tags": {
"highway": "service"
}
},
{
"speed": "90",
"tags": {
"highway": "motorway_link"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "80",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary_link"
}
},
{
"speed": "20",
"tags": {
"highway": "living_street"
}
}
]
},
"DE": {
"Default": [
{
"speed": "none",
"tags": {
"highway": "motorway"
}
},
{
"speed": "10",
"tags": {
"highway": "living_street"
}
},
{
"speed": "100",
"tags": {
"zone:traffic": "DE:rural"
}
},
{
"speed": "50",
"tags": {
"zone:traffic": "DE:urban"
}
},
{
"speed": "30",
"tags": {
"bicycle_road": "yes"
}
}
]
},
"AU": {
"Default": [
{
"speed": "100",
"tags": {
"highway": "motorway"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk"
}
},
{
"speed": "80",
"tags": {
"highway": "primary"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "80",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "50",
"tags": {
"highway": "residential"
}
},
{
"speed": "40",
"tags": {
"highway": "service"
}
},
{
"speed": "90",
"tags": {
"highway": "motorway_link"
}
},
{
"speed": "80",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "80",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "50",
"tags": {
"highway": "tertiary_link"
}
},
{
"speed": "30",
"tags": {
"highway": "living_street"
}
}
]
},
"US": {
"South Dakota": [
{
"speed": "80 mph",
"tags": {
"highway": "motorway"
}
},
{
"speed": "70 mph",
"tags": {
"highway": "trunk"
}
},
{
"speed": "65 mph",
"tags": {
"highway": "primary"
}
},
{
"speed": "70 mph",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "65 mph",
"tags": {
"highway": "primary_link"
}
}
],
"Wisconsin": [
{
"speed": "65 mph",
"tags": {
"highway": "trunk"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "65 mph",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "tertiary_link"
}
}
],
"Default": [
{
"speed": "65 mph",
"tags": {
"highway": "motorway"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "trunk"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "primary"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "secondary"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "unclassified"
}
},
{
"speed": "25 mph",
"tags": {
"highway": "residential"
}
},
{
"speed": "25 mph",
"tags": {
"highway": "service"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "motorway_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "trunk_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "tertiary_link"
}
},
{
"speed": "15 mph",
"tags": {
"highway": "living_street"
}
}
],
"Michigan": [
{
"speed": "70 mph",
"tags": {
"highway": "motorway"
}
}
],
"Oregon": [
{
"speed": "55 mph",
"tags": {
"highway": "motorway"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "secondary"
}
},
{
"speed": "30 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "15 mph",
"tags": {
"highway": "service"
}
},
{
"speed": "35 mph",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "30 mph",
"tags": {
"highway": "tertiary_link"
}
}
],
"New York": [
{
"speed": "45 mph",
"tags": {
"highway": "primary"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "secondary"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "tertiary"
}
},
{
"speed": "30 mph",
"tags": {
"highway": "residential"
}
},
{
"speed": "45 mph",
"tags": {
"highway": "primary_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "secondary_link"
}
},
{
"speed": "55 mph",
"tags": {
"highway": "tertiary_link"
}
}
]
}
}

@ -0,0 +1,209 @@
#!/usr/bin/env python
import json
OUTPUT_FILENAME = "default_speeds_by_region.json"
def main():
countries = []
"""
--------------------------------------------------
US - United State of America
--------------------------------------------------
"""
US = Country("US") # First step, create the country using the ISO 3166 two letter code
countries.append(US) # Second step, add the country to countries list
""" Default rules """
# Third step, add some default rules for the country
# Speed limit rules are based on OpenStreetMaps (OSM) tags.
# The dictionary {...} defines the tag_name: value
# if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied.
# The text at the end is the speed limit (use no unit for km/h)
# Rules apply in the order in which they are written for each country
# Rules for specific regions (states) take priority over country rules
# If you modify existing country rules, you must update all existing states without that rule to use the old rule
US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to 65 mph
US.add_rule({"highway": "trunk"}, "55 mph")
US.add_rule({"highway": "primary"}, "55 mph")
US.add_rule({"highway": "secondary"}, "45 mph")
US.add_rule({"highway": "tertiary"}, "35 mph")
US.add_rule({"highway": "unclassified"}, "55 mph")
US.add_rule({"highway": "residential"}, "25 mph")
US.add_rule({"highway": "service"}, "25 mph")
US.add_rule({"highway": "motorway_link"}, "55 mph")
US.add_rule({"highway": "trunk_link"}, "55 mph")
US.add_rule({"highway": "primary_link"}, "55 mph")
US.add_rule({"highway": "secondary_link"}, "45 mph")
US.add_rule({"highway": "tertiary_link"}, "35 mph")
US.add_rule({"highway": "living_street"}, "15 mph")
""" States """
new_york = US.add_region("New York") # Fourth step, add a state/region to country
new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules
new_york.add_rule({"highway": "secondary"}, "55 mph")
new_york.add_rule({"highway": "tertiary"}, "55 mph")
new_york.add_rule({"highway": "residential"}, "30 mph")
new_york.add_rule({"highway": "primary_link"}, "45 mph")
new_york.add_rule({"highway": "secondary_link"}, "55 mph")
new_york.add_rule({"highway": "tertiary_link"}, "55 mph")
# All if not written by the state, the rules will default to the country rules
#california = US.add_region("California")
# California uses only the default US rules
michigan = US.add_region("Michigan")
michigan.add_rule({"highway": "motorway"}, "70 mph")
oregon = US.add_region("Oregon")
oregon.add_rule({"highway": "motorway"}, "55 mph")
oregon.add_rule({"highway": "secondary"}, "35 mph")
oregon.add_rule({"highway": "tertiary"}, "30 mph")
oregon.add_rule({"highway": "service"}, "15 mph")
oregon.add_rule({"highway": "secondary_link"}, "35 mph")
oregon.add_rule({"highway": "tertiary_link"}, "30 mph")
south_dakota = US.add_region("South Dakota")
south_dakota.add_rule({"highway": "motorway"}, "80 mph")
south_dakota.add_rule({"highway": "trunk"}, "70 mph")
south_dakota.add_rule({"highway": "primary"}, "65 mph")
south_dakota.add_rule({"highway": "trunk_link"}, "70 mph")
south_dakota.add_rule({"highway": "primary_link"}, "65 mph")
wisconsin = US.add_region("Wisconsin")
wisconsin.add_rule({"highway": "trunk"}, "65 mph")
wisconsin.add_rule({"highway": "tertiary"}, "45 mph")
wisconsin.add_rule({"highway": "unclassified"}, "35 mph")
wisconsin.add_rule({"highway": "trunk_link"}, "65 mph")
wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph")
"""
--------------------------------------------------
AU - Australia
--------------------------------------------------
"""
AU = Country("AU")
countries.append(AU)
""" Default rules """
AU.add_rule({"highway": "motorway"}, "100")
AU.add_rule({"highway": "trunk"}, "80")
AU.add_rule({"highway": "primary"}, "80")
AU.add_rule({"highway": "secondary"}, "50")
AU.add_rule({"highway": "tertiary"}, "50")
AU.add_rule({"highway": "unclassified"}, "80")
AU.add_rule({"highway": "residential"}, "50")
AU.add_rule({"highway": "service"}, "40")
AU.add_rule({"highway": "motorway_link"}, "90")
AU.add_rule({"highway": "trunk_link"}, "80")
AU.add_rule({"highway": "primary_link"}, "80")
AU.add_rule({"highway": "secondary_link"}, "50")
AU.add_rule({"highway": "tertiary_link"}, "50")
AU.add_rule({"highway": "living_street"}, "30")
"""
--------------------------------------------------
CA - Canada
--------------------------------------------------
"""
CA = Country("CA")
countries.append(CA)
""" Default rules """
CA.add_rule({"highway": "motorway"}, "100")
CA.add_rule({"highway": "trunk"}, "80")
CA.add_rule({"highway": "primary"}, "80")
CA.add_rule({"highway": "secondary"}, "50")
CA.add_rule({"highway": "tertiary"}, "50")
CA.add_rule({"highway": "unclassified"}, "80")
CA.add_rule({"highway": "residential"}, "40")
CA.add_rule({"highway": "service"}, "40")
CA.add_rule({"highway": "motorway_link"}, "90")
CA.add_rule({"highway": "trunk_link"}, "80")
CA.add_rule({"highway": "primary_link"}, "80")
CA.add_rule({"highway": "secondary_link"}, "50")
CA.add_rule({"highway": "tertiary_link"}, "50")
CA.add_rule({"highway": "living_street"}, "20")
"""
--------------------------------------------------
DE - Germany
--------------------------------------------------
"""
DE = Country("DE")
countries.append(DE)
""" Default rules """
DE.add_rule({"highway": "motorway"}, "none")
DE.add_rule({"highway": "living_street"}, "10")
DE.add_rule({"zone:traffic": "DE:rural"}, "100")
DE.add_rule({"zone:traffic": "DE:urban"}, "50")
DE.add_rule({"bicycle_road": "yes"}, "30")
""" --- DO NOT MODIFY CODE BELOW THIS LINE --- """
""" --- ADD YOUR COUNTRY OR STATE ABOVE --- """
# Final step
write_json(countries)
def write_json(countries):
out_dict = {}
for country in countries:
out_dict.update(country.jsonify())
json_string = json.dumps(out_dict, indent=2)
with open(OUTPUT_FILENAME, "wb") as f:
f.write(json_string)
class Region(object):
ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road"]
ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"]
def __init__(self, name):
self.name = name
self.rules = []
def add_rule(self, tag_conditions, speed):
new_rule = {}
if not isinstance(tag_conditions, dict):
raise TypeError("Rule tag conditions must be dictionary")
if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions):
raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS
if 'highway' in tag_conditions:
if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES:
raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"]))
new_rule['tags'] = tag_conditions
try:
new_rule['speed'] = str(speed)
except ValueError:
raise ValueError("Rule speed must be string")
self.rules.append(new_rule)
def jsonify(self):
ret_dict = {}
ret_dict[self.name] = self.rules
return ret_dict
class Country(Region):
ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"]
def __init__(self, ISO_3166_alpha_2):
Region.__init__(self, ISO_3166_alpha_2)
if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES:
raise ValueError("Not valid IOS 3166 country code")
self.regions = {}
def add_region(self, name):
self.regions[name] = Region(name)
return self.regions[name]
def jsonify(self):
ret_dict = {}
ret_dict[self.name] = {}
for r_name, region in self.regions.iteritems():
ret_dict[self.name].update(region.jsonify())
ret_dict[self.name]['Default'] = self.rules
return ret_dict
if __name__ == '__main__':
main()

@ -73,11 +73,14 @@ def setup_thread_excepthook():
def build_way_query(lat, lon, radius=50): def build_way_query(lat, lon, radius=50):
"""Builds a query to find all highways within a given radius around a point""" """Builds a query to find all highways within a given radius around a point"""
pos = " (around:%f,%f,%f)" % (radius, lat, lon) pos = " (around:%f,%f,%f)" % (radius, lat, lon)
lat_lon = "(%f,%f)" % (lat, lon)
q = """( q = """(
way way
""" + pos + """ """ + pos + """
[highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"]; [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"];
>;);out; >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"];
convert area ::id = id(), admin_level = t['admin_level'],
name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out;
""" """
return q return q
@ -97,7 +100,7 @@ def query_thread():
cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude)) 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)) prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude))
dist = np.linalg.norm(cur_ecef - prev_ecef) dist = np.linalg.norm(cur_ecef - prev_ecef)
if dist < 1000: if dist < 1000: #updated when we are 1km from the edge of the downloaded circle
continue continue
if dist > 3000: if dist > 3000:
@ -111,6 +114,7 @@ def query_thread():
nodes = [] nodes = []
real_nodes = [] real_nodes = []
node_to_way = defaultdict(list) node_to_way = defaultdict(list)
location_info = {}
for n in new_result.nodes: for n in new_result.nodes:
nodes.append((float(n.lat), float(n.lon), 0)) nodes.append((float(n.lat), float(n.lon), 0))
@ -120,12 +124,18 @@ def query_thread():
for n in way.nodes: for n in way.nodes:
node_to_way[n.id].append(way) node_to_way[n.id].append(way)
for area in new_result.areas:
if area.tags.get('admin_level', '') == "2":
location_info['country'] = area.tags.get('ISO3166-1:alpha2', '')
if area.tags.get('admin_level', '') == "4":
location_info['region'] = area.tags.get('name', '')
nodes = np.asarray(nodes) nodes = np.asarray(nodes)
nodes = geodetic2ecef(nodes) nodes = geodetic2ecef(nodes)
tree = spatial.cKDTree(nodes) tree = spatial.cKDTree(nodes)
query_lock.acquire() query_lock.acquire()
last_query_result = new_result, tree, real_nodes, node_to_way last_query_result = new_result, tree, real_nodes, node_to_way, location_info
last_query_pos = last_gps last_query_pos = last_gps
cache_valid = True cache_valid = True
query_lock.release() query_lock.release()
@ -182,7 +192,7 @@ def mapsd_thread():
query_lock.acquire() query_lock.acquire()
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way) cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
if cur_way is not None: if cur_way is not None:
pnts, curvature_valid = cur_way.get_lookahead(last_query_result, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE) pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
xs = pnts[:, 0] xs = pnts[:, 0]
ys = pnts[:, 1] ys = pnts[:, 1]
@ -251,11 +261,24 @@ def mapsd_thread():
dat.liveMapData.wayId = cur_way.id dat.liveMapData.wayId = cur_way.id
# Seed limit # Seed limit
max_speed = cur_way.max_speed max_speed = cur_way.max_speed()
if max_speed is not None: if max_speed is not None:
dat.liveMapData.speedLimitValid = True dat.liveMapData.speedLimitValid = True
dat.liveMapData.speedLimit = max_speed dat.liveMapData.speedLimit = max_speed
# TODO: use the function below to anticipate upcoming speed limits
#max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
#if max_speed_ahead is not None and max_speed_ahead_dist is not None:
# dat.liveMapData.speedLimitAheadValid = True
# dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
# dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
advisory_max_speed = cur_way.advisory_max_speed()
if advisory_max_speed is not None:
dat.liveMapData.speedAdvisoryValid = True
dat.liveMapData.speedAdvisory = advisory_max_speed
# Curvature # Curvature
dat.liveMapData.curvatureValid = curvature_valid dat.liveMapData.curvatureValid = curvature_valid
dat.liveMapData.curvature = float(upcoming_curvature) dat.liveMapData.curvature = float(upcoming_curvature)

@ -1,13 +1,23 @@
import math import math
import json
import numpy as np import numpy as np
from datetime import datetime from datetime import datetime
from common.basedir import BASEDIR
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from common.transformations.coordinates import LocalCoord, geodetic2ecef from common.transformations.coordinates import LocalCoord, geodetic2ecef
LOOKAHEAD_TIME = 10. LOOKAHEAD_TIME = 10.
MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME
DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json"
DEFAULT_SPEEDS = {}
with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f:
DEFAULT_SPEEDS = json.loads(f.read())
DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
DEFAULT_SPEEDS_BY_REGION = {}
with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f:
DEFAULT_SPEEDS_BY_REGION = json.loads(f.read())
def circle_through_points(p1, p2, p3): def circle_through_points(p1, p2, p3):
"""Fits a circle through three points """Fits a circle through three points
@ -23,28 +33,90 @@ def circle_through_points(p1, p2, p3):
return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2))) 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): def parse_speed_unit(max_speed):
"""Converts a maxspeed string to m/s based on the unit present in the input. """Converts a maxspeed string to m/s based on the unit present in the input.
OpenStreetMap defaults to kph if no unit is present. """ OpenStreetMap defaults to kph if no unit is present. """
if not max_speed:
return None
conversion = CV.KPH_TO_MS conversion = CV.KPH_TO_MS
if 'mph' in max_speed: if 'mph' in max_speed:
max_speed = max_speed.replace(' mph', '') max_speed = max_speed.replace(' mph', '')
conversion = CV.MPH_TO_MS conversion = CV.MPH_TO_MS
try: try:
max_speed = float(max_speed) * conversion return float(max_speed) * conversion
except ValueError: except ValueError:
max_speed = None return None
def parse_speed_tags(tags):
"""Parses tags on a way to find the maxspeed string"""
max_speed = None
if 'maxspeed' in tags:
max_speed = tags['maxspeed']
if 'maxspeed:conditional' in tags:
try:
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 = max_speed_cond
except ValueError:
pass
if not max_speed and 'source:maxspeed' in tags:
max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None)
if not max_speed and 'maxspeed:type' in tags:
max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None)
max_speed = parse_speed_unit(max_speed)
return max_speed return max_speed
def geocode_maxspeed(tags, location_info):
max_speed = None
try:
geocode_country = location_info.get('country', '')
geocode_region = location_info.get('region', '')
country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {})
country_defaults = country_rules.get('Default', [])
for rule in country_defaults:
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in rule['tags'].iteritems()
)
if rule_valid:
max_speed = rule['speed']
break #stop searching country
region_rules = country_rules.get(geocode_region, [])
for rule in region_rules:
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in rule['tags'].iteritems()
)
if rule_valid:
max_speed = rule['speed']
break #stop searching region
except KeyError:
pass
max_speed = parse_speed_unit(max_speed)
return max_speed
class Way: class Way:
def __init__(self, way): def __init__(self, way, query_results):
self.id = way.id self.id = way.id
self.way = way self.way = way
self.query_results = query_results
points = list() points = list()
@ -55,7 +127,7 @@ class Way:
@classmethod @classmethod
def closest(cls, query_results, lat, lon, heading, prev_way=None): def closest(cls, query_results, lat, lon, heading, prev_way=None):
results, tree, real_nodes, node_to_way = query_results results, tree, real_nodes, node_to_way, location_info = query_results
cur_pos = geodetic2ecef((lat, lon, 0)) cur_pos = geodetic2ecef((lat, lon, 0))
nodes = tree.query_ball_point(cur_pos, 500) nodes = tree.query_ball_point(cur_pos, 500)
@ -73,7 +145,7 @@ class Way:
closest_way = None closest_way = None
best_score = None best_score = None
for way in ways: for way in ways:
way = Way(way) way = Way(way, query_results)
points = way.points_in_car_frame(lat, lon, heading) points = way.points_in_car_frame(lat, lon, heading)
on_way = way.on_way(lat, lon, heading, points) on_way = way.on_way(lat, lon, heading, points)
@ -124,34 +196,64 @@ class Way:
def __str__(self): def __str__(self):
return "%s %s" % (self.id, self.way.tags) return "%s %s" % (self.id, self.way.tags)
@property
def max_speed(self): def max_speed(self):
"""Extracts the (conditional) speed limit from a way""" """Extracts the (conditional) speed limit from a way"""
if not self.way: if not self.way:
return None return None
tags = self.way.tags max_speed = parse_speed_tags(self.way.tags)
max_speed = None if not max_speed:
location_info = self.query_results[4]
max_speed = geocode_maxspeed(self.way.tags, location_info)
if 'maxspeed' in tags: return max_speed
max_speed = parse_speed_unit(tags['maxspeed'])
try: def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
if 'maxspeed:conditional' in tags: """Look ahead for a max speed"""
max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ') if not self.way:
cond = cond[1:-1] return None
start, end = cond.split('-') speed_ahead = None
now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays speed_ahead_dist = None
start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day) lookahead_ways = 5
end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day) way = self
for i in range(lookahead_ways):
way_pts = way.points_in_car_frame(lat, lon, heading)
if start <= now <= end: # Check current lookahead distance
max_speed = parse_speed_unit(max_speed_cond) max_dist = np.linalg.norm(way_pts[-1, :])
except ValueError:
pass
return max_speed if max_dist > 2 * lookahead:
break
if 'maxspeed' in way.way.tags:
spd = parse_speed_tags(way.way.tags)
if not spd:
location_info = self.query_results[4]
spd = geocode_maxspeed(way.way.tags, location_info)
if spd < current_speed_limit:
speed_ahead = spd
min_dist = np.linalg.norm(way_pts[1, :])
speed_ahead_dist = min_dist
break
# Find next way
way = way.next_way()
if not way:
break
return speed_ahead, speed_ahead_dist
def advisory_max_speed(self):
if not self.way:
return None
tags = self.way.tags
adv_speed = None
if 'maxspeed:advisory' in tags:
adv_speed = tags['maxspeed:advisory']
adv_speed = parse_speed_unit(adv_speed)
return adv_speed
def on_way(self, lat, lon, heading, points=None): def on_way(self, lat, lon, heading, points=None):
if points is None: if points is None:
@ -186,8 +288,8 @@ class Way:
return points_carframe return points_carframe
def next_way(self, query_results, lat, lon, heading, backwards=False): def next_way(self, backwards=False):
results, tree, real_nodes, node_to_way = query_results results, tree, real_nodes, node_to_way, location_info = self.query_results
if backwards: if backwards:
node = self.way.nodes[0] node = self.way.nodes[0]
@ -215,18 +317,20 @@ class Way:
# Filter on number of lanes # Filter on number of lanes
cur_num_lanes = int(self.way.tags['lanes']) cur_num_lanes = int(self.way.tags['lanes'])
if len(ways) > 1: if len(ways) > 1:
ways = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes] ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes]
if len(ways_same_lanes) == 1:
ways = ways_same_lanes
if len(ways) > 1: if len(ways) > 1:
ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes] ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes]
if len(ways) == 1: if len(ways) == 1:
way = Way(ways[0]) way = Way(ways[0], self.query_results)
except (KeyError, ValueError): except (KeyError, ValueError):
pass pass
return way return way
def get_lookahead(self, query_results, lat, lon, heading, lookahead): def get_lookahead(self, lat, lon, heading, lookahead):
pnts = None pnts = None
way = self way = self
valid = False valid = False
@ -249,7 +353,7 @@ class Way:
break break
# Find next way # Find next way
way = way.next_way(query_results, lat, lon, heading) way = way.next_way()
if not way: if not way:
break break

Binary file not shown.

Binary file not shown.

@ -72,6 +72,9 @@ orbFeaturesSummary: [8062, true]
driverMonitoring: [8063, true] driverMonitoring: [8063, true]
liveParameters: [8064, true] liveParameters: [8064, true]
liveMapData: [8065, true] liveMapData: [8065, true]
cameraOdometry: [8066, true]
pathPlan: [8067, true]
kalmanOdometry: [8068, true]
testModel: [8040, false] testModel: [8040, false]
testLiveLocation: [8045, false] testLiveLocation: [8045, false]

@ -293,14 +293,17 @@ class LongitudinalControl(unittest.TestCase):
manager.gctx = {} manager.gctx = {}
manager.prepare_managed_process('radard') manager.prepare_managed_process('radard')
manager.prepare_managed_process('controlsd') manager.prepare_managed_process('controlsd')
manager.prepare_managed_process('plannerd')
manager.start_managed_process('radard') manager.start_managed_process('radard')
manager.start_managed_process('controlsd') manager.start_managed_process('controlsd')
manager.start_managed_process('plannerd')
@classmethod @classmethod
def tearDownClass(cls): def tearDownClass(cls):
manager.kill_managed_process('radard') manager.kill_managed_process('radard')
manager.kill_managed_process('controlsd') manager.kill_managed_process('controlsd')
manager.kill_managed_process('plannerd')
time.sleep(5) time.sleep(5)
# hack # hack
@ -321,4 +324,3 @@ for k in xrange(WORKERS):
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()

@ -97,7 +97,7 @@ else
OPENGL_LIBS = -lGLESv3 -lEGL OPENGL_LIBS = -lGLESv3 -lEGL
SNPE_FLAGS = -I$(PHONELIBS)/snpe/include/ SNPE_FLAGS = -I$(PHONELIBS)/snpe/include/
SNPE_LIBS = -L$(PHONELIBS)/snpe/lib -lSNPE -lsymphony-cpu -lsymphonypower SNPE_LIBS = -lSNPE -lsymphony-cpu -lsymphonypower
OTHER_LIBS = -lz -lcutils -lm -llog -lui -ladreno_utils OTHER_LIBS = -lz -lcutils -lm -llog -lui -ladreno_utils
@ -137,7 +137,7 @@ OBJS += $(PLATFORM_OBJS) \
$(CEREAL_OBJS) $(CEREAL_OBJS)
#MODEL_DATA = ../../models/driving_bigmodel.dlc ../../models/monitoring_model.dlc #MODEL_DATA = ../../models/driving_bigmodel.dlc ../../models/monitoring_model.dlc
MODEL_DATA = ../../models/driving_model.dlc ../../models/monitoring_model.dlc MODEL_DATA = ../../models/driving_model.dlc ../../models/monitoring_model.dlc ../../models/posenet.dlc
MODEL_OBJS = $(MODEL_DATA:.dlc=.o) MODEL_OBJS = $(MODEL_DATA:.dlc=.o)
OBJS += $(MODEL_OBJS) OBJS += $(MODEL_OBJS)

@ -49,6 +49,8 @@
#include "cereal/gen/cpp/log.capnp.h" #include "cereal/gen/cpp/log.capnp.h"
#define M_PI 3.14159265358979323846
#define UI_BUF_COUNT 4 #define UI_BUF_COUNT 4
//#define DUMP_RGB //#define DUMP_RGB
@ -166,6 +168,9 @@ struct VisionState {
zsock_t *recorder_sock; zsock_t *recorder_sock;
void* recorder_sock_raw; void* recorder_sock_raw;
zsock_t *posenet_sock;
void* posenet_sock_raw;
pthread_mutex_t clients_lock; pthread_mutex_t clients_lock;
VisionClientState clients[MAX_CLIENTS]; VisionClientState clients[MAX_CLIENTS];
@ -894,6 +899,15 @@ void* frontview_thread(void *arg) {
return NULL; return NULL;
} }
#define POSENET
#ifdef POSENET
#include "snpemodel.h"
extern const uint8_t posenet_model_data[] asm("_binary_posenet_dlc_start");
extern const uint8_t posenet_model_end[] asm("_binary_posenet_dlc_end");
const size_t posenet_model_size = posenet_model_end - posenet_model_data;
#endif
void* processing_thread(void *arg) { void* processing_thread(void *arg) {
int err; int err;
VisionState *s = (VisionState*)arg; VisionState *s = (VisionState*)arg;
@ -924,6 +938,14 @@ void* processing_thread(void *arg) {
FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb"); FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb");
#endif #endif
#ifdef POSENET
int posenet_counter = 0;
float pose_output[12];
float *posenet_input = (float*)malloc(2*200*532*sizeof(float));
SNPEModel *posenet = new SNPEModel(posenet_model_data, posenet_model_size,
pose_output, sizeof(pose_output)/sizeof(float));
#endif
// init the net // init the net
LOG("processing start!"); LOG("processing start!");
@ -947,11 +969,6 @@ void* processing_thread(void *arg) {
int ui_idx = tbuffer_select(&s->ui_tb); int ui_idx = tbuffer_select(&s->ui_tb);
int rgb_idx = ui_idx; int rgb_idx = ui_idx;
// printf("idx %d\n", rgb_idx);
/*FILE *f = fopen("/tmp/test_dump", "wb");
fwrite(s->camera_bufs[buf_idx].addr, 1, s->camera_bufs[buf_idx].len, f);
fclose(f);*/
cl_event debayer_event; cl_event debayer_event;
if (s->cameras.rear.ci.bayer) { if (s->cameras.rear.ci.bayer) {
@ -1069,6 +1086,89 @@ void* processing_thread(void *arg) {
} }
#ifdef POSENET
double pt1 = 0, pt2 = 0, pt3 = 0;
pt1 = millis_since_boot();
// move second frame to first frame
memmove(&posenet_input[0], &posenet_input[1], sizeof(float)*(200*532*2 - 1));
// fill posenet input
float a;
// posenet uses a half resolution cropped frame
// with upper left corner: [50, 237] and
// bottom right corner: [1114, 637]
// So the resulting crop is 532 X 200
for (int y=237; y<637; y+=2) {
int yy = (y-237)/2;
for (int x = 50; x < 1114; x+=2) {
int xx = (x-50)/2;
a = 0;
a += yuv_ptr_y[s->yuv_width*(y+0) + (x+1)];
a += yuv_ptr_y[s->yuv_width*(y+1) + (x+1)];
a += yuv_ptr_y[s->yuv_width*(y+0) + (x+0)];
a += yuv_ptr_y[s->yuv_width*(y+1) + (x+0)];
// The posenet takes a normalized image input
// like the driving model so [0,255] is remapped
// to [-1,1]
posenet_input[(yy*532+xx)*2 + 1] = (a/512.0 - 1.0);
}
}
//FILE *fp;
//fp = fopen( "testing" , "r" );
//fread(posenet_input , sizeof(float) , 200*532*2 , fp);
//fclose(fp);
//sleep(5);
pt2 = millis_since_boot();
posenet_counter++;
if (posenet_counter % 5 == 0){
// run posenet
//printf("avg %f\n", pose_output[0]);
posenet->execute(posenet_input);
// fix stddevs
for (int i = 6; i < 12; i++) {
pose_output[i] = log1p(exp(pose_output[i])) + 1e-6;
}
// to radians
for (int i = 3; i < 6; i++) {
pose_output[i] = M_PI * pose_output[i] / 180.0;
}
// to radians
for (int i = 9; i < 12; i++) {
pose_output[i] = M_PI * pose_output[i] / 180.0;
}
// send posenet event
{
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto posenetd = event.initCameraOdometry();
kj::ArrayPtr<const float> trans_vs(&pose_output[0], 3);
posenetd.setTrans(trans_vs);
kj::ArrayPtr<const float> rot_vs(&pose_output[3], 3);
posenetd.setRot(rot_vs);
kj::ArrayPtr<const float> trans_std_vs(&pose_output[6], 3);
posenetd.setTransStd(trans_std_vs);
kj::ArrayPtr<const float> rot_std_vs(&pose_output[9], 3);
posenetd.setRotStd(rot_std_vs);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s->posenet_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
}
pt3 = millis_since_boot();
LOGD("pre: %.2fms | posenet: %.2fms", (pt2-pt1), (pt3-pt1));
}
#endif
tbuffer_dispatch(&s->ui_tb, ui_idx); tbuffer_dispatch(&s->ui_tb, ui_idx);
// auto exposure over big box // auto exposure over big box
@ -1319,6 +1419,10 @@ int main(int argc, char **argv) {
assert(s->monitoring_sock); assert(s->monitoring_sock);
s->monitoring_sock_raw = zsock_resolve(s->monitoring_sock); s->monitoring_sock_raw = zsock_resolve(s->monitoring_sock);
s->posenet_sock = zsock_new_pub("@tcp://*:8066");
assert(s->posenet_sock);
s->posenet_sock_raw = zsock_resolve(s->posenet_sock);
cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]); cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]);
if (test_run) { if (test_run) {

Loading…
Cancel
Save