openpilot v0.5.9 release
old-commit-hash: 0207a970400ee28d3e366f2e8f5c551281accf02
This commit is contained in:
Vehicle Researcher
parent
34c9a4743d
commit
2cc4edde68
10
README.md
10
README.md
|
|
@ -62,6 +62,7 @@ Supported Cars
|
|||
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
|
||||
| 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 |
|
||||
| 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>| 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>|
|
||||
|
|
@ -81,19 +82,20 @@ Supported Cars
|
|||
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Elantra 2017 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>6</sup>|
|
||||
| Kia | 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 | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
|
||||
| Lexus | RX Hybrid 2016-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-18<sup>4</sup> | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | 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 Hybrid 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius Prime 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 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<sup>1</sup>| 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)***
|
||||
|
|
@ -166,7 +168,7 @@ Directory structure
|
|||
├── sensord # IMU / GPS interface code
|
||||
├── test # Car simulator running code through virtual maneuvers
|
||||
├── ui # The UI
|
||||
└── visiond # Embedded vision pipeline
|
||||
└── visiond # Vision pipeline
|
||||
|
||||
To understand how the services interact, see `selfdrive/service_list.yaml`
|
||||
|
||||
|
|
|
|||
13
RELEASES.md
13
RELEASES.md
|
|
@ -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)
|
||||
========================
|
||||
* Open sourced visiond
|
||||
* Auto-slowdown for upcoming turns
|
||||
* Chrysler/Jeep/Fiat support thanks to adhintz!
|
||||
* 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
|
||||
* EON stops charging when 12V battery drops below 11.8V
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:b8851680f5bd393fa23868a7d4f3911083280e9bc7ae4a5dbe251ba8dfa21a11
|
||||
size 2586396
|
||||
oid sha256:2d45eb035af4adeaf31fe96ab36fc310962cc8c375319ac2a5e707b2cdc03097
|
||||
size 2588994
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:9dd7d3fa39ebf435b467c4bc8ced9e51c440f1cc7c09089c65fdcd3d1a92fa3d
|
||||
size 18387934
|
||||
oid sha256:94b4a6bc1f7fc720b9f638268ff2345368164efd8a4710616b3e9ad5fd473fcf
|
||||
size 18388501
|
||||
|
|
|
|||
|
|
@ -374,6 +374,7 @@ struct Live100Data {
|
|||
l20MonoTimeDEPRECATED @17 :UInt64;
|
||||
mdMonoTimeDEPRECATED @18 :UInt64;
|
||||
planMonoTime @28 :UInt64;
|
||||
pathPlanMonoTime @50 :UInt64;
|
||||
|
||||
state @31 :ControlState;
|
||||
vEgo @0 :Float32;
|
||||
|
|
@ -401,6 +402,7 @@ struct Live100Data {
|
|||
cumLagMs @15 :Float32;
|
||||
startMonoTime @48 :UInt64;
|
||||
mapValid @49 :Bool;
|
||||
forceDecel @51 :Bool;
|
||||
|
||||
enabled @19 :Bool;
|
||||
active @36 :Bool;
|
||||
|
|
@ -546,12 +548,12 @@ struct Plan {
|
|||
events @13 :List(Car.CarEvent);
|
||||
|
||||
# lateral, 3rd order polynomial
|
||||
lateralValid @0 :Bool;
|
||||
dPoly @1 :List(Float32);
|
||||
laneWidth @11 :Float32;
|
||||
lateralValidDEPRECATED @0 :Bool;
|
||||
dPolyDEPRECATED @1 :List(Float32);
|
||||
laneWidthDEPRECATED @11 :Float32;
|
||||
|
||||
# longitudinal
|
||||
longitudinalValid @2 :Bool;
|
||||
longitudinalValidDEPRECATED @2 :Bool;
|
||||
vCruise @16 :Float32;
|
||||
aCruise @17 :Float32;
|
||||
vTarget @3 :Float32;
|
||||
|
|
@ -560,10 +562,14 @@ struct Plan {
|
|||
aTargetMinDEPRECATED @4 :Float32;
|
||||
aTargetMaxDEPRECATED @5 :Float32;
|
||||
aTarget @18 :Float32;
|
||||
|
||||
vStart @26 :Float32;
|
||||
aStart @27 :Float32;
|
||||
|
||||
jerkFactor @6 :Float32;
|
||||
hasLead @7 :Bool;
|
||||
hasLeftLane @23 :Bool;
|
||||
hasRightLane @24 :Bool;
|
||||
hasLeftLaneDEPRECATED @23 :Bool;
|
||||
hasRightLaneDEPRECATED @24 :Bool;
|
||||
fcw @8 :Bool;
|
||||
longitudinalPlanSource @15 :LongitudinalPlanSource;
|
||||
|
||||
|
|
@ -577,6 +583,7 @@ struct Plan {
|
|||
decelForTurn @22 :Bool;
|
||||
mapValid @25 :Bool;
|
||||
|
||||
|
||||
struct GpsTrajectory {
|
||||
x @0 :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 {
|
||||
status @0 :UInt8;
|
||||
|
||||
|
|
@ -1276,6 +1298,7 @@ struct UbloxGnss {
|
|||
carrierPhaseStdev @10 :Float32;
|
||||
# doppler standard deviation in Hz
|
||||
dopplerStdev @11 :Float32;
|
||||
sigId @12 :UInt8;
|
||||
|
||||
struct TrackingStatus {
|
||||
# pseudorange valid
|
||||
|
|
@ -1584,6 +1607,11 @@ struct LiveParametersData {
|
|||
struct LiveMapData {
|
||||
speedLimitValid @0 :Bool;
|
||||
speedLimit @1 :Float32;
|
||||
speedAdvisoryValid @12 :Bool;
|
||||
speedAdvisory @13 :Float32;
|
||||
speedLimitAheadValid @14 :Bool;
|
||||
speedLimitAhead @15 :Float32;
|
||||
speedLimitAheadDistance @16 :Float32;
|
||||
curvatureValid @2 :Bool;
|
||||
curvature @3 :Float32;
|
||||
wayId @4 :UInt64;
|
||||
|
|
@ -1603,6 +1631,13 @@ struct CameraOdometry {
|
|||
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 {
|
||||
# in nanoseconds?
|
||||
logMonoTime @0 :UInt64;
|
||||
|
|
@ -1671,5 +1706,7 @@ struct Event {
|
|||
liveParameters @61 :LiveParametersData;
|
||||
liveMapData @62 :LiveMapData;
|
||||
cameraOdometry @63 :CameraOdometry;
|
||||
pathPlan @64 :PathPlan;
|
||||
kalmanOdometry @65 :KalmanOdometry;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@ keys = {
|
|||
# written: visiond
|
||||
# read: visiond, controlsd
|
||||
"CalibrationParams": TxType.PERSISTENT,
|
||||
"ControlsParams": TxType.PERSISTENT,
|
||||
# written: controlsd
|
||||
# read: radard
|
||||
"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):
|
||||
vp_norm = normalize(vp)
|
||||
yaw_calib = np.arctan(vp_norm[0])
|
||||
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))
|
||||
pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
|
||||
roll_calib = 0
|
||||
return roll_calib, pitch_calib, yaw_calib
|
||||
|
||||
# aka 'extrinsic_matrix'
|
||||
# road : x->forward, y -> left, z->up
|
||||
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]))
|
||||
view_from_road = view_frame_from_device_frame.dot(device_from_road)
|
||||
return np.hstack((view_from_road, [[0], [height], [0]]))
|
||||
|
|
|
|||
|
|
@ -0,0 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:23a8d66c0b1de9c29f1cd9a74beeba9a67fd6e27392a9971f9771cc387125061
|
||||
size 5303349
|
||||
|
|
@ -15,7 +15,7 @@ cffi==1.11.5
|
|||
contextlib2==0.5.4
|
||||
crc16==0.1.1
|
||||
crcmod==1.7
|
||||
cryptography==1.4
|
||||
cryptography==2.1.4
|
||||
cycler==0.10.0
|
||||
decorator==4.0.10
|
||||
docopt==0.6.2
|
||||
|
|
@ -31,7 +31,7 @@ nose==1.3.7
|
|||
numpy==1.11.1
|
||||
pause==0.1.2
|
||||
py==1.4.31
|
||||
pyOpenSSL==16.0.0
|
||||
pyOpenSSL==17.5.0
|
||||
pyasn1-modules==0.0.8
|
||||
pyasn1==0.1.9
|
||||
pycapnp==0.6.3
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!/usr/bin/env python
|
||||
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.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
|
@ -256,7 +256,7 @@ class CarInterface(object):
|
|||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c, perception_state=log.Live20Data.new_message()):
|
||||
def apply(self, c):
|
||||
|
||||
if (self.CS.frame == -1):
|
||||
return False # if we haven't seen a frame 220, then do not update.
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!/usr/bin/env python
|
||||
from common.realtime import sec_since_boot
|
||||
from cereal import car, log
|
||||
from cereal import car
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
|
|
@ -210,7 +210,7 @@ class CarInterface(object):
|
|||
|
||||
# pass in a car.CarControl
|
||||
# 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,
|
||||
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.car import apply_std_steer_torque_limits
|
||||
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
|
||||
|
||||
|
||||
class CarControllerParams():
|
||||
def __init__(self, car_fingerprint):
|
||||
if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
|
||||
self.STEER_MAX = 300
|
||||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s)
|
||||
self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero
|
||||
elif car_fingerprint == CAR.CADILLAC_CT6:
|
||||
if car_fingerprint in SUPERCRUISE_CARS:
|
||||
self.STEER_MAX = 150
|
||||
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_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_MULTIPLIER = 4 # weight driver torque heavily
|
||||
|
|
@ -94,7 +96,7 @@ class CarController(object):
|
|||
### STEER ###
|
||||
|
||||
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:
|
||||
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)
|
||||
|
|
@ -104,16 +106,16 @@ class CarController(object):
|
|||
self.apply_steer_last = apply_steer
|
||||
idx = (frame / P.STEER_STEP) % 4
|
||||
|
||||
if self.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt,
|
||||
canbus.powertrain, apply_steer, idx, lkas_enabled))
|
||||
if self.car_fingerprint == CAR.CADILLAC_CT6:
|
||||
if self.car_fingerprint in SUPERCRUISE_CARS:
|
||||
can_sends += gmcan.create_steering_control_ct6(self.packer_pt,
|
||||
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 ###
|
||||
|
||||
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
|
||||
# treat pedals as one
|
||||
final_pedal = actuators.gas - actuators.brake
|
||||
|
|
@ -164,17 +166,17 @@ class CarController(object):
|
|||
if frame % P.ADAS_KEEPALIVE_STEP == 0:
|
||||
can_sends += gmcan.create_adas_keepalive(canbus.powertrain)
|
||||
|
||||
# Show green icon when LKA torque is applied, and
|
||||
# alarming orange icon when approaching torque limit.
|
||||
# If not sent again, LKA icon disappears in about 5 seconds.
|
||||
# Conveniently, sending camera message periodically also works as a keepalive.
|
||||
lka_active = CS.lkas_status == 1
|
||||
lka_critical = lka_active and abs(actuators.steer) > 0.9
|
||||
lka_icon_status = (lka_active, lka_critical)
|
||||
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
|
||||
or lka_icon_status != self.lka_icon_status_last:
|
||||
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
|
||||
self.lka_icon_status_last = lka_icon_status
|
||||
# Show green icon when LKA torque is applied, and
|
||||
# alarming orange icon when approaching torque limit.
|
||||
# If not sent again, LKA icon disappears in about 5 seconds.
|
||||
# Conveniently, sending camera message periodically also works as a keepalive.
|
||||
lka_active = CS.lkas_status == 1
|
||||
lka_critical = lka_active and abs(actuators.steer) > 0.9
|
||||
lka_icon_status = (lka_active, lka_critical)
|
||||
if frame % P.CAMERA_KEEPALIVE_STEP == 0 \
|
||||
or lka_icon_status != self.lka_icon_status_last:
|
||||
can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical))
|
||||
self.lka_icon_status_last = lka_icon_status
|
||||
|
||||
# Send chimes
|
||||
if self.chime != chime:
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV
|
|||
from selfdrive.can.parser import CANParser
|
||||
from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \
|
||||
CruiseButtons, is_eps_status_ok, \
|
||||
STEER_THRESHOLD
|
||||
STEER_THRESHOLD, SUPERCRUISE_CARS
|
||||
|
||||
def get_powertrain_can_parser(CP, canbus):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
|
|
@ -35,17 +35,17 @@ def get_powertrain_can_parser(CP, canbus):
|
|||
signals += [
|
||||
("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 += [
|
||||
("TractionControlOn", "ESPStatus", 0),
|
||||
("EPBClosed", "EPBStatus", 0),
|
||||
("CruiseMainOn", "ECMEngineStatus", 0),
|
||||
("CruiseState", "AcceleratorPedal2", 0),
|
||||
]
|
||||
if CP.carFingerprint == CAR.CADILLAC_CT6:
|
||||
signals += [
|
||||
("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0)
|
||||
]
|
||||
|
||||
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.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.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
|
||||
self.acc_active = False
|
||||
|
|
@ -131,13 +138,6 @@ class CarState(object):
|
|||
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
|
||||
else:
|
||||
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
|
||||
# even when pedal is not pressed.
|
||||
|
|
|
|||
|
|
@ -1,10 +1,10 @@
|
|||
#!/usr/bin/env python
|
||||
from cereal import car, log
|
||||
from cereal import car
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
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
|
||||
|
||||
try:
|
||||
|
|
@ -107,6 +107,15 @@ class CarInterface(object):
|
|||
ret.steerRatioRear = 0.
|
||||
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:
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.mass = 1601 + std_cargo
|
||||
|
|
@ -285,7 +294,13 @@ class CarInterface(object):
|
|||
if ret.seatbeltUnlatched:
|
||||
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
|
||||
if self.CS.car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, 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:
|
||||
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
|
||||
if not self.CS.gear_shifter_valid:
|
||||
|
|
@ -318,13 +333,6 @@ class CarInterface(object):
|
|||
if b.type == "cancel" and b.pressed:
|
||||
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
|
||||
|
||||
# update previous brake/gas pressed
|
||||
|
|
@ -337,7 +345,7 @@ class CarInterface(object):
|
|||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c, perception_state=log.Live20Data.new_message()):
|
||||
def apply(self, c):
|
||||
hud_v_cruise = c.hudControl.setSpeed
|
||||
if hud_v_cruise > 70:
|
||||
hud_v_cruise = 0
|
||||
|
|
|
|||
|
|
@ -10,6 +10,9 @@ class CAR:
|
|||
CADILLAC_CT6 = "CADILLAC CT6 SUPERCRUISE 2018"
|
||||
MALIBU = "CHEVROLET MALIBU PREMIER 2017"
|
||||
ACADIA = "GMC ACADIA DENALI 2018"
|
||||
BUICK_REGAL = "BUICK REGAL ESSENCE 2018"
|
||||
|
||||
SUPERCRUISE_CARS = [CAR.CADILLAC_CT6]
|
||||
|
||||
class CruiseButtons:
|
||||
UNPRESS = 1
|
||||
|
|
@ -39,10 +42,10 @@ AUDIO_HUD = {
|
|||
|
||||
def is_eps_status_ok(eps_status, car_fingerprint):
|
||||
valid_eps_status = []
|
||||
if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
|
||||
valid_eps_status += [0, 1]
|
||||
elif car_fingerprint == CAR.CADILLAC_CT6:
|
||||
if car_fingerprint in SUPERCRUISE_CARS:
|
||||
valid_eps_status += [0, 1, 4, 5, 6]
|
||||
else:
|
||||
valid_eps_status += [0, 1]
|
||||
return eps_status in valid_eps_status
|
||||
|
||||
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
|
||||
}],
|
||||
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: [
|
||||
# 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.ACADIA: [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
|
||||
}
|
||||
|
||||
|
|
@ -108,5 +117,6 @@ DBC = {
|
|||
CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
|
||||
CAR.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'),
|
||||
}
|
||||
|
|
|
|||
|
|
@ -86,7 +86,7 @@ class CarController(object):
|
|||
|
||||
def update(self, sendcan, enabled, CS, frame, actuators, \
|
||||
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):
|
||||
|
||||
""" Controls thread """
|
||||
|
|
@ -121,7 +121,7 @@ class CarController(object):
|
|||
|
||||
# For lateral control-only, send chimes as a beep since we don't send 0x1fa
|
||||
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
|
||||
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),
|
||||
("HUD_LEAD", "ACC_HUD", 0),
|
||||
("USER_BRAKE", "VSA_STATUS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
|
||||
("STEER_STATUS", "STEER_STATUS", 5),
|
||||
("GEAR_SHIFTER", "GEARBOX", 0),
|
||||
("PEDAL_GAS", "POWERTRAIN_DATA", 0),
|
||||
|
|
@ -76,7 +77,6 @@ def get_can_signals(CP):
|
|||
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
|
||||
("MAIN_ON", "SCM_FEEDBACK", 0),
|
||||
("EPB_STATE", "EPB_STATUS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0),
|
||||
("CRUISE_SPEED", "ACC_HUD", 0)]
|
||||
checks += [("GAS_PEDAL_2", 100)]
|
||||
else:
|
||||
|
|
@ -101,8 +101,7 @@ def get_can_signals(CP):
|
|||
if CP.carFingerprint == CAR.CIVIC:
|
||||
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
|
||||
("MAIN_ON", "SCM_FEEDBACK", 0),
|
||||
("EPB_STATE", "EPB_STATUS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)]
|
||||
("EPB_STATE", "EPB_STATUS", 0)]
|
||||
elif CP.carFingerprint == CAR.ACURA_ILX:
|
||||
signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
|
||||
("MAIN_ON", "SCM_BUTTONS", 0)]
|
||||
|
|
@ -110,8 +109,7 @@ def get_can_signals(CP):
|
|||
signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
|
||||
elif CP.carFingerprint == CAR.ODYSSEY:
|
||||
signals += [("MAIN_ON", "SCM_FEEDBACK", 0),
|
||||
("EPB_STATE", "EPB_STATUS", 0),
|
||||
("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)]
|
||||
("EPB_STATE", "EPB_STATUS", 0)]
|
||||
checks += [("EPB_STATUS", 50)]
|
||||
elif CP.carFingerprint == CAR.PILOT:
|
||||
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.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_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):
|
||||
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']
|
||||
else:
|
||||
self.park_brake = 0 # TODO
|
||||
self.brake_hold = 0 # TODO
|
||||
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
|
||||
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.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
|
||||
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.user_brake > 0.05:
|
||||
self.brake_pressed = 1
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
import numpy as np
|
||||
from cereal import car, log
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
|
@ -575,7 +575,7 @@ class CarInterface(object):
|
|||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c, perception_state=log.Live20Data.new_message()):
|
||||
def apply(self, c):
|
||||
if c.hudControl.speedVisible:
|
||||
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
|
||||
else:
|
||||
|
|
@ -584,19 +584,19 @@ class CarInterface(object):
|
|||
hud_alert = VISUAL_HUD[c.hudControl.visualAlert.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, \
|
||||
c.actuators, \
|
||||
c.cruiseControl.speedOverride, \
|
||||
c.cruiseControl.override, \
|
||||
c.cruiseControl.cancel, \
|
||||
pcm_accel, \
|
||||
perception_state.radarErrors, \
|
||||
hud_v_cruise, c.hudControl.lanesVisible, \
|
||||
hud_show_car = c.hudControl.leadVisible, \
|
||||
hud_alert = hud_alert, \
|
||||
snd_beep = snd_beep, \
|
||||
snd_chime = snd_chime)
|
||||
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
|
||||
c.actuators,
|
||||
c.cruiseControl.speedOverride,
|
||||
c.cruiseControl.override,
|
||||
c.cruiseControl.cancel,
|
||||
pcm_accel,
|
||||
hud_v_cruise,
|
||||
c.hudControl.lanesVisible,
|
||||
hud_show_car=c.hudControl.leadVisible,
|
||||
hud_alert=hud_alert,
|
||||
snd_beep=snd_beep,
|
||||
snd_chime=snd_chime)
|
||||
|
||||
self.frame += 1
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@ from selfdrive.can.packer import CANPacker
|
|||
# Steer torque limits
|
||||
|
||||
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_DOWN = 7
|
||||
STEER_DRIVER_ALLOWANCE = 50
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ def get_can_parser(CP):
|
|||
("CF_Clu_InhibitR", "CLU15", 0),
|
||||
|
||||
("CF_Lvr_Gear","LVR12",0),
|
||||
("CUR_GR", "TCU12",0),
|
||||
|
||||
("ACCEnable", "TCS13", 0),
|
||||
("ACC_REQ", "TCS13", 0),
|
||||
|
|
@ -234,6 +235,17 @@ class CarState(object):
|
|||
else:
|
||||
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
|
||||
self.lkas11 = cp_cam.vl["LKAS11"]
|
||||
self.clu11 = cp.vl["CLU11"]
|
||||
|
|
|
|||
|
|
@ -1,11 +1,11 @@
|
|||
#!/usr/bin/env python
|
||||
from cereal import car, log
|
||||
from cereal import car
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
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.values import CAMERA_MSGS, CAR, get_hud_alerts
|
||||
from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES
|
||||
|
||||
try:
|
||||
from selfdrive.car.hyundai.carcontroller import CarController
|
||||
|
|
@ -113,6 +113,14 @@ class CarInterface(object):
|
|||
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
|
||||
ret.steerKpV, ret.steerKiV = [[0.16], [0.01]]
|
||||
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:
|
||||
ret.steerKf = 0.00005
|
||||
ret.steerRateCost = 0.5
|
||||
|
|
@ -192,8 +200,10 @@ class CarInterface(object):
|
|||
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
|
||||
|
||||
# gear shifter
|
||||
if self.CP.carFingerprint == CAR.ELANTRA:
|
||||
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
|
||||
ret.gearShifter = self.CS.gear_shifter_cluster
|
||||
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
|
||||
ret.gearShifter = self.CS.gear_tcu
|
||||
else:
|
||||
ret.gearShifter = self.CS.gear_shifter
|
||||
|
||||
|
|
@ -300,7 +310,7 @@ class CarInterface(object):
|
|||
|
||||
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)
|
||||
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@ def get_hud_alerts(visual_alert, audible_alert):
|
|||
class CAR:
|
||||
ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
|
||||
GENESIS = "HYUNDAI GENESIS 2018"
|
||||
KIA_OPTIMA = "KIA OPTIMA SX 2019"
|
||||
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
|
||||
KIA_STINGER = "KIA STINGER GT2 2018"
|
||||
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
|
||||
}],
|
||||
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: [{
|
||||
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 = {
|
||||
"crc8": [CAR.SANTA_FE],
|
||||
"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 = {
|
||||
CAR.ELANTRA: 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_STINGER: dbc_dict('hyundai_kia_generic', None),
|
||||
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None),
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!/usr/bin/env python
|
||||
import zmq
|
||||
from cereal import car, log
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
|
@ -118,6 +118,6 @@ class CarInterface(object):
|
|||
|
||||
return ret.as_reader()
|
||||
|
||||
def apply(self, c, perception_state=log.Live20Data.new_message()):
|
||||
def apply(self, c):
|
||||
# in mock no carcontrols
|
||||
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,\
|
||||
create_steer_command, create_ui_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.can.packer import CANPacker
|
||||
|
||||
|
|
@ -129,7 +129,16 @@ class CarController(object):
|
|||
# *** compute control surfaces ***
|
||||
|
||||
# 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 = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
|
||||
|
||||
|
|
@ -213,6 +222,11 @@ class CarController(object):
|
|||
else:
|
||||
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:
|
||||
for addr in TARGET_IDS:
|
||||
can_sends.append(create_video_target(frame/10, addr))
|
||||
|
|
|
|||
|
|
@ -62,6 +62,11 @@ def get_can_parser(CP):
|
|||
|
||||
if CP.carFingerprint == CAR.PRIUS:
|
||||
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)
|
||||
|
||||
|
|
@ -112,7 +117,10 @@ class CarState(object):
|
|||
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
|
||||
|
||||
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.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!/usr/bin/env python
|
||||
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.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
|
@ -61,7 +61,7 @@ class CarInterface(object):
|
|||
ret.safetyModel = car.CarParams.SafetyModels.toyota
|
||||
|
||||
# pedal
|
||||
ret.enableCruise = True
|
||||
ret.enableCruise = not ret.enableGasInterceptor
|
||||
|
||||
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
|
||||
# scale unknown params for other cars
|
||||
|
|
@ -77,6 +77,7 @@ class CarInterface(object):
|
|||
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
|
||||
|
||||
if candidate == CAR.PRIUS:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 15.00 # unknown end-to-end spec
|
||||
|
|
@ -88,6 +89,7 @@ class CarInterface(object):
|
|||
ret.steerActuatorDelay = 0.25
|
||||
|
||||
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.wheelbase = 2.65
|
||||
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
|
||||
|
||||
elif candidate == CAR.COROLLA:
|
||||
stop_and_go = False
|
||||
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.70
|
||||
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
|
||||
|
||||
elif candidate == CAR.LEXUS_RXH:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.79
|
||||
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
|
||||
|
||||
elif candidate in [CAR.CHR, CAR.CHRH]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100
|
||||
ret.wheelbase = 2.63906
|
||||
ret.steerRatio = 13.6
|
||||
|
|
@ -124,6 +129,7 @@ class CarInterface(object):
|
|||
ret.steerKf = 0.00006
|
||||
|
||||
elif candidate in [CAR.CAMRY, CAR.CAMRYH]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100
|
||||
ret.wheelbase = 2.82448
|
||||
ret.steerRatio = 13.7
|
||||
|
|
@ -133,6 +139,7 @@ class CarInterface(object):
|
|||
ret.steerKf = 0.00006
|
||||
|
||||
elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 16.0
|
||||
|
|
@ -147,14 +154,12 @@ class CarInterface(object):
|
|||
ret.longPidDeadzoneBP = [0., 9.]
|
||||
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
|
||||
# to a negative value, so it won't matter.
|
||||
# hybrid models can't do stop and go even though the stock ACC can't
|
||||
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
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS
|
||||
|
||||
centerToRear = ret.wheelbase - ret.centerToFront
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
|
|
@ -178,8 +183,6 @@ class CarInterface(object):
|
|||
# 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.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph
|
||||
ret.gasMaxBP = [0.]
|
||||
ret.gasMaxV = [0.5]
|
||||
ret.brakeMaxBP = [5., 20.]
|
||||
ret.brakeMaxV = [1., 0.8]
|
||||
|
||||
|
|
@ -190,15 +193,24 @@ class CarInterface(object):
|
|||
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu)
|
||||
cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs)
|
||||
cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
|
||||
ret.steerLimitAlert = False
|
||||
ret.longitudinalKpBP = [0., 5., 35.]
|
||||
ret.longitudinalKiBP = [0., 35.]
|
||||
ret.stoppingControl = False
|
||||
ret.startAccel = 0.0
|
||||
|
||||
ret.longitudinalKpBP = [0., 5., 35.]
|
||||
ret.longitudinalKpV = [3.6, 2.4, 1.5]
|
||||
ret.longitudinalKiBP = [0., 35.]
|
||||
ret.longitudinalKiV = [0.54, 0.36]
|
||||
if ret.enableGasInterceptor:
|
||||
ret.gasMaxBP = [0., 9., 35]
|
||||
ret.gasMaxV = [0.2, 0.5, 0.7]
|
||||
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
|
||||
|
||||
|
|
@ -234,7 +246,11 @@ class CarInterface(object):
|
|||
|
||||
# gas pedal
|
||||
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
|
||||
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.available = bool(self.CS.main_on)
|
||||
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
|
||||
# receiving any special command
|
||||
# also if interceptor is detected
|
||||
ret.cruiseState.standstill = False
|
||||
else:
|
||||
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
|
||||
|
|
@ -346,7 +364,7 @@ class CarInterface(object):
|
|||
|
||||
# pass in a car.CarControl
|
||||
# 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, 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)
|
||||
|
||||
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):
|
||||
values = {
|
||||
|
|
|
|||
|
|
@ -79,7 +79,7 @@ def check_ecu_msgs(fingerprint, ecu):
|
|||
|
||||
FINGERPRINTS = {
|
||||
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: [{
|
||||
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
|
||||
}],
|
||||
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
|
||||
{
|
||||
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
|
||||
{
|
||||
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: [{
|
||||
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: [{
|
||||
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.services import service_list
|
||||
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, \
|
||||
get_events, \
|
||||
create_event, \
|
||||
|
|
@ -23,6 +22,7 @@ from selfdrive.controls.lib.latcontrol import LatControl
|
|||
from selfdrive.controls.lib.alertmanager import AlertManager
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.controls.lib.driver_monitor import DriverStatus
|
||||
from selfdrive.controls.lib.planner import _DT_MPC
|
||||
from selfdrive.locationd.calibration_helpers import Calibration, Filter
|
||||
|
||||
ThermalStatus = log.ThermalData.ThermalStatus
|
||||
|
|
@ -39,9 +39,9 @@ def isEnabled(state):
|
|||
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,
|
||||
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"""
|
||||
|
||||
# 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
|
||||
hh = None
|
||||
dm = None
|
||||
gps = None
|
||||
|
||||
for socket, event in poller.poll(0):
|
||||
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)
|
||||
elif socket is driver_monitor:
|
||||
dm = messaging.recv_one(socket)
|
||||
elif socket is gps_location:
|
||||
gps = messaging.recv_one(socket)
|
||||
elif socket is plan_sock:
|
||||
plan = messaging.recv_one(socket)
|
||||
elif socket is path_plan_sock:
|
||||
path_plan = messaging.recv_one(socket)
|
||||
|
||||
if td is not None:
|
||||
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:
|
||||
driver_status.get_pose(dm.driverMonitoring, params)
|
||||
|
||||
# Geofence
|
||||
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
|
||||
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
def state_control(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):
|
||||
def state_control(plan, path_plan, 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):
|
||||
"""Given the state, this function returns an actuators packet"""
|
||||
|
||||
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)
|
||||
|
||||
# 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,
|
||||
PL.PP.c_poly, PL.PP.c_prob, CS.steeringAngle,
|
||||
path_plan.cPoly, path_plan.cProb, CS.steeringAngle,
|
||||
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
|
||||
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,
|
||||
CP, PL.lead_1)
|
||||
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
|
||||
# Steering PID loop and lateral MPC
|
||||
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
|
||||
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())
|
||||
|
||||
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,
|
||||
carcontrol, live100, livempc, AM, driver_status,
|
||||
LaC, LoC, angle_offset, passive, start_time):
|
||||
def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
|
||||
carcontrol, live100, AM, driver_status,
|
||||
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"""
|
||||
plan_ts = plan.logMonoTime
|
||||
plan = plan.plan
|
||||
|
||||
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.lanesVisible = isEnabled(state)
|
||||
CC.hudControl.leadVisible = plan.hasLead
|
||||
CC.hudControl.rightLaneVisible = plan.hasRightLane
|
||||
CC.hudControl.leftLaneVisible = plan.hasLeftLane
|
||||
CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5)
|
||||
CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5)
|
||||
CC.hudControl.visualAlert = AM.visual_alert
|
||||
CC.hudControl.audibleAlert = AM.audible_alert
|
||||
|
||||
# send car controls over can
|
||||
CI.apply(CC, perception_state)
|
||||
CI.apply(CC)
|
||||
|
||||
force_decel = driver_status.awareness < 0.
|
||||
|
||||
# live100
|
||||
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),
|
||||
"canMonoTimes": list(CS.canMonoTimes),
|
||||
"planMonoTime": plan_ts,
|
||||
"pathPlanMonoTime": path_plan.logMonoTime,
|
||||
"enabled": isEnabled(state),
|
||||
"active": isActive(state),
|
||||
"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),
|
||||
"uiSteer": float(LaC.pid.i),
|
||||
"ufSteer": float(LaC.pid.f),
|
||||
"vTargetLead": float(plan.vTarget),
|
||||
"aTarget": float(plan.aTarget),
|
||||
"vTargetLead": float(v_acc),
|
||||
"aTarget": float(a_acc),
|
||||
"jerkFactor": float(plan.jerkFactor),
|
||||
"angleOffset": float(angle_offset),
|
||||
"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.,
|
||||
"startMonoTime": start_time,
|
||||
"mapValid": plan.mapValid,
|
||||
"forceDecel": bool(force_decel),
|
||||
}
|
||||
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
|
||||
carcontrol.send(cc_send.to_bytes())
|
||||
|
||||
# send MPC when updated (20 Hz)
|
||||
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
|
||||
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())
|
||||
if (rk.frame % 36000) == 0: # update angle offset every 6 minutes
|
||||
params.put("ControlsParams", json.dumps({'angle_offset': angle_offset}))
|
||||
|
||||
return CC
|
||||
|
||||
|
||||
def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
||||
def controlsd_thread(gctx=None, rate=100):
|
||||
gc.disable()
|
||||
|
||||
# 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)
|
||||
carstate = messaging.pub_sock(context, service_list['carState'].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"
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
|
||||
CC = car.CarControl.new_message()
|
||||
|
|
@ -449,11 +430,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
if passive:
|
||||
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)
|
||||
VM = VehicleModel(CP)
|
||||
LaC = LatControl(CP)
|
||||
|
|
@ -478,17 +454,19 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
mismatch_counter = 0
|
||||
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
|
||||
angle_offset = default_bias
|
||||
calibration_params = params.get("CalibrationParams")
|
||||
if calibration_params:
|
||||
try:
|
||||
calibration_params = json.loads(calibration_params)
|
||||
angle_offset = calibration_params["angle_offset2"]
|
||||
except (ValueError, KeyError):
|
||||
pass
|
||||
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
|
||||
controls_params = params.get("ControlsParams")
|
||||
# Read angle offset from previous drive
|
||||
if controls_params is not None:
|
||||
controls_params = json.loads(controls_params)
|
||||
angle_offset = controls_params['angle_offset']
|
||||
else:
|
||||
angle_offset = 0.
|
||||
|
||||
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)
|
||||
|
||||
# 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,
|
||||
driver_monitor, gps_location, poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, geofence, state, mismatch_counter, params)
|
||||
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\
|
||||
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")
|
||||
|
||||
# Define longitudinal plan (MPC)
|
||||
plan, plan_ts = calc_plan(CS, CP, VM, events, PL, LaC, LoC, v_cruise_kph, driver_status, geofence)
|
||||
prof.checkpoint("Plan")
|
||||
path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
|
||||
plan_age = (start_time - plan.logMonoTime) / 1e9
|
||||
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:
|
||||
# update control state
|
||||
|
|
@ -512,13 +498,16 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
|
|||
prof.checkpoint("State transition")
|
||||
|
||||
# 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,
|
||||
v_cruise_kph_last, AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
|
||||
actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \
|
||||
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")
|
||||
|
||||
# Publish data
|
||||
CC = data_send(PL.perception_state, plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
|
||||
live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive, start_time)
|
||||
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
|
||||
live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc)
|
||||
prof.checkpoint("Sent")
|
||||
|
||||
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.drive_helpers import MPC_COST_LAT
|
||||
from selfdrive.controls.lib.lateral_mpc import libmpc_py
|
||||
from common.numpy_fast import interp
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from cereal import car
|
||||
|
||||
_DT = 0.01 # 100Hz
|
||||
_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):
|
||||
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
|
||||
|
||||
|
|
@ -28,75 +16,12 @@ class LatControl(object):
|
|||
(CP.steerKiBP, CP.steerKiV),
|
||||
k_f=CP.steerKf, pos_limit=1.0)
|
||||
self.last_cloudlog_t = 0.0
|
||||
self.setup_mpc(CP.steerRateCost)
|
||||
|
||||
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
|
||||
self.angle_steers_des = 0.
|
||||
|
||||
def reset(self):
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL):
|
||||
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")
|
||||
|
||||
def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan):
|
||||
if v_ego < 0.3 or not active:
|
||||
output_steer = 0.0
|
||||
self.pid.reset()
|
||||
|
|
@ -105,7 +30,8 @@ class LatControl(object):
|
|||
# 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
|
||||
#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)
|
||||
self.pid.pos_limit = steers_max
|
||||
self.pid.neg_limit = -steers_max
|
||||
|
|
|
|||
|
|
@ -71,7 +71,7 @@ class LongControl(object):
|
|||
self.pid.reset()
|
||||
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"""
|
||||
# Actuation limits
|
||||
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
|
||||
from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
|
||||
import zmq
|
||||
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):
|
||||
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()
|
||||
def __init__(self, CP):
|
||||
self.MP = ModelParser()
|
||||
|
||||
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.
|
||||
self.last_cloudlog_t = 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
|
||||
context = zmq.Context()
|
||||
self.plan = messaging.pub_sock(context, service_list['pathPlan'].port)
|
||||
self.livempc = messaging.pub_sock(context, service_list['liveMpc'].port)
|
||||
|
||||
# only offset left and right lane lines; offsetting p_poly does not make sense
|
||||
l_poly[3] += CAMERA_OFFSET
|
||||
r_poly[3] += CAMERA_OFFSET
|
||||
self.setup_mpc(CP.steerRateCost)
|
||||
self.invalid_counter = 0
|
||||
|
||||
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
|
||||
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)
|
||||
|
||||
# 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
|
||||
self.mpc_solution = libmpc_py.ffi.new("log_t *")
|
||||
self.cur_state = libmpc_py.ffi.new("state_t *")
|
||||
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
|
||||
|
||||
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])
|
||||
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
|
||||
|
||||
r_prob *= lane_r_prob
|
||||
def update(self, CP, VM, CS, md, live100):
|
||||
v_ego = CS.carState.vEgo
|
||||
angle_steers = CS.carState.steeringAngle
|
||||
active = live100.live100.active
|
||||
angle_offset = live100.live100.angleOffset
|
||||
self.MP.update(v_ego, md)
|
||||
|
||||
self.lead_dist = md.model.lead.dist
|
||||
self.lead_prob = md.model.lead.prob
|
||||
self.lead_var = md.model.lead.std**2
|
||||
# Run MPC
|
||||
self.angle_steers_des_prev = self.angle_steers_des_mpc
|
||||
curvature_factor = VM.curvature_factor(v_ego)
|
||||
|
||||
# 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)
|
||||
l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly))
|
||||
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.r_poly = r_poly
|
||||
self.r_prob = r_prob
|
||||
# account for actuation delay
|
||||
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
|
||||
|
||||
self.l_poly = l_poly
|
||||
self.l_prob = l_prob
|
||||
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,
|
||||
self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width)
|
||||
|
||||
self.p_poly = p_poly
|
||||
self.p_prob = p_prob
|
||||
# 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)
|
||||
|
||||
# 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
|
||||
import os
|
||||
import zmq
|
||||
import math
|
||||
import numpy as np
|
||||
from copy import copy
|
||||
from cereal import log
|
||||
from collections import defaultdict
|
||||
from common.params import Params
|
||||
from common.realtime import sec_since_boot
|
||||
|
|
@ -14,23 +11,17 @@ from selfdrive.swaglog import cloudlog
|
|||
from selfdrive.config import Conversions as CV
|
||||
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.pathplanner import PathPlanner
|
||||
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
|
||||
from selfdrive.controls.lib.speed_smoother import speed_smoother
|
||||
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
|
||||
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
|
||||
|
||||
# Max lateral acceleration, used to caclulate how much to slow down in turns
|
||||
A_Y_MAX = 1.85 # m/s^2
|
||||
NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS
|
||||
|
||||
_DT = 0.01 # 100Hz
|
||||
_DT_MPC = 0.2 # 5Hz
|
||||
MAX_SPEED_ERROR = 2.0
|
||||
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
|
||||
# make sure these accelerations are smaller than mpc limits
|
||||
_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.
|
||||
# This helps underweighting ARel when v_lead is close to zero.
|
||||
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 = v_rel**2 + 2 * x_lead * a_rel
|
||||
|
|
@ -186,6 +177,8 @@ class LongitudinalMpc(object):
|
|||
self.cur_state[0].a_ego = a
|
||||
|
||||
def update(self, CS, lead, v_cruise_setpoint):
|
||||
v_ego = CS.carState.vEgo
|
||||
|
||||
# Setup current mpc state
|
||||
self.cur_state[0].x_ego = 0.0
|
||||
|
||||
|
|
@ -194,7 +187,6 @@ class LongitudinalMpc(object):
|
|||
v_lead = max(0.0, lead.vLead)
|
||||
a_lead = lead.aLeadK
|
||||
|
||||
|
||||
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
|
||||
v_lead = 0.0
|
||||
a_lead = 0.0
|
||||
|
|
@ -213,7 +205,7 @@ class LongitudinalMpc(object):
|
|||
self.prev_lead_status = False
|
||||
# Fake a fast lead car, so mpc keeps running
|
||||
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
|
||||
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,
|
||||
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.v_mpc = CS.vEgo
|
||||
self.a_mpc = CS.aEgo
|
||||
self.v_mpc = v_ego
|
||||
self.a_mpc = CS.carState.aEgo
|
||||
self.prev_lead_status = False
|
||||
|
||||
|
||||
|
|
@ -255,38 +247,20 @@ class Planner(object):
|
|||
self.CP = CP
|
||||
self.poller = zmq.Poller()
|
||||
|
||||
self.live20 = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=self.poller)
|
||||
self.model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=self.poller)
|
||||
self.live_map_data = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=self.poller)
|
||||
|
||||
if os.environ.get('GPS_PLANNER_ACTIVE', False):
|
||||
self.gps_planner_plan = messaging.sub_sock(context, service_list['gpsPlannerPlan'].port, conflate=True, poller=self.poller, addr=GPS_PLANNER_ADDR)
|
||||
else:
|
||||
self.gps_planner_plan = None
|
||||
|
||||
self.plan = messaging.pub_sock(context, service_list['plan'].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.PP = PathPlanner()
|
||||
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
|
||||
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
|
||||
|
||||
self.v_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_sol = 0.0
|
||||
self.v_acc_future = 0.0
|
||||
self.a_acc = 0.0
|
||||
self.a_acc_sol = 0.0
|
||||
self.v_cruise = 0.0
|
||||
self.a_cruise = 0.0
|
||||
|
||||
|
|
@ -298,11 +272,6 @@ class Planner(object):
|
|||
self.fcw_checker = FCWChecker()
|
||||
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.v_curvature = NO_CURVATURE_SPEED
|
||||
self.v_speedlimit = NO_CURVATURE_SPEED
|
||||
|
|
@ -319,12 +288,6 @@ class Planner(object):
|
|||
|
||||
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
|
||||
|
||||
# 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])
|
||||
|
||||
# this runs whenever we get a packet that can change the plan
|
||||
def update(self, CS, CP, VM, LaC, LoC, v_cruise_kph, force_slow_decel):
|
||||
cur_time = sec_since_boot()
|
||||
def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
|
||||
"""Gets called when new live20 is available"""
|
||||
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
|
||||
|
||||
md = None
|
||||
l20 = None
|
||||
gps_planner_plan = None
|
||||
self.last_md_ts = md.logMonoTime
|
||||
|
||||
for socket, event in self.poller.poll(0):
|
||||
if socket is self.model:
|
||||
md = messaging.recv_one(socket)
|
||||
elif socket is self.live20:
|
||||
l20 = messaging.recv_one(socket)
|
||||
elif socket is self.gps_planner_plan:
|
||||
gps_planner_plan = messaging.recv_one(socket)
|
||||
elif socket is self.live_map_data:
|
||||
self.last_live_map_data = messaging.recv_one(socket).liveMapData
|
||||
self.radar_errors = list(live20.live20.radarErrors)
|
||||
|
||||
if gps_planner_plan is not None:
|
||||
self.last_gps_planner_plan = gps_planner_plan
|
||||
self.lead_1 = live20.live20.leadOne
|
||||
self.lead_2 = live20.live20.leadTwo
|
||||
|
||||
if md is not None:
|
||||
self.last_md_ts = md.logMonoTime
|
||||
self.last_model = cur_time
|
||||
self.model_dead = False
|
||||
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
|
||||
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
|
||||
|
||||
self.PP.update(CS.vEgo, md)
|
||||
self.v_speedlimit = NO_CURVATURE_SPEED
|
||||
self.v_curvature = NO_CURVATURE_SPEED
|
||||
self.map_valid = live_map_data.liveMapData.mapValid
|
||||
|
||||
if self.last_gps_planner_plan is not None:
|
||||
plan = self.last_gps_planner_plan.gpsPlannerPlan
|
||||
self.gps_planner_active = plan.valid
|
||||
if plan.valid:
|
||||
self.PP.d_poly = plan.poly
|
||||
self.PP.p_poly = plan.poly
|
||||
self.PP.c_poly = plan.poly
|
||||
self.PP.l_prob = 0.0
|
||||
self.PP.r_prob = 0.0
|
||||
self.PP.c_prob = 1.0
|
||||
# Speed limit and curvature
|
||||
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
|
||||
if set_speed_limit_active:
|
||||
if live_map_data.liveMapData.speedLimitValid:
|
||||
speed_limit = live_map_data.liveMapData.speedLimit
|
||||
offset = float(self.params.get("SpeedLimitOffset"))
|
||||
self.v_speedlimit = speed_limit + offset
|
||||
|
||||
if l20 is not None:
|
||||
self.perception_state = copy(l20.live20)
|
||||
self.last_l20_ts = l20.logMonoTime
|
||||
self.last_l20 = cur_time
|
||||
self.radar_dead = False
|
||||
self.radar_errors = list(l20.live20.radarErrors)
|
||||
if live_map_data.liveMapData.curvatureValid:
|
||||
curvature = abs(live_map_data.liveMapData.curvature)
|
||||
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
|
||||
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
|
||||
self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
|
||||
|
||||
self.v_acc_start = self.v_acc_sol
|
||||
self.a_acc_start = self.a_acc_sol
|
||||
self.acc_start_time = cur_time
|
||||
self.decel_for_turn = bool(self.v_curvature < min([v_cruise_setpoint, self.v_speedlimit, v_ego + 1.]))
|
||||
v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
|
||||
|
||||
self.lead_1 = l20.live20.leadOne
|
||||
self.lead_2 = l20.live20.leadTwo
|
||||
# Calculate speed for normal cruise control
|
||||
if enabled:
|
||||
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
|
||||
accel_limits = limit_accel_in_turns(v_ego, CS.carState.steeringAngle, accel_limits, self.CP)
|
||||
|
||||
enabled = (LoC.long_control_state == LongCtrlState.pid) or (LoC.long_control_state == LongCtrlState.stopping)
|
||||
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
|
||||
if force_slow_decel:
|
||||
# if required so, force a smooth deceleration
|
||||
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
|
||||
accel_limits[0] = min(accel_limits[0], accel_limits[1])
|
||||
|
||||
if self.last_live_map_data:
|
||||
self.v_speedlimit = NO_CURVATURE_SPEED
|
||||
self.v_curvature = NO_CURVATURE_SPEED
|
||||
self.map_valid = self.last_live_map_data.mapValid
|
||||
# Change accel limits based on time remaining to turn
|
||||
if self.decel_for_turn:
|
||||
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)
|
||||
accel_limits[0] = max(accel_limits[0], required_decel)
|
||||
|
||||
# Speed limit
|
||||
if self.last_live_map_data.speedLimitValid:
|
||||
speed_limit = self.last_live_map_data.speedLimit
|
||||
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
|
||||
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 = long_control_state == LongCtrlState.starting
|
||||
a_ego = min(CS.carState.aEgo, 0.0)
|
||||
reset_speed = MIN_CAN_SPEED if starting else v_ego
|
||||
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
|
||||
|
||||
if set_speed_limit_active:
|
||||
offset = float(self.params.get("SpeedLimitOffset"))
|
||||
self.v_speedlimit = speed_limit + offset
|
||||
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)
|
||||
|
||||
# Curvature
|
||||
if self.last_live_map_data.curvatureValid:
|
||||
curvature = abs(self.last_live_map_data.curvature)
|
||||
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.]))
|
||||
v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
|
||||
self.choose_solution(v_cruise_setpoint, enabled)
|
||||
|
||||
# Calculate speed for normal cruise control
|
||||
if enabled:
|
||||
accel_limits = map(float, calc_cruise_accel_limits(CS.vEgo, following))
|
||||
# TODO: make a separate lookup for jerk tuning
|
||||
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
|
||||
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle, accel_limits, self.CP)
|
||||
# determine fcw
|
||||
if self.mpc1.new_lead:
|
||||
self.fcw_checker.reset_lead(cur_time)
|
||||
|
||||
if force_slow_decel:
|
||||
# if required so, force a smooth deceleration
|
||||
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
|
||||
accel_limits[0] = min(accel_limits[0], accel_limits[1])
|
||||
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
|
||||
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.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.carState.brakePressed
|
||||
if self.fcw:
|
||||
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
|
||||
|
||||
# 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)
|
||||
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
|
||||
|
||||
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 ****
|
||||
plan_send = messaging.new_message()
|
||||
plan_send.init('plan')
|
||||
|
||||
# TODO: Move all these events to controlsd. This has nothing to do with planning
|
||||
events = []
|
||||
if self.model_dead:
|
||||
if model_dead:
|
||||
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:
|
||||
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.mdMonoTime = self.last_md_ts
|
||||
plan_send.plan.l20MonoTime = self.last_l20_ts
|
||||
|
||||
# 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)
|
||||
plan_send.plan.mdMonoTime = md.logMonoTime
|
||||
plan_send.plan.l20MonoTime = live20.logMonoTime
|
||||
|
||||
# longitudal plan
|
||||
plan_send.plan.longitudinalValid = not self.radar_dead
|
||||
plan_send.plan.vCruise = self.v_cruise
|
||||
plan_send.plan.aCruise = self.a_cruise
|
||||
plan_send.plan.vTarget = self.v_acc_sol
|
||||
plan_send.plan.aTarget = self.a_acc_sol
|
||||
plan_send.plan.vStart = self.v_acc_start
|
||||
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.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.gpsPlannerActive = self.gps_planner_active
|
||||
|
||||
plan_send.plan.vCurvature = self.v_curvature
|
||||
plan_send.plan.decelForTurn = self.decel_for_turn
|
||||
plan_send.plan.mapValid = self.map_valid
|
||||
|
||||
# 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
|
||||
|
||||
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
|
||||
from selfdrive.services import service_list
|
||||
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, \
|
||||
RDR_TO_LDR, NO_FUSION_SCORE
|
||||
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)
|
||||
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
|
||||
|
||||
PP = PathPlanner()
|
||||
MP = ModelParser()
|
||||
RI = RadarInterface(CP)
|
||||
|
||||
last_md_ts = 0
|
||||
|
|
@ -134,26 +134,26 @@ def radard_thread(gctx=None):
|
|||
last_md_ts = md.logMonoTime
|
||||
|
||||
# *** 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
|
||||
if PP.lead_prob > 0.7:
|
||||
reading = speedSensorV.read(PP.lead_dist, covar=np.matrix(PP.lead_var))
|
||||
if MP.lead_prob > 0.7:
|
||||
reading = speedSensorV.read(MP.lead_dist, covar=np.matrix(MP.lead_var))
|
||||
ekfv.update_scalar(reading)
|
||||
ekfv.predict(tsv)
|
||||
|
||||
# When changing lanes the distance to the lead car can suddenly change,
|
||||
# which makes the Kalman filter output large relative acceleration
|
||||
if mocked and abs(PP.lead_dist - ekfv.state[XV]) > 2.0:
|
||||
ekfv.state[XV] = PP.lead_dist
|
||||
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
|
||||
if mocked and abs(MP.lead_dist - ekfv.state[XV]) > 2.0:
|
||||
ekfv.state[XV] = MP.lead_dist
|
||||
ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
|
||||
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)
|
||||
else:
|
||||
ekfv.state[XV] = PP.lead_dist
|
||||
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
|
||||
ekfv.state[XV] = MP.lead_dist
|
||||
ekfv.covar = (np.diag([MP.lead_var, ekfv.var_init]))
|
||||
ekfv.state[SPEEDV] = 0.
|
||||
|
||||
if VISION_POINT in ar_pts:
|
||||
|
|
@ -162,7 +162,7 @@ def radard_thread(gctx=None):
|
|||
# *** compute the likely path_y ***
|
||||
if (active and not steer_override) or mocked:
|
||||
# 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:
|
||||
# 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]
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@ if __name__ == "__main__":
|
|||
parser.add_argument('--proxy', action='store_true', help='republish on localhost')
|
||||
parser.add_argument('--map', action='store_true')
|
||||
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")
|
||||
args = parser.parse_args()
|
||||
|
||||
|
|
@ -53,6 +54,10 @@ if __name__ == "__main__":
|
|||
server_thread.start()
|
||||
print 'server running'
|
||||
|
||||
values = None
|
||||
if args.values:
|
||||
values = [s.strip().split(".") for s in args.values.split(",")]
|
||||
|
||||
while 1:
|
||||
polld = poller.poll(timeout=1000)
|
||||
for sock, mode in polld:
|
||||
|
|
@ -79,5 +84,14 @@ if __name__ == "__main__":
|
|||
print(json.loads(msg))
|
||||
elif args.dump_json:
|
||||
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:
|
||||
print evt
|
||||
|
|
|
|||
|
|
@ -1,208 +1,77 @@
|
|||
#!/usr/bin/env python
|
||||
import os
|
||||
import zmq
|
||||
import cv2
|
||||
import copy
|
||||
import json
|
||||
import numpy as np
|
||||
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.services import service_list
|
||||
from common.params import Params
|
||||
from common.ffi_wrapper import ffi_wrap
|
||||
import common.transformations.orientation as orient
|
||||
from common.transformations.model import model_height, get_camera_frame_from_model_frame, get_camera_frame_from_bigmodel_frame
|
||||
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
|
||||
eon_intrinsics, get_calib_from_vp, normalize, denormalize, H, W
|
||||
eon_intrinsics, get_calib_from_vp, H, W
|
||||
|
||||
|
||||
FRAMES_NEEDED = 120 # allow to update VP every so many frames
|
||||
VP_CYCLES_NEEDED = 2
|
||||
CALIBRATION_CYCLES_NEEDED = FRAMES_NEEDED * VP_CYCLES_NEEDED
|
||||
DT = 0.1 # nominal time step of 10Hz (orbd_live runs slower on pc)
|
||||
VP_RATE_LIM = 2. * DT # 2 px/s
|
||||
MPH_TO_MS = 0.44704
|
||||
MIN_SPEED_FILTER = 15 * MPH_TO_MS
|
||||
MAX_YAW_RATE_FILTER = np.radians(2) # per second
|
||||
INPUTS_NEEDED = 300 # allow to update VP every so many frames
|
||||
INPUTS_WANTED = 600 # We want a little bit more than we need for stability
|
||||
WRITE_CYCLES = 400 # write every 400 cycles
|
||||
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
|
||||
|
||||
# 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):
|
||||
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]
|
||||
|
||||
|
||||
class Calibrator(object):
|
||||
def __init__(self, param_put=False):
|
||||
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.vps = []
|
||||
self.cal_status = Calibration.UNCALIBRATED
|
||||
self.frame_counter = 0
|
||||
self.write_counter = 0
|
||||
self.params = Params()
|
||||
calibration_params = self.params.get("CalibrationParams")
|
||||
if calibration_params:
|
||||
try:
|
||||
calibration_params = json.loads(calibration_params)
|
||||
self.vp = np.array(calibration_params["vanishing_point"])
|
||||
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
|
||||
self.vp_cycles = VP_CYCLES_NEEDED
|
||||
self.frame_counter = CALIBRATION_CYCLES_NEEDED
|
||||
self.vps = np.tile(self.vp, (calibration_params['valid_points'], 1)).tolist()
|
||||
self.update_status()
|
||||
except Exception:
|
||||
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):
|
||||
if self.cal_status == Calibration.UNCALIBRATED:
|
||||
# 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:
|
||||
def update_status(self):
|
||||
if len(self.vps) < INPUTS_NEEDED:
|
||||
self.cal_status = Calibration.UNCALIBRATED
|
||||
else:
|
||||
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
|
||||
|
||||
def handle_orb_features(self, log):
|
||||
self.update_vp()
|
||||
self.time_orb, frame_raw = self.parse_orb_features(log)
|
||||
self.dt = min(self.time_orb - self.orb_last_updated, 1.)
|
||||
self.orb_last_updated = self.time_orb
|
||||
if self.time_orb - self.l100_last_updated < 1:
|
||||
return self.update(frame_raw, self.yaw_rate, self.speed)
|
||||
|
||||
def handle_live100(self, log):
|
||||
self.l100_last_updated = self.time_orb
|
||||
self.speed = log.live100.vEgo
|
||||
self.angle_offset = log.live100.angleOffset
|
||||
self.yaw_rate = log.live100.curvature * self.speed
|
||||
|
||||
def handle_debug(self):
|
||||
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 handle_cam_odom(self, log):
|
||||
trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot
|
||||
if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
|
||||
new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
|
||||
new_vp = new_vp[:2]/new_vp[2]
|
||||
self.vps.append(new_vp)
|
||||
self.vps = self.vps[-INPUTS_WANTED:]
|
||||
self.vp = np.mean(self.vps, axis=0)
|
||||
self.update_status()
|
||||
self.write_counter += 1
|
||||
if self.param_put and self.write_counter % WRITE_CYCLES == 0:
|
||||
cal_params = {"vanishing_point": list(self.vp),
|
||||
"valid_points": len(self.vps)}
|
||||
self.params.put("CalibrationParams", json.dumps(cal_params))
|
||||
return new_vp
|
||||
|
||||
def send_data(self, livecalibration):
|
||||
calib = get_calib_from_vp(self.vp)
|
||||
|
|
@ -214,7 +83,7 @@ class Calibrator(object):
|
|||
cal_send = messaging.new_message()
|
||||
cal_send.init('liveCalibration')
|
||||
cal_send.liveCalibration.calStatus = self.cal_status
|
||||
cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100)
|
||||
cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 / INPUTS_NEEDED, 100)
|
||||
cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten())
|
||||
cal_send.liveCalibration.warpMatrixBig = map(float, warp_matrix_big.flatten())
|
||||
cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten())
|
||||
|
|
@ -225,24 +94,17 @@ class Calibrator(object):
|
|||
def calibrationd_thread(gctx=None, addr="127.0.0.1"):
|
||||
context = zmq.Context()
|
||||
|
||||
live100 = messaging.sub_sock(context, service_list['live100'].port, addr=addr, conflate=True)
|
||||
orbfeatures = messaging.sub_sock(context, service_list['orbFeatures'].port, addr=addr, conflate=True)
|
||||
cameraodometry = messaging.sub_sock(context, service_list['cameraOdometry'].port, addr=addr, conflate=True)
|
||||
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
|
||||
while 1:
|
||||
of = messaging.recv_one(orbfeatures)
|
||||
l100 = messaging.recv_one_or_none(live100)
|
||||
co = messaging.recv_one(cameraodometry)
|
||||
|
||||
new_vp = calibrator.handle_orb_features(of)
|
||||
new_vp = calibrator.handle_cam_odom(co)
|
||||
if DEBUG and new_vp is not None:
|
||||
print 'got new vp', new_vp
|
||||
if l100 is not None:
|
||||
calibrator.handle_live100(l100)
|
||||
if DEBUG:
|
||||
calibrator.handle_debug()
|
||||
|
||||
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"):
|
||||
calibrationd_thread(gctx, addr)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
|
|
|
|||
|
|
@ -459,7 +459,7 @@ msg_types = {
|
|||
UBloxDescriptor('RXM_RAW', '<dHbBB3B', [
|
||||
'rcvTow', 'week', 'leapS', 'numMeas', 'recStat', 'reserved1[3]'
|
||||
], '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'
|
||||
]),
|
||||
(CLASS_RXM, MSG_RXM_SFRB):
|
||||
|
|
|
|||
|
|
@ -195,6 +195,7 @@ def gen_raw(msg):
|
|||
'halfCycleSubtracted': trackingStatus_bools[3]}
|
||||
measurements_parsed.append({
|
||||
'svId': m['svId'],
|
||||
'sigId': m['sigId'],
|
||||
'pseudorange': m['prMes'],
|
||||
'carrierCycles': m['cpMes'],
|
||||
'doppler': m['doMes'],
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:55ed3bafdbe1908778383bb5fd310d5a1279b15a84eb85ae1ee65d1107a0752c
|
||||
oid sha256:f4ce9c8b78e58c57b0f033354f1411811457b9e401c16fe5c5b130aa9fe86d3a
|
||||
size 1630952
|
||||
|
|
|
|||
|
|
@ -136,7 +136,7 @@ class Uploader(object):
|
|||
|
||||
def next_file_to_compress(self):
|
||||
for name, key, fn in self.gen_upload_files():
|
||||
if name == "rlog":
|
||||
if name.endswith("log"):
|
||||
return (key, fn, 0)
|
||||
return None
|
||||
|
||||
|
|
|
|||
|
|
@ -89,6 +89,7 @@ managed_processes = {
|
|||
"thermald": "selfdrive.thermald",
|
||||
"uploader": "selfdrive.loggerd.uploader",
|
||||
"controlsd": "selfdrive.controls.controlsd",
|
||||
"plannerd": "selfdrive.controls.plannerd",
|
||||
"radard": "selfdrive.controls.radard",
|
||||
"ubloxd": "selfdrive.locationd.ubloxd",
|
||||
"mapd": "selfdrive.mapd.mapd",
|
||||
|
|
@ -104,7 +105,6 @@ managed_processes = {
|
|||
"visiond": ("selfdrive/visiond", ["./visiond"]),
|
||||
"sensord": ("selfdrive/sensord", ["./sensord"]),
|
||||
"gpsd": ("selfdrive/sensord", ["./gpsd"]),
|
||||
"orbd": ("selfdrive/orbd", ["./orbd_wrapper.sh"]),
|
||||
"updated": "selfdrive.updated",
|
||||
}
|
||||
android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame")
|
||||
|
|
@ -132,6 +132,7 @@ persistent_processes = [
|
|||
|
||||
car_started_processes = [
|
||||
'controlsd',
|
||||
'plannerd',
|
||||
'loggerd',
|
||||
'sensord',
|
||||
'radard',
|
||||
|
|
@ -139,7 +140,6 @@ car_started_processes = [
|
|||
'visiond',
|
||||
'proclogd',
|
||||
'ubloxd',
|
||||
'orbd',
|
||||
'mapd',
|
||||
]
|
||||
|
||||
|
|
@ -452,6 +452,7 @@ def main():
|
|||
del managed_processes['proclogd']
|
||||
if os.getenv("NOCONTROL") is not None:
|
||||
del managed_processes['controlsd']
|
||||
del managed_processes['plannerd']
|
||||
del managed_processes['radard']
|
||||
|
||||
# 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):
|
||||
"""Builds a query to find all highways within a given radius around a point"""
|
||||
pos = " (around:%f,%f,%f)" % (radius, lat, lon)
|
||||
lat_lon = "(%f,%f)" % (lat, lon)
|
||||
q = """(
|
||||
way
|
||||
""" + pos + """
|
||||
[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
|
||||
|
||||
|
|
@ -97,7 +100,7 @@ def query_thread():
|
|||
cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude))
|
||||
prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude))
|
||||
dist = np.linalg.norm(cur_ecef - prev_ecef)
|
||||
if dist < 1000:
|
||||
if dist < 1000: #updated when we are 1km from the edge of the downloaded circle
|
||||
continue
|
||||
|
||||
if dist > 3000:
|
||||
|
|
@ -111,6 +114,7 @@ def query_thread():
|
|||
nodes = []
|
||||
real_nodes = []
|
||||
node_to_way = defaultdict(list)
|
||||
location_info = {}
|
||||
|
||||
for n in new_result.nodes:
|
||||
nodes.append((float(n.lat), float(n.lon), 0))
|
||||
|
|
@ -120,12 +124,18 @@ def query_thread():
|
|||
for n in way.nodes:
|
||||
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 = geodetic2ecef(nodes)
|
||||
tree = spatial.cKDTree(nodes)
|
||||
|
||||
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
|
||||
cache_valid = True
|
||||
query_lock.release()
|
||||
|
|
@ -182,7 +192,7 @@ def mapsd_thread():
|
|||
query_lock.acquire()
|
||||
cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
|
||||
if cur_way is not None:
|
||||
pnts, curvature_valid = cur_way.get_lookahead(last_query_result, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
|
||||
pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
|
||||
|
||||
xs = pnts[:, 0]
|
||||
ys = pnts[:, 1]
|
||||
|
|
@ -251,11 +261,24 @@ def mapsd_thread():
|
|||
dat.liveMapData.wayId = cur_way.id
|
||||
|
||||
# Seed limit
|
||||
max_speed = cur_way.max_speed
|
||||
max_speed = cur_way.max_speed()
|
||||
if max_speed is not None:
|
||||
dat.liveMapData.speedLimitValid = True
|
||||
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
|
||||
dat.liveMapData.curvatureValid = curvature_valid
|
||||
dat.liveMapData.curvature = float(upcoming_curvature)
|
||||
|
|
|
|||
|
|
@ -1,13 +1,23 @@
|
|||
import math
|
||||
import json
|
||||
import numpy as np
|
||||
from datetime import datetime
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.transformations.coordinates import LocalCoord, geodetic2ecef
|
||||
|
||||
LOOKAHEAD_TIME = 10.
|
||||
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):
|
||||
"""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)))
|
||||
|
||||
|
||||
def parse_speed_unit(max_speed):
|
||||
"""Converts a maxspeed string to m/s based on the unit present in the input.
|
||||
OpenStreetMap defaults to kph if no unit is present. """
|
||||
|
||||
if not max_speed:
|
||||
return None
|
||||
|
||||
conversion = CV.KPH_TO_MS
|
||||
if 'mph' in max_speed:
|
||||
max_speed = max_speed.replace(' mph', '')
|
||||
conversion = CV.MPH_TO_MS
|
||||
|
||||
try:
|
||||
max_speed = float(max_speed) * conversion
|
||||
return float(max_speed) * conversion
|
||||
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
|
||||
|
||||
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:
|
||||
def __init__(self, way):
|
||||
def __init__(self, way, query_results):
|
||||
self.id = way.id
|
||||
self.way = way
|
||||
self.query_results = query_results
|
||||
|
||||
points = list()
|
||||
|
||||
|
|
@ -55,7 +127,7 @@ class Way:
|
|||
|
||||
@classmethod
|
||||
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))
|
||||
nodes = tree.query_ball_point(cur_pos, 500)
|
||||
|
|
@ -73,7 +145,7 @@ class Way:
|
|||
closest_way = None
|
||||
best_score = None
|
||||
for way in ways:
|
||||
way = Way(way)
|
||||
way = Way(way, query_results)
|
||||
points = way.points_in_car_frame(lat, lon, heading)
|
||||
|
||||
on_way = way.on_way(lat, lon, heading, points)
|
||||
|
|
@ -124,35 +196,65 @@ class Way:
|
|||
def __str__(self):
|
||||
return "%s %s" % (self.id, self.way.tags)
|
||||
|
||||
@property
|
||||
def max_speed(self):
|
||||
"""Extracts the (conditional) speed limit from a way"""
|
||||
if not self.way:
|
||||
return None
|
||||
|
||||
tags = self.way.tags
|
||||
max_speed = None
|
||||
|
||||
if 'maxspeed' in tags:
|
||||
max_speed = parse_speed_unit(tags['maxspeed'])
|
||||
|
||||
try:
|
||||
if 'maxspeed:conditional' in tags:
|
||||
max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ')
|
||||
cond = cond[1:-1]
|
||||
|
||||
start, end = cond.split('-')
|
||||
now = datetime.now() # TODO: Get time and timezone from gps fix so this will work correctly on replays
|
||||
start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
|
||||
end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
|
||||
|
||||
if start <= now <= end:
|
||||
max_speed = parse_speed_unit(max_speed_cond)
|
||||
except ValueError:
|
||||
pass
|
||||
max_speed = parse_speed_tags(self.way.tags)
|
||||
if not max_speed:
|
||||
location_info = self.query_results[4]
|
||||
max_speed = geocode_maxspeed(self.way.tags, location_info)
|
||||
|
||||
return max_speed
|
||||
|
||||
def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
|
||||
"""Look ahead for a max speed"""
|
||||
if not self.way:
|
||||
return None
|
||||
|
||||
speed_ahead = None
|
||||
speed_ahead_dist = None
|
||||
lookahead_ways = 5
|
||||
way = self
|
||||
for i in range(lookahead_ways):
|
||||
way_pts = way.points_in_car_frame(lat, lon, heading)
|
||||
|
||||
# Check current lookahead distance
|
||||
max_dist = np.linalg.norm(way_pts[-1, :])
|
||||
|
||||
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):
|
||||
if points is None:
|
||||
points = self.points_in_car_frame(lat, lon, heading)
|
||||
|
|
@ -186,8 +288,8 @@ class Way:
|
|||
|
||||
return points_carframe
|
||||
|
||||
def next_way(self, query_results, lat, lon, heading, backwards=False):
|
||||
results, tree, real_nodes, node_to_way = query_results
|
||||
def next_way(self, backwards=False):
|
||||
results, tree, real_nodes, node_to_way, location_info = self.query_results
|
||||
|
||||
if backwards:
|
||||
node = self.way.nodes[0]
|
||||
|
|
@ -215,18 +317,20 @@ class Way:
|
|||
# Filter on number of lanes
|
||||
cur_num_lanes = int(self.way.tags['lanes'])
|
||||
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:
|
||||
ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes]
|
||||
if len(ways) == 1:
|
||||
way = Way(ways[0])
|
||||
way = Way(ways[0], self.query_results)
|
||||
|
||||
except (KeyError, ValueError):
|
||||
pass
|
||||
|
||||
return way
|
||||
|
||||
def get_lookahead(self, query_results, lat, lon, heading, lookahead):
|
||||
def get_lookahead(self, lat, lon, heading, lookahead):
|
||||
pnts = None
|
||||
way = self
|
||||
valid = False
|
||||
|
|
@ -249,7 +353,7 @@ class Way:
|
|||
break
|
||||
|
||||
# Find next way
|
||||
way = way.next_way(query_results, lat, lon, heading)
|
||||
way = way.next_way()
|
||||
if not way:
|
||||
break
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:f21b25b1bede44778eadba4e647eaace2a91617518d80a738947d4301ff54733
|
||||
oid sha256:a912e6346d7da8591acc72b5aa2d2382cb815d44f8567c656097ac180fe846b6
|
||||
size 1171544
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:10fb68f4fef47fa7a16bfdeca0c262c4c6aab6bbde3693122baf177e12da4679
|
||||
oid sha256:09b15277bb78a39ef3734b4a4773e8fed6431541395385ea49b78fbb1c3f37c4
|
||||
size 1159016
|
||||
|
|
|
|||
|
|
@ -72,6 +72,9 @@ orbFeaturesSummary: [8062, true]
|
|||
driverMonitoring: [8063, true]
|
||||
liveParameters: [8064, true]
|
||||
liveMapData: [8065, true]
|
||||
cameraOdometry: [8066, true]
|
||||
pathPlan: [8067, true]
|
||||
kalmanOdometry: [8068, true]
|
||||
|
||||
testModel: [8040, false]
|
||||
testLiveLocation: [8045, false]
|
||||
|
|
|
|||
|
|
@ -293,14 +293,17 @@ class LongitudinalControl(unittest.TestCase):
|
|||
manager.gctx = {}
|
||||
manager.prepare_managed_process('radard')
|
||||
manager.prepare_managed_process('controlsd')
|
||||
manager.prepare_managed_process('plannerd')
|
||||
|
||||
manager.start_managed_process('radard')
|
||||
manager.start_managed_process('controlsd')
|
||||
manager.start_managed_process('plannerd')
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
manager.kill_managed_process('radard')
|
||||
manager.kill_managed_process('controlsd')
|
||||
manager.kill_managed_process('plannerd')
|
||||
time.sleep(5)
|
||||
|
||||
# hack
|
||||
|
|
@ -321,4 +324,3 @@ for k in xrange(WORKERS):
|
|||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
||||
|
|
|
|||
|
|
@ -97,7 +97,7 @@ else
|
|||
OPENGL_LIBS = -lGLESv3 -lEGL
|
||||
|
||||
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
|
||||
|
||||
|
|
@ -137,7 +137,7 @@ OBJS += $(PLATFORM_OBJS) \
|
|||
$(CEREAL_OBJS)
|
||||
|
||||
#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)
|
||||
OBJS += $(MODEL_OBJS)
|
||||
|
||||
|
|
|
|||
|
|
@ -49,6 +49,8 @@
|
|||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#define M_PI 3.14159265358979323846
|
||||
|
||||
#define UI_BUF_COUNT 4
|
||||
|
||||
//#define DUMP_RGB
|
||||
|
|
@ -166,6 +168,9 @@ struct VisionState {
|
|||
zsock_t *recorder_sock;
|
||||
void* recorder_sock_raw;
|
||||
|
||||
zsock_t *posenet_sock;
|
||||
void* posenet_sock_raw;
|
||||
|
||||
pthread_mutex_t clients_lock;
|
||||
VisionClientState clients[MAX_CLIENTS];
|
||||
|
||||
|
|
@ -894,6 +899,15 @@ void* frontview_thread(void *arg) {
|
|||
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) {
|
||||
int err;
|
||||
VisionState *s = (VisionState*)arg;
|
||||
|
|
@ -924,6 +938,14 @@ void* processing_thread(void *arg) {
|
|||
FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb");
|
||||
#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
|
||||
LOG("processing start!");
|
||||
|
||||
|
|
@ -947,11 +969,6 @@ void* processing_thread(void *arg) {
|
|||
|
||||
int ui_idx = tbuffer_select(&s->ui_tb);
|
||||
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;
|
||||
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);
|
||||
|
||||
// auto exposure over big box
|
||||
|
|
@ -1319,6 +1419,10 @@ int main(int argc, char **argv) {
|
|||
assert(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]);
|
||||
|
||||
if (test_run) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue