mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 19:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.5.1 release

Browse Source 
old-commit-hash: 6f3d10a4c4
This commit is contained in:
Vehicle Researcher
2018-08-02 02:58:52 +00:00
parent a61e12f777
commit 86d4bbcba6
40 changed files with 583 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/assets/img_driver_face.png
+3
View File
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:9a9b85f959e3881c65d018486ae2a6c41b25b4586cc61b0caad27d211fb517d9
size 24112
selfdrive/car/ford/interface.py
+2 -2
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
from cereal import car, log
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
@@ -209,7 +209,7 @@ class CarInterface(object):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
def apply(self, c, perception_state=log.Live20Data.new_message()):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators,
c.hudControl.visualAlert, c.cruiseControl.cancel)
selfdrive/car/gm/carstate.py
+3 -2
View File
@@ -4,7 +4,8 @@ from common.kalman.simple_kalman import KF1D
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
CruiseButtons, is_eps_status_ok, \
STEER_THRESHOLD
def get_powertrain_can_parser(CP, canbus):
# this function generates lists for signal, messages and initial values
@@ -91,7 +92,7 @@ class CarState(object):
self.user_gas_pressed = self.pedal_gas > 0
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
self.steer_override = abs(self.steer_torque_driver) > 1.0
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
selfdrive/car/gm/interface.py
+2 -2
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python
from cereal import car
from cereal import car, log
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
@@ -308,7 +308,7 @@ class CarInterface(object):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
def apply(self, c, perception_state=log.Live20Data.new_message()):
hud_v_cruise = c.hudControl.setSpeed
if hud_v_cruise > 70:
hud_v_cruise = 0
selfdrive/car/gm/values.py
+1
View File
@@ -47,6 +47,7 @@ FINGERPRINTS = {
}],
}
STEER_THRESHOLD = 1.0
DBC = {
CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
selfdrive/car/honda/carcontroller.py
+7 -3
View File
@@ -1,3 +1,4 @@
from cereal import car
from collections import namedtuple
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.controls.lib.drive_helpers import rate_limit
@@ -61,11 +62,12 @@ class CarController(object):
self.brake_last = 0.
self.enable_camera = enable_camera
self.packer = CANPacker(dbc_name)
self.new_radar_config = False
def update(self, sendcan, enabled, CS, frame, actuators, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
snd_beep, snd_chime):
radar_error, hud_v_cruise, hud_show_lanes, hud_show_car, \
hud_alert, snd_beep, snd_chime):
""" Controls thread """
@@ -167,6 +169,8 @@ class CarController(object):
if (frame % radar_send_step) == 0:
idx = (frame/radar_send_step) % 4
can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, idx))
if not self.new_radar_config: # only change state once
self.new_radar_config = car.RadarState.Error.wrongConfig in radar_error
can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.CP.carFingerprint, self.new_radar_config, idx))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
selfdrive/car/honda/carstate.py
+2 -4
View File
@@ -2,7 +2,7 @@ from common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import CAR, DBC
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD
def parse_gear_shifter(can_gear_shifter, car_fingerprint):
@@ -284,10 +284,8 @@ class CarState(object):
else:
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
#rdx has different steer override threshold
steer_thrsld = 400 if self.CP.carFingerprint == CAR.ACURA_RDX else 1200
self.steer_override = abs(cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']) > steer_thrsld
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
selfdrive/car/honda/hondacan.py
+3 -2
View File
@@ -117,7 +117,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
return commands
def create_radar_commands(v_ego, car_fingerprint, idx):
def create_radar_commands(v_ego, car_fingerprint, new_radar_config, idx):
"""Creates an iterable of CAN messages for the radar system."""
commands = []
v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255)
@@ -129,7 +129,8 @@ def create_radar_commands(v_ego, car_fingerprint, idx):
if car_fingerprint == CAR.CIVIC:
msg_0x301 = "\x02\x38\x44\x32\x4f\x00\x00"
commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1)) # add 8 on idx.
idx_offset = 0xc if new_radar_config else 0x8 # radar in civic 2018 requires 0xc
commands.append(make_can_msg(0x300, msg_0x300, idx + idx_offset, 1))
else:
if car_fingerprint == CAR.CRV:
msg_0x301 = "\x00\x00\x50\x02\x51\x00\x00"
selfdrive/car/honda/interface.py
+3 -2
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python
import os
import numpy as np
from cereal import car
from cereal import car, log
from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
@@ -567,7 +567,7 @@ class CarInterface(object):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
def apply(self, c, perception_state=log.Live20Data.new_message()):
if c.hudControl.speedVisible:
hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
else:
@@ -600,6 +600,7 @@ class CarInterface(object):
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, \
selfdrive/car/honda/radar_interface.py
+4
View File
@@ -28,6 +28,7 @@ class RadarInterface(object):
self.pts = {}
self.track_id = 0
self.radar_fault = False
self.radar_wrong_config = False
self.radar_off_can = CP.radarOffCan
self.delay = 0.1 # Delay of radar
@@ -61,6 +62,7 @@ class RadarInterface(object):
if ii == 0x400:
# check for radar faults
self.radar_fault = cpt['RADAR_STATE'] != 0x79
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
elif cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
@@ -81,6 +83,8 @@ class RadarInterface(object):
errors.append("commIssue")
if self.radar_fault:
errors.append("fault")
if self.radar_wrong_config:
errors.append("wrongConfig")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
selfdrive/car/honda/values.py
+14
View File
@@ -104,5 +104,19 @@ DBC = {
CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'),
}
STEER_THRESHOLD = {
CAR.ACCORD: 1200,
CAR.ACURA_ILX: 1200,
CAR.ACURA_RDX: 400,
CAR.CIVIC: 1200,
CAR.CIVIC_HATCH: 1200,
CAR.CRV: 1200,
CAR.CRV_5G: 1200,
CAR.ODYSSEY: 1200,
CAR.PILOT: 1200,
CAR.RIDGELINE: 1200,
}
# TODO: get these from dbc file
HONDA_BOSCH = [CAR.ACCORD, CAR.CIVIC_HATCH, CAR.CRV_5G]
selfdrive/car/mock/interface.py
+2 -2
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env python
import zmq
from cereal import car
from cereal import car, log
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
@@ -117,6 +117,6 @@ class CarInterface(object):
return ret.as_reader()
def apply(self, c):
def apply(self, c, perception_state=log.Live20Data.new_message()):
# in mock no carcontrols
return False
selfdrive/car/toyota/carcontroller.py
+2 -2
View File
@@ -4,7 +4,7 @@ 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
from selfdrive.car.toyota.values import ECU, STATIC_MSGS
from selfdrive.car.toyota.values import ECU, STATIC_MSGS, NO_DSU_CAR
from selfdrive.can.packer import CANPacker
# Accel limits
@@ -209,7 +209,7 @@ class CarController(object):
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus:
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and self.car_fingerprint not in NO_DSU_CAR:
for addr in TARGET_IDS:
can_sends.append(create_video_target(frame/10, addr))
selfdrive/car/toyota/carstate.py
+5 -5
View File
@@ -1,4 +1,4 @@
from selfdrive.car.toyota.values import CAR, DBC
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from common.kalman.simple_kalman import KF1D
@@ -7,7 +7,7 @@ import numpy as np
def parse_gear_shifter(can_gear, car_fingerprint):
# TODO: Use values from DBC to parse this field
if car_fingerprint == CAR.PRIUS:
if car_fingerprint in [CAR.PRIUS, CAR.CHRH, CAR.CAMRYH]:
if can_gear == 0x0:
return "park"
elif can_gear == 0x1:
@@ -19,7 +19,7 @@ def parse_gear_shifter(can_gear, car_fingerprint):
elif can_gear == 0x4:
return "brake"
elif car_fingerprint in [CAR.RAV4, CAR.RAV4H,
CAR.LEXUS_RXH, CAR.COROLLA]:
CAR.LEXUS_RXH, CAR.COROLLA, CAR.CHR, CAR.CAMRY]:
if can_gear == 0x20:
return "park"
elif can_gear == 0x10:
@@ -147,8 +147,6 @@ class CarState(object):
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']) > 100
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
@@ -156,6 +154,8 @@ class CarState(object):
self.brake_error = 0
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
# we could use the override bit from dbc, but it's triggered at too high torque values
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
self.user_brake = 0
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
selfdrive/car/toyota/interface.py
+26 -8
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env python
from common.realtime import sec_since_boot
from cereal import car
from cereal import car, log
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
@@ -75,8 +75,8 @@ class CarInterface(object):
if candidate == CAR.PRIUS:
ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.wheelbase = 2.70
ret.steerRatio = 15.59 # unknown end-to-end spec
tire_stiffness_factor = 0.7933
ret.steerRatio = 15.00 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045 * CV.LB_TO_KG + std_cargo
ret.steerKpV, ret.steerKiV = [[0.4], [0.01]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
@@ -110,6 +110,24 @@ class CarInterface(object):
ret.steerKpV, ret.steerKiV = [[0.6], [0.1]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
elif candidate in [CAR.CHR, CAR.CHRH]:
ret.safetyParam = 100
ret.wheelbase = 2.63906
ret.steerRatio = 13.6
tire_stiffness_factor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG + std_cargo
ret.steerKpV, ret.steerKiV = [[0.723], [0.0428]]
ret.steerKf = 0.00006
elif candidate in [CAR.CAMRY, CAR.CAMRYH]:
ret.safetyParam = 100
ret.wheelbase = 2.82448
ret.steerRatio = 13.7
tire_stiffness_factor = 0.7933
ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid
ret.steerKpV, ret.steerKiV = [[0.6], [0.1]]
ret.steerKf = 0.00006
ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44
@@ -118,7 +136,7 @@ class CarInterface(object):
# 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.
if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH]: # rav4 hybrid can do stop and go
if candidate in [CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]: # rav4 hybrid can do stop and go
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
@@ -150,9 +168,9 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.]
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = not check_ecu_msgs(fingerprint, candidate, ECU.CAM)
ret.enableDsu = not check_ecu_msgs(fingerprint, candidate, ECU.DSU)
ret.enableApgs = False #not check_ecu_msgs(fingerprint, candidate, ECU.APGS)
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU)
ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu)
cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs)
@@ -304,7 +322,7 @@ class CarInterface(object):
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
def apply(self, c, perception_state=log.Live20Data.new_message()):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
selfdrive/car/toyota/radar_interface.py
+11 -4
View File
@@ -1,11 +1,13 @@
#!/usr/bin/env python
import os
import zmq
import time
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
import zmq
from selfdrive.services import service_list
import selfdrive.messaging as messaging
from selfdrive.car.toyota.values import NO_DSU_CAR
RADAR_MSGS = list(range(0x210, 0x220))
@@ -30,15 +32,21 @@ class RadarInterface(object):
self.delay = 0.0 # Delay of radar
# Nidec
self.rcp = _create_radard_can_parser()
self.no_dsu_car = CP.carFingerprint in NO_DSU_CAR
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
def update(self):
canMonoTimes = []
ret = car.RadarState.new_message()
if self.no_dsu_car:
# TODO: make a adas dbc file for dsu-less models
time.sleep(0.05)
return ret
canMonoTimes = []
updated_messages = set()
while 1:
tm = int(sec_since_boot() * 1e9)
@@ -47,7 +55,6 @@ class RadarInterface(object):
if 0x21f in updated_messages:
break
ret = car.RadarState.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
selfdrive/car/toyota/values.py
+36 -7
View File
@@ -2,10 +2,14 @@ from selfdrive.car import dbc_dict
class CAR:
PRIUS = "TOYOTA PRIUS 2017"
RAV4H = "TOYOTA RAV4 2017 HYBRID"
RAV4H = "TOYOTA RAV4 HYBRID 2017"
RAV4 = "TOYOTA RAV4 2017"
COROLLA = "TOYOTA COROLLA 2017"
LEXUS_RXH = "LEXUS RX HYBRID 2017"
CHR = "TOYOTA C-HR 2018"
CHRH = "TOYOTA C-HR HYBRID 2018"
CAMRY = "TOYOTA CAMRY 2018"
CAMRYH = "TOYOTA CAMRY HYBRID 2018"
class ECU:
@@ -59,14 +63,16 @@ STATIC_MSGS = [(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4,
(0x470, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'),
]
ECU_FINGERPRINT = {
ECU.CAM: 0x2e4, # steer torque cmd
ECU.DSU: 0x343, # accel cmd
ECU.APGS: 0x835, # angle cmd
}
def check_ecu_msgs(fingerprint, candidate, ecu):
def check_ecu_msgs(fingerprint, ecu):
# return True if fingerprint contains messages normally sent by a given ecu
ecu_msgs = [x[0] for x in STATIC_MSGS if (x[1] == ecu and
candidate in x[2] and
x[3] == 0)]
return any(msg for msg in fingerprint if msg in ecu_msgs)
return ECU_FINGERPRINT[ecu] in fingerprint
FINGERPRINTS = {
@@ -97,8 +103,25 @@ FINGERPRINTS = {
CAR.LEXUS_RXH: [{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
}],
CAR.CHR: [{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 740: 5, 800: 8, 810: 2, 812: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8
}],
CAR.CHRH: [{
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, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 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, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8,1059: 1, 1071: 8, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 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, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.CAMRY: [
#XLE and LE
{
36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.CAMRYH: [
#LE
{
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, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 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, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 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, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572:8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
}
STEER_THRESHOLD = 100
DBC = {
CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'),
@@ -106,4 +129,10 @@ DBC = {
CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.CHR: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.CHRH: dbc_dict('toyota_chr_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.CAMRY: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'),
}
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]
selfdrive/common/efd.c
+56
View File
@@ -0,0 +1,56 @@
#include <stdlib.h>
#include <assert.h>
#ifdef __linux__
#include <sys/eventfd.h>
#else
#include <sys/time.h>
#include <sys/event.h>
#define EVENT_IDENT 42
#endif
#include "efd.h"
int efd_init() {
#ifdef __linux__
return eventfd(0, EFD_CLOEXEC);
#else
int fd = kqueue();
assert(fd >= 0);
struct kevent kev;
EV_SET(&kev, EVENT_IDENT, EVFILT_USER, EV_ADD | EV_CLEAR, 0, 0, NULL);
struct timespec timeout = {0, 0};
int err = kevent(fd, &kev, 1, NULL, 0, &timeout);
assert(err != -1);
return fd;
#endif
}
void efd_write(int fd) {
#ifdef __linux__
eventfd_write(fd, 1);
#else
struct kevent kev;
EV_SET(&kev, EVENT_IDENT, EVFILT_USER, 0, NOTE_TRIGGER, 0, NULL);
struct timespec timeout = {0, 0};
int err = kevent(fd, &kev, 1, NULL, 0, &timeout);
assert(err != -1);
#endif
}
void efd_clear(int fd) {
#ifdef __linux__
eventfd_t efd_cnt;
eventfd_read(fd, &efd_cnt);
#else
struct kevent kev;
struct timespec timeout = {0, 0};
int nfds = kevent(fd, NULL, 0, &kev, 1, &timeout);
assert(nfds != -1);
#endif
}
selfdrive/common/efd.h
+17
View File
@@ -0,0 +1,17 @@
#ifndef EFD_H
#define EFD_H
#ifdef __cplusplus
extern "C" {
#endif
// event fd: a semaphore that can be poll()'d
int efd_init();
void efd_write(int fd);
void efd_clear(int fd);
#ifdef __cplusplus
}
#endif
#endif
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.5-release"
#define COMMA_VERSION "0.5.1-release"
selfdrive/controls/controlsd.py
+5 -5
View File
@@ -292,7 +292,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
return actuators, v_cruise_kph, driver_status, angle_offset
def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
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):
@@ -323,7 +323,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
CI.apply(CC, perception_state)
# ***** publish state to logger *****
# publish controls state at 100Hz
@@ -337,6 +337,7 @@ def data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_
"alertStatus": AM.alert_status,
"alertBlinkingRate": AM.alert_rate,
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn": bool(driver_status.monitor_on),
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": plan_ts,
"enabled": isEnabled(state),
@@ -446,7 +447,6 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
CP.safetyModel = car.CarParams.SafetyModels.noOutput
fcw_enabled = params.get("IsFcwEnabled") == "1"
driver_monitor_on = params.get("IsDriverMonitoringEnabled") == "1"
geofence = None
try:
from selfdrive.controls.lib.geofence import Geofence
@@ -460,7 +460,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
VM = VehicleModel(CP)
LaC = LatControl(VM)
AM = AlertManager()
driver_status = DriverStatus(driver_monitor_on)
driver_status = DriverStatus()
if not passive:
AM.add("startup", False)
@@ -516,7 +516,7 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.):
prof.checkpoint("State Control")
# publish data
CC = data_send(plan, plan_ts, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
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)
prof.checkpoint("Sent")
selfdrive/controls/lib/alertmanager.py
+34 -4
View File
@@ -74,7 +74,7 @@ class AlertManager(object):
Priority.MID, None, "beepSingle", .2, 0., 0.),
"fcw": Alert(
"Brake!",
"BRAKE!",
"Risk of Collision",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, "fcw", "chimeRepeated", 1., 2., 2.),
@@ -98,23 +98,53 @@ class AlertManager(object):
Priority.LOW, None, None, .2, .2, .2),
"preDriverDistracted": Alert(
"TAKE CONTROL: User Appears Distracted",
"KEEP EYES ON ROAD: User Appears Distracted",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, "steerRequired", None, 0., .1, .1, alert_rate=0.75),
"promptDriverDistracted": Alert(
"TAKE CONTROL",
"KEEP EYES ON ROAD",
"User Appears Distracted",
AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
"driverDistracted": Alert(
"DISENGAGEMENT REQUIRED",
"DISENGAGE IMMEDIATELY",
"User Was Distracted",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1),
"preDriverUnresponsive": Alert(
"TOUCH STEERING WHEEL: No Driver Monitoring",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, "steerRequired", None, 0., .1, .1, alert_rate=0.75),
"promptDriverUnresponsive": Alert(
"TOUCH STEERING WHEEL",
"User Is Unresponsive",
AlertStatus.userPrompt, AlertSize.mid,
Priority.MID, "steerRequired", "chimeRepeated", .1, .1, .1),
"driverUnresponsive": Alert(
"DISENGAGE IMMEDIATELY",
"User Was Unresponsive",
AlertStatus.critical, AlertSize.full,
Priority.HIGH, "steerRequired", "chimeRepeated", .1, .1, .1),
"driverMonitorOff": Alert(
"DRIVER MONITOR IS UNAVAILABLE",
"Accuracy Is Low",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, None, None, .4, 0., 4.),
"driverMonitorOn": Alert(
"DRIVER MONITOR IS AVAILABLE",
"Accuracy Is High",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, None, None, .4, 0., 4.),
"geofence": Alert(
"DISENGAGEMENT REQUIRED",
"Not in Geofenced Area",
selfdrive/controls/lib/driver_monitor.py
+75 -15
View File
@@ -18,12 +18,22 @@ _CAMERA_X_CONV = 0.375 # 160*864/320/1152
_PITCH_WEIGHT = 1.5 # pitch matters a lot more
_METRIC_THRESHOLD = 0.4
_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch
_DTM = 0.2 # driver monitor runs at 5Hz
_DISTRACTED_FILTER_F = 0.3 # 0.3Hz
_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM)
_DTM = 0.1 # driver monitor runs at 10Hz
_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid
_DISTRACTED_FILTER_F = 0.6 # 0.6Hz, 0.25s ts
_DISTRACTED_FILTER_K = 2 * np.pi * _DISTRACTED_FILTER_F * _DTM / (1 + 2 * np.pi * _DISTRACTED_FILTER_F * _DTM)
_VARIANCE_FILTER_F = 0.008 # 0.008Hz, 20s ts
_VARIANCE_FILTER_K = 2 * np.pi * _VARIANCE_FILTER_F * _DTM / (1 + 2 * np.pi * _VARIANCE_FILTER_F * _DTM)
class MonitorChangeEvent:
NO_CHANGE = 0
PARAM_ON = 1
PARAM_OFF = 2
VARIANCE_HIGH = 3
VARIANCE_LOW = 4
class _DriverPose():
def __init__(self):
self.yaw = 0.
@@ -32,16 +42,30 @@ class _DriverPose():
self.yaw_offset = 0.
self.pitch_offset = 0.
def _monitor_hysteresys(variance_level, monitor_valid_prev):
var_thr = 0.63 if monitor_valid_prev else 0.37
return variance_level < var_thr
class DriverStatus():
def __init__(self, monitor_on):
def __init__(self, monitor_on=False):
self.pose = _DriverPose()
self.monitor_on = monitor_on
self.monitor_param_on = monitor_on
self.monitor_valid = True # variance needs to be low
self.monitor_change_event = MonitorChangeEvent.NO_CHANGE
self.awareness = 1.
self.driver_distracted = False
self.driver_distraction_level = 0.
self.variance_high = False
self.variance_level = 0.
self.ts_last_check = 0.
self._set_timers()
def _reset_filters(self):
self.driver_distraction_level = 0.
self.variance_level = 0.
self.monitor_valid = True
def _set_timers(self):
if self.monitor_on:
self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME
@@ -64,14 +88,23 @@ class DriverStatus():
#print "%02.4f" % np.degrees(pose.pitch), "%02.4f" % np.degrees(pitch_error), "%03.4f" % np.degrees(pose.pitch_offset), metric
return 1 if metric > _METRIC_THRESHOLD else 0
def get_pose(self, driver_monitoring, params):
ts = sec_since_boot()
# don's check for param too often as it's a kernel call
if ts - self.ts_last_check > 1.:
self.monitor_on = params.get("IsDriverMonitoringEnabled") == "1"
self._set_timers()
self.ts_last_check = ts
def _monitor_change_check(self, monitor_param_on_prev, monitor_valid_prev, monitor_on_prev):
if not monitor_param_on_prev and self.monitor_param_on:
self._reset_filters()
return MonitorChangeEvent.PARAM_ON
elif monitor_param_on_prev and not self.monitor_param_on:
self._reset_filters()
return MonitorChangeEvent.PARAM_OFF
elif monitor_on_prev and not self.monitor_valid:
return MonitorChangeEvent.VARIANCE_HIGH
elif self.monitor_param_on and not monitor_valid_prev and self.monitor_valid:
return MonitorChangeEvent.VARIANCE_LOW
else:
return MonitorChangeEvent.NO_CHANGE
def get_pose(self, driver_monitoring, params):
self.pose.pitch = driver_monitoring.descriptor[0]
self.pose.yaw = driver_monitoring.descriptor[1]
@@ -79,9 +112,28 @@ class DriverStatus():
self.pose.yaw_offset = (driver_monitoring.descriptor[3] * _CAMERA_X_CONV + _CAMERA_OFFSET_X) * _CAMERA_FOV_X
self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y # positive y is down
self.driver_distracted = self._is_driver_distracted(self.pose)
# first order filter
# first order filters
self.driver_distraction_level = (1. - _DISTRACTED_FILTER_K) * self.driver_distraction_level + \
_DISTRACTED_FILTER_K * self.driver_distracted
self.variance_high = driver_monitoring.std > _STD_THRESHOLD
self.variance_level = (1. - _VARIANCE_FILTER_K) * self.variance_level + \
_VARIANCE_FILTER_K * self.variance_high
monitor_param_on_prev = self.monitor_param_on
monitor_valid_prev = self.monitor_valid
monitor_on_prev = self.monitor_on
# don't check for param too often as it's a kernel call
ts = sec_since_boot()
if ts - self.ts_last_check > 1.:
self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
self.ts_last_check = ts
self.monitor_valid = _monitor_hysteresys(self.variance_level, monitor_valid_prev)
self.monitor_on = self.monitor_valid and self.monitor_param_on
self.monitor_change_event = self._monitor_change_check(monitor_param_on_prev, monitor_valid_prev, monitor_on_prev)
self._set_timers()
def update(self, events, driver_engaged, ctrl_active, standstill):
@@ -90,20 +142,28 @@ class DriverStatus():
if (driver_engaged and self.awareness > 0.) or not ctrl_active:
# always reset if driver is in control (unless we are in red alert state) or op isn't active
self.awareness = 1.
if not ctrl_active:
# hold monitor_level constant when control isn't active
self.variance_level = 0. if self.monitor_valid else 1.
if (not self.monitor_on or (self.driver_distraction_level > 0.63 and self.driver_distracted)) and \
not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
self.awareness = max(self.awareness - self.step_change, -0.1)
alert = None
if self.awareness <= 0.:
# terminal red alert: disengagement required
events.append(create_event('driverDistracted', [ET.WARNING]))
alert = 'driverDistracted' if self.monitor_on else 'driverUnresponsive'
elif self.awareness <= self.threshold_prompt:
# prompt orange alert
events.append(create_event('promptDriverDistracted', [ET.WARNING]))
alert = 'promptDriverDistracted' if self.monitor_on else 'promptDriverUnresponsive'
elif self.awareness <= self.threshold_pre:
# pre green alert
events.append(create_event('preDriverDistracted', [ET.WARNING]))
alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
if alert is not None:
events.append(create_event(alert, [ET.WARNING]))
self.monitor_change_event = MonitorChangeEvent.NO_CHANGE
return events
selfdrive/controls/lib/planner.py
+5 -3
View File
@@ -1,11 +1,11 @@
#!/usr/bin/env python
import os
import zmq
import numpy as np
import math
import numpy as np
from copy import copy
from cereal import log
from collections import defaultdict
from common.realtime import sec_since_boot
from common.numpy_fast import interp
import selfdrive.messaging as messaging
@@ -303,6 +303,7 @@ class Planner(object):
self.last_gps_planner_plan = None
self.gps_planner_active = False
self.perception_state = log.Live20Data.new_message()
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
@@ -374,6 +375,7 @@ class Planner(object):
self.PP.c_prob = 1.0
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
selfdrive/controls/radard.py
+4 -1
View File
@@ -26,6 +26,7 @@ DIMSV = 2
XV, SPEEDV = 0, 1
VISION_POINT = -1
class EKFV1D(EKF):
def __init__(self):
super(EKFV1D, self).__init__(False)
@@ -137,7 +138,8 @@ def radard_thread(gctx=None):
# run kalman filter only if prob is high enough
if PP.lead_prob > 0.7:
ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
reading = speedSensorV.read(PP.lead_dist, covar=np.matrix(PP.lead_var))
ekfv.update_scalar(reading)
ekfv.predict(tsv)
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
float(ekfv.state[SPEEDV]), False)
@@ -145,6 +147,7 @@ def radard_thread(gctx=None):
ekfv.state[XV] = PP.lead_dist
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
ekfv.state[SPEEDV] = 0.
if VISION_POINT in ar_pts:
del ar_pts[VISION_POINT]
selfdrive/loggerd/loggerd
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:819e9c55b925cdaa7b9f818c09c649926420fb6892f47e4234954961b9d9191c
size 1622728
oid sha256:9102036c0e855b6c95c8579a7f9b447a981b4dd505a06cfc403eeb319bc74295
size 1626840
selfdrive/manager.py
+4 -2
View File
@@ -118,7 +118,6 @@ persistent_processes = [
'uploader',
'ui',
'gpsd',
'ubloxd',
'updated',
]
@@ -129,7 +128,7 @@ car_started_processes = [
'radard',
'visiond',
'proclogd',
'orbd',
'ubloxd',
]
def register_managed_process(name, desc, car_started=False):
@@ -298,6 +297,9 @@ def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
# save boot log
subprocess.call(["./loggerd", "--bootlog"], cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
for p in persistent_processes:
start_managed_process(p)
selfdrive/sensord/gpsd
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a539d59efc3c5009af06ea1dffb2acecbdc3798aaf414d08d0c9cb258e7771cb
oid sha256:9ecc73ffc43883c4407af3c88678262bc0374fb057814fc8b2c09e56619cbe15
size 1171544
selfdrive/sensord/sensord
+1 -1
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:aafc76b775889ecfb96feb3a2072c3b168a0e026cf2c0ad2f603349cdf6ab779
oid sha256:9368e1101d7bcf298761955ae0577aa98235d2f4775278d79d591b4bdcb9f249
size 1159016
selfdrive/thermald.py
+29 -28
View File
@@ -2,7 +2,7 @@
import os
import zmq
from smbus2 import SMBus
from cereal import log
from selfdrive.version import training_version
from selfdrive.swaglog import cloudlog
import selfdrive.messaging as messaging
@@ -10,9 +10,9 @@ from selfdrive.services import service_list
from selfdrive.loggerd.config import ROOT
from common.params import Params
from common.realtime import sec_since_boot
from common.numpy_fast import clip
import cereal
ThermalStatus = cereal.log.ThermalData.ThermalStatus
ThermalStatus = log.ThermalData.ThermalStatus
def read_tz(x):
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
@@ -74,9 +74,9 @@ _FAN_SPEEDS = [0, 16384, 32768, 65535]
# max fan speed only allowed if battery if hot
_BAT_TEMP_THERSHOLD = 45.
def handle_fan(max_temp, bat_temp, fan_speed):
new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_temp)
new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_temp)
def handle_fan(max_cpu_temp, bat_temp, fan_speed):
new_speed_h = next(speed for speed, temp_h in zip(_FAN_SPEEDS, _TEMP_THRS_H) if temp_h > max_cpu_temp)
new_speed_l = next(speed for speed, temp_l in zip(_FAN_SPEEDS, _TEMP_THRS_L) if temp_l > max_cpu_temp)
if new_speed_h > fan_speed:
# update speed if using the high thresholds results in fan speed increment
@@ -164,31 +164,32 @@ def thermald_thread():
msg.thermal.usbOnline = bool(int(f.read()))
# TODO: add car battery voltage check
max_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1,
msg.thermal.cpu2, msg.thermal.cpu3) / 10.0
max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.)
bat_temp = msg.thermal.bat/1000.
fan_speed = handle_fan(max_temp, bat_temp, fan_speed)
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed)
msg.thermal.fanSpeed = fan_speed
# thermal logic here
if max_temp < 70.0:
thermal_status = ThermalStatus.green
if max_temp > 85.0:
cloudlog.warning("over temp: %r", max_temp)
thermal_status = ThermalStatus.yellow
# from controls
overtemp_proc = any(t > 950 for t in
(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2,
msg.thermal.cpu3, msg.thermal.mem, msg.thermal.gpu))
overtemp_bat = msg.thermal.bat > 60000 # 60c
if overtemp_proc or overtemp_bat:
# TODO: hysteresis
thermal_status = ThermalStatus.red
if max_temp > 107.0 or msg.thermal.bat >= 63000:
# thermal logic with hysterisis
if max_cpu_temp > 107. or bat_temp >= 63.:
# onroad not allowed
thermal_status = ThermalStatus.danger
elif max_comp_temp > 95. or bat_temp > 60.:
# hysteresis between onroad not allowed and engage not allowed
thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger)
elif max_cpu_temp > 90.0:
# hysteresis between engage not allowed and uploader not allowed
thermal_status = clip(thermal_status, ThermalStatus.yellow, ThermalStatus.red)
elif max_cpu_temp > 85.0:
# uploader not allowed
thermal_status = ThermalStatus.yellow
elif max_cpu_temp > 75.0:
# hysteresis between uploader not allowed and all good
thermal_status = clip(thermal_status, ThermalStatus.green, ThermalStatus.yellow)
else:
# all good
thermal_status = ThermalStatus.green
# **** starting logic ****
@@ -225,7 +226,7 @@ def thermald_thread():
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
if msg.thermal.thermalStatus >= ThermalStatus.danger:
if thermal_status >= ThermalStatus.danger:
# TODO: Add a better warning when this is happening
should_start = False
selfdrive/ui/ui.c
+49 -9
View File
@@ -65,6 +65,8 @@ const int box_w = vwp_w-sbr_w-(bdr_s*2);
const int box_h = vwp_h-(bdr_s*2);
const int viz_w = vwp_w-(bdr_s*2);
const int header_h = 420;
const int footer_h = 280;
const int footer_y = vwp_h-bdr_s-footer_h;
const uint8_t bg_colors[][4] = {
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
@@ -110,6 +112,7 @@ typedef struct UIScene {
float curvature;
int engaged;
bool engageable;
bool monitoring_active;
bool uilayout_sidebarcollapsed;
bool uilayout_mapenabled;
@@ -158,6 +161,7 @@ typedef struct UIState {
int font_sans_semibold;
int font_sans_bold;
int img_wheel;
int img_face;
zsock_t *thermal_sock;
void *thermal_sock_raw;
@@ -379,6 +383,9 @@ static void ui_init(UIState *s) {
assert(s->img_wheel >= 0);
s->img_wheel = nvgCreateImage(s->vg, "../assets/img_chffr_wheel.png", 1);
assert(s->img_face >= 0);
s->img_face = nvgCreateImage(s->vg, "../assets/img_driver_face.png", 1);
// init gl
s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader);
assert(s->frame_program);
@@ -981,6 +988,31 @@ static void ui_draw_vision_wheel(UIState *s) {
nvgFill(s->vg);
}
static void ui_draw_vision_face(UIState *s) {
const UIScene *scene = &s->scene;
const int face_size = 96;
const int face_x = (scene->ui_viz_rx + face_size + (bdr_s * 2));
const int face_y = (footer_y + ((footer_h - face_size) / 2));
const int face_img_size = (face_size * 1.5);
const int face_img_x = (face_x - (face_img_size / 2));
const int face_img_y = (face_y - (face_size / 4));
float face_img_alpha = scene->monitoring_active ? 1.0f : 0.15f;
float face_bg_alpha = scene->monitoring_active ? 0.3f : 0.1f;
NVGcolor face_bg = nvgRGBA(0, 0, 0, (255 * face_bg_alpha));
NVGpaint face_img = nvgImagePattern(s->vg, face_img_x, face_img_y,
face_img_size, face_img_size, 0, s->img_face, face_img_alpha);
nvgBeginPath(s->vg);
nvgCircle(s->vg, face_x, (face_y + (bdr_s * 1.5)), face_size);
nvgFillColor(s->vg, face_bg);
nvgFill(s->vg);
nvgBeginPath(s->vg);
nvgRect(s->vg, face_img_x, face_img_y, face_img_size, face_img_size);
nvgFillPaint(s->vg, face_img);
nvgFill(s->vg);
}
static void ui_draw_vision_header(UIState *s) {
const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx;
@@ -1000,6 +1032,18 @@ static void ui_draw_vision_header(UIState *s) {
ui_draw_vision_wheel(s);
}
static void ui_draw_vision_footer(UIState *s) {
const UIScene *scene = &s->scene;
int ui_viz_rx = scene->ui_viz_rx;
int ui_viz_rw = scene->ui_viz_rw;
nvgBeginPath(s->vg);
nvgRect(s->vg, ui_viz_rx, footer_y, ui_viz_rw, footer_h);
// Driver Monitoring
ui_draw_vision_face(s);
}
static void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
const char* va_text1, const char* va_text2) {
const UIScene *scene = &s->scene;
@@ -1109,8 +1153,11 @@ static void ui_draw_vision(UIState *s) {
} else if (scene->cal_status == CALIBRATION_UNCALIBRATED) {
// Calibration Status
ui_draw_calibration_status(s);
} else {
ui_draw_vision_footer(s);
}
nvgEndFrame(s->vg);
glDisable(GL_BLEND);
}
@@ -1399,6 +1446,7 @@ static void ui_update(UIState *s) {
s->scene.engaged = datad.enabled;
s->scene.engageable = datad.engageable;
s->scene.gps_planner_active = datad.gpsPlannerActive;
s->scene.monitoring_active = datad.driverMonitoringOn;
// printf("recv %f\n", datad.vEgo);
s->scene.frontview = datad.rearViewCam;
@@ -1634,12 +1682,6 @@ static void* light_sensor_thread(void *args) {
int SENSOR_LIGHT = 7;
struct stat buffer;
if (stat("/sys/bus/i2c/drivers/cyccg", &buffer) == 0) {
LOGD("LeEco light sensor detected");
SENSOR_LIGHT = 5;
}
device->activate(device, SENSOR_LIGHT, 0);
device->activate(device, SENSOR_LIGHT, 1);
device->setDelay(device, SENSOR_LIGHT, ms2ns(100));
@@ -1653,9 +1695,7 @@ static void* light_sensor_thread(void *args) {
LOG_100("light_sensor_poll failed: %d", n);
}
if (n > 0) {
if (SENSOR_LIGHT == 5) s->light_sensor = buffer[0].light * 2;
else s->light_sensor = buffer[0].light;
//printf("new light sensor value: %f\n", s->light_sensor);
s->light_sensor = buffer[0].light;
}
}
selfdrive/visiond/visiond
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a21acff35bda607b6780df7730e005a3b289acfc737a1340a7b9f7a3178d83a0
size 13965736
oid sha256:97110fee2fdc891137a643a7eab38bd269f472658ac05e950fe6dceab306822c
size 12539240