def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
all_valid = CS.canValid and self.sm.all_alive_and_valid()
if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 2.0):
self.initialized = True
Params().put_bool("ControlsReady", True)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
self.controlsAllowed = self.sm['pandaState'].controlsAllowed
if not self.enabled:
self.mismatch_counter = 0
elif not self.controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
return CS
def radard_thread(sm=None, pm=None, can_sock=None):
config_realtime_process(5, Priority.CTRL_LOW)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
f'selfdrive.car.{CP.carName}.radar_interface').RadarInterface
# *** setup messaging
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(
['modelV2', 'carState'], ignore_avg_freq=[
'modelV2', 'carState'
]) # Can't check average frequency, since radar determines timing
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
# TODO: always log leads once we can hide them conditionally
enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(sm, rr, enable_lead)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
if not self.sm['health'].controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
return CS
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
# *** publish radarState and liveTracks
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
has_radar = not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(rk.frame, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message()
dat.init('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def flyingcand_thread(sm=None, pm=None, flying_can_sock=None):
set_realtime_priority(52)
# wait for stats about the car to come in from controls
cloudlog.info("flyingcand is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("flyingcand got CarParams")
# import the wifi interfaces
cloudlog.info("flyingcand is importing flying can interfaces")
flyingCanInterface = importlib.import_module('selfdrive.controls.lib.flyingcan_interface').FlyingCANInterface
if flying_can_sock is None:
flying_can_sock = messaging.sub_sock('flying_can')
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters', 'gpsLocation', 'gpsNMEA', 'sensorEvents'])
# *** publish wifiState and liveTracks
if pm is None:
pm = messaging.PubMaster(['wifiState', 'liveTracks'])
FI = flyingCanInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
FCD = FlyingCanD(CP.radarTimeStep, FI.delay)
has_radar = not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(flying_can_sock, wait_for_one=True)
rr = FI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = FCD.update(rk.frame, sm, rr, has_radar)
dat.radarState.cumLagMs = -rk.remaining*1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = FCD.tracks
dat = messaging.new_message('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def get_ecu_addrs(logcan: messaging.SubSocket,
sendcan: messaging.PubSocket,
queries: Set[Tuple[int, Optional[int], int]],
responses: Set[Tuple[int, Optional[int], int]],
timeout: float = 1,
debug: bool = False) -> Set[Tuple[int, Optional[int], int]]:
ecu_responses: Set[Tuple[int, Optional[int],
int]] = set() # set((addr, subaddr, bus),)
try:
msgs = [
make_tester_present_msg(addr, bus, subaddr)
for addr, subaddr, bus in queries
]
messaging.drain_sock_raw(logcan)
sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan'))
start_time = time.monotonic()
while time.monotonic() - start_time < timeout:
can_packets = messaging.drain_sock(logcan, wait_for_one=True)
for packet in can_packets:
for msg in packet.can:
subaddr = None if (msg.address, None,
msg.src) in responses else msg.dat[0]
if (msg.address, subaddr, msg.src
) in responses and is_tester_present_response(
msg, subaddr):
if debug:
print(
f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}"
)
if (msg.address, subaddr,
msg.src) in ecu_responses:
print(
f"Duplicate ECU address: {hex(msg.address)}"
)
ecu_responses.add((msg.address, subaddr, msg.src))
except Exception:
cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}")
return ecu_responses
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS = self.CI.update(self.CC, can_strs)
self.sm.update(0)
if not self.initialized:
all_valid = CS.canValid and self.sm.all_alive_and_valid()
if all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION:
if not self.read_only:
self.CI.init(self.CP, self.can_sock,
self.pm.sock['sendcan'])
self.initialized = True
if REPLAY and self.sm['pandaStates'][0].controlsAllowed:
self.state = State.enabled
Params().put_bool("ControlsReady", True)
# Check for CAN timeout
if not can_strs:
self.can_rcv_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed
for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
return CS
def sendcan_function(sendcan):
sc = messaging.drain_sock_raw(sendcan)
cp.update_strings(sc, sendcan=True)
if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
else:
brake = 0.0
if cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = cp.vl[0x200]['GAS_COMMAND'] / 256.0
else:
gas = 0.0
if cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0x1000
else:
steer_torque = 0.0
return (gas, brake, steer_torque)
def sendcan_function(sendcan):
sc = messaging.drain_sock_raw(sendcan)
cp.update_strings(sc, sendcan=True)
if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] / 1024.
else:
brake = 0.0
if cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = (cp.vl[0x200]['GAS_COMMAND'] + 83.3) / (0.253984064 * 2**16)
else:
gas = 0.0
if cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = cp.vl[0xe4]['STEER_TORQUE'] / 3840
else:
steer_torque = 0.0
return gas, brake, steer_torque
def test_conflate(self):
sock = random_sock()
pub_sock = messaging.pub_sock(sock)
for conflate in [True, False]:
for _ in range(10):
num_msgs = random.randint(3, 10)
sub_sock = messaging.sub_sock(sock,
conflate=conflate,
timeout=None)
zmq_sleep()
sent_msgs = []
for __ in range(num_msgs):
msg = random_bytes()
pub_sock.send(msg)
sent_msgs.append(msg)
time.sleep(0.1)
recvd_msgs = messaging.drain_sock_raw(sub_sock)
if conflate:
self.assertEqual(len(recvd_msgs), 1)
else:
# TODO: compare actual data
self.assertEqual(len(recvd_msgs), len(sent_msgs))
def data_sample(self):
"""Receive data from sockets and update carState"""
# Update carState from CAN
can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
CS, CS_arne182 = self.CI.update(self.CC, can_strs)
self.sm.update(0)
# Check for CAN timeout
if not can_strs:
self.can_error_counter += 1
self.can_rcv_error = True
else:
self.can_rcv_error = False
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not self.enabled:
self.mismatch_counter = 0
if not self.sm['health'].controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled_now += CS.vEgo * DT_CTRL
self.distance_traveled += CS.vEgo * DT_CTRL
if self.enabled:
self.distance_traveled_engaged += CS.vEgo * DT_CTRL
if CS.steeringPressed:
self.distance_traveled_override += CS.vEgo * DT_CTRL
if (self.sm.frame - self.distance_traveled_frame) * DT_CTRL > 10.0 and not travis:
y = threading.Thread(target=send_params, args=(str(self.distance_traveled),str(self.distance_traveled_engaged),str(self.distance_traveled_override),))
y.start()
self.distance_traveled_frame = self.sm.frame
return CS, CS_arne182
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter,
params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
CS = CI.update(CC, can_strs)
sm.update(0)
events = list(CS.events)
add_lane_change_event(events, sm['pathPlan'])
enabled = isEnabled(state)
lane_change_bsm = sm['pathPlan'].laneChangeBSM
# Check for CAN timeout
if not can_strs:
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
free_space = sm[
'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = sm['thermal'].batteryPercent < 1 and sm[
'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed
mem_low = sm['thermal'].memUsedPercent > 90
#bsm alerts
if lane_change_bsm == LaneChangeBSM.left:
events.append(create_event('preventLCA', [ET.WARNING]))
if lane_change_bsm == LaneChangeBSM.right:
events.append(create_event('preventLCA', [ET.WARNING]))
# Create events for battery, temperature and disk space
if low_battery:
events.append(
create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
if mem_low:
events.append(
create_event('lowMemory',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
if CS.stockAeb:
events.append(create_event('stockAeb', []))
# GPS coords RHD parsing, once every restart
if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude,
sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", "1" if is_rhd else "0")
# Handle calibration
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
cal_rpy = [0, 0, 0]
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationIncomplete',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
rpy = sm['liveCalibration'].rpyCalib
if len(rpy) == 3:
cal_rpy = rpy
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
controls_allowed = sm['health'].controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 200:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if sm.updated['model']:
driver_status.set_policy(sm['model'])
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo,
enabled)
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
return CS, events, cal_perc, mismatch_counter
def steer_thread():
poller = messaging.Poller()
logcan = messaging.sub_sock('can')
health = messaging.sub_sock('health')
joystick_sock = messaging.sub_sock('testJoystick',
conflate=True,
poller=poller)
carstate = messaging.pub_sock('carState')
carcontrol = messaging.pub_sock('carControl')
sendcan = messaging.pub_sock('sendcan')
button_1_last = 0
enabled = False
# wait for health and CAN packets
hw_type = messaging.recv_one(health).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
print("Waiting for CAN messages...")
messaging.get_one_can(logcan)
CI, CP = get_car(logcan, sendcan, has_relay)
Params().put("CarParams", CP.to_bytes())
CC = car.CarControl.new_message()
while True:
# send
joystick = messaging.recv_one(joystick_sock)
can_strs = messaging.drain_sock_raw(logcan, wait_for_one=True)
CS = CI.update(CC, can_strs)
# Usually axis run in pairs, up/down for one, and left/right for
# the other.
actuators = car.CarControl.Actuators.new_message()
if joystick is not None:
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1.,
1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1.,
1.) # -1 to 1
actuators.gas = max(axis_1, 0.)
actuators.brake = max(-axis_1, 0.)
pcm_cancel_cmd = joystick.testJoystick.buttons[0]
button_1 = joystick.testJoystick.buttons[1]
if button_1 and not button_1_last:
enabled = not enabled
button_1_last = button_1
#print "enable", enabled, "steer", actuators.steer, "accel", actuators.gas - actuators.brake
hud_alert = 0
if joystick.testJoystick.buttons[3]:
hud_alert = "steerRequired"
CC.actuators.gas = actuators.gas
CC.actuators.brake = actuators.brake
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.hudControl.visualAlert = hud_alert
CC.hudControl.setSpeed = 20
CC.cruiseControl.cancel = pcm_cancel_cmd
CC.enabled = enabled
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
# broadcast carState
cs_send = messaging.new_message('carState')
cs_send.carState = copy(CS)
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message('carControl')
cc_send.carControl = copy(CC)
carcontrol.send(cc_send.to_bytes())
def data_sample(CI, CC, sm, can_sock, state, mismatch_counter,
can_error_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
CS = CI.update(CC, can_strs)
sm.update(0)
events = list(CS.events)
events += list(sm['dMonitoringState'].events)
add_lane_change_event(events, sm['pathPlan'])
enabled = isEnabled(state)
# Check for CAN timeout
if not can_strs:
can_error_counter += 1
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
free_space = sm[
'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = sm['thermal'].batteryPercent < 1 and sm[
'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed
mem_low = sm['thermal'].memUsedPercent > 90
# Create events for battery, temperature and disk space
if low_battery:
events.append(
create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
if mem_low:
events.append(
create_event('lowMemory',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
if CS.stockAeb:
events.append(create_event('stockAeb', []))
# Handle calibration
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationIncomplete',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if CS.vEgo > 92 * CV.MPH_TO_MS:
events.append(
create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
controls_allowed = sm['health'].controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 200:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
return CS, events, cal_perc, mismatch_counter, can_error_counter
def step(self,
v_lead=0.0,
cruise_buttons=None,
grade=0.0,
publish_model=True):
gen_signals, gen_checks = get_can_signals(CP)
sgs = [s[0] for s in gen_signals]
msgs = [s[1] for s in gen_signals]
cks_msgs = set(check[0] for check in gen_checks)
cks_msgs.add(0x18F)
cks_msgs.add(0x30C)
# ******** get messages sent to the car ********
can_strings = messaging.drain_sock_raw(Plant.sendcan,
wait_for_one=self.response_seen)
# After the first response the car is done fingerprinting, so we can run in lockstep with controlsd
if can_strings:
self.response_seen = True
self.cp.update_strings(can_strings, sendcan=True)
# ******** get controlsState messages for plotting ***
controls_state_msgs = []
for a in messaging.drain_sock(Plant.controls_state,
wait_for_one=self.response_seen):
controls_state_msgs.append(a.controlsState)
fcw = None
for a in messaging.drain_sock(Plant.plan):
if a.longitudinalPlan.fcw:
fcw = True
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE'] * 0.003906248
else:
brake = 0.0
if self.cp.vl[0x200]['GAS_COMMAND'] > 0:
gas = self.cp.vl[0x200]['GAS_COMMAND'] / 256.0
else:
gas = 0.0
if self.cp.vl[0xe4]['STEER_TORQUE_REQUEST']:
steer_torque = self.cp.vl[0xe4]['STEER_TORQUE'] * 1.0 / 0xf00
else:
steer_torque = 0.0
distance_lead = self.distance_lead_prev + v_lead * self.ts
# ******** run the car ********
speed, acceleration = car_plant(self.distance_prev, self.speed_prev,
grade, gas, brake)
distance = self.distance_prev + speed * self.ts
speed = self.speed_prev + self.ts * acceleration
if speed <= 0:
speed = 0
acceleration = 0
# ******** lateral ********
self.angle_steer -= (steer_torque / 10.0) * self.ts
# *** radar model ***
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
else:
d_rel = 200.
v_rel = 0.
lateral_pos_rel = 0.
# print at 5hz
if (self.frame % (self.rate // 5)) == 0:
print(
"%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s"
% (distance, speed, acceleration, self.angle_steer, gas, brake,
steer_torque, d_rel, v_rel))
# ******** publish the car ********
vls_tuple = namedtuple('vls', [
'XMISSION_SPEED',
'WHEEL_SPEED_FL',
'WHEEL_SPEED_FR',
'WHEEL_SPEED_RL',
'WHEEL_SPEED_RR',
'STEER_ANGLE',
'STEER_ANGLE_RATE',
'STEER_TORQUE_SENSOR',
'STEER_TORQUE_MOTOR',
'LEFT_BLINKER',
'RIGHT_BLINKER',
'GEAR',
'WHEELS_MOVING',
'BRAKE_ERROR_1',
'BRAKE_ERROR_2',
'SEATBELT_DRIVER_LAMP',
'SEATBELT_DRIVER_LATCHED',
'BRAKE_PRESSED',
'BRAKE_SWITCH',
'CRUISE_BUTTONS',
'ESP_DISABLED',
'HUD_LEAD',
'USER_BRAKE',
'STEER_STATUS',
'GEAR_SHIFTER',
'PEDAL_GAS',
'CRUISE_SETTING',
'ACC_STATUS',
'CRUISE_SPEED_PCM',
'CRUISE_SPEED_OFFSET',
'DOOR_OPEN_FL',
'DOOR_OPEN_FR',
'DOOR_OPEN_RL',
'DOOR_OPEN_RR',
'CAR_GAS',
'MAIN_ON',
'EPB_STATE',
'BRAKE_HOLD_ACTIVE',
'INTERCEPTOR_GAS',
'INTERCEPTOR_GAS2',
'IMPERIAL_UNIT',
'MOTOR_TORQUE',
])
vls = vls_tuple(
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.angle_steer,
self.angle_steer_rate,
0,
0, # Steer torque sensor
0,
0, # Blinkers
self.gear_choice,
speed != 0,
self.brake_error,
self.brake_error,
not self.seatbelt,
self.seatbelt, # Seatbelt
self.brake_pressed,
0., # Brake pressed, Brake switch
cruise_buttons,
self.esp_disabled,
0, # HUD lead
self.user_brake,
self.steer_error,
self.gear_shifter,
self.pedal_gas,
self.cruise_setting,
self.acc_status,
self.v_cruise,
0, # Cruise speed offset
0,
0,
0,
0, # Doors
self.user_gas,
self.main_on,
0, # EPB State
0, # Brake hold
0, # Interceptor feedback
0, # Interceptor 2 feedback
False,
0,
)
# TODO: publish each message at proper frequency
can_msgs = []
for msg in set(msgs):
msg_struct = {}
indxs = [i for i, x in enumerate(msgs) if msg == msgs[i]]
for i in indxs:
msg_struct[sgs[i]] = getattr(vls, sgs[i])
if "COUNTER" in honda.get_signals(msg):
msg_struct["COUNTER"] = self.frame % 4
if "COUNTER_PEDAL" in honda.get_signals(msg):
msg_struct["COUNTER_PEDAL"] = self.frame % 0xf
msg = honda.lookup_msg_id(msg)
msg_data = honda.encode(msg, msg_struct)
if "CHECKSUM" in honda.get_signals(msg):
msg_data = fix(msg_data, msg)
if "CHECKSUM_PEDAL" in honda.get_signals(msg):
msg_struct["CHECKSUM_PEDAL"] = crc8_pedal(msg_data[:-1])
msg_data = honda.encode(msg, msg_struct)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
if self.frame % 5 == 0:
radar_state_msg = b'\x79\x00\x00\x00\x00\x00\x00\x00'
radar_msg = to_3_byte(d_rel * 16.0) + \
to_3_byte(int(lateral_pos_rel * 16.0) & 0x3ff) + \
to_3s_byte(int(v_rel * 32.0)) + \
b"0f00000"
radar_msg = binascii.unhexlify(radar_msg)
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg, 1])
# add camera msg so controlsd thinks it's alive
msg_struct["COUNTER"] = self.frame % 4
msg = honda.lookup_msg_id(0xe4)
msg_data = honda.encode(msg, msg_struct)
msg_data = fix(msg_data, 0xe4)
can_msgs.append([0xe4, 0, msg_data, 2])
# Fake sockets that controlsd subscribes to
live_parameters = messaging.new_message('liveParameters')
live_parameters.liveParameters.valid = True
live_parameters.liveParameters.sensorValid = True
live_parameters.liveParameters.posenetValid = True
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
Plant.live_params.send(live_parameters.to_bytes())
dmon_state = messaging.new_message('driverMonitoringState')
dmon_state.driverMonitoringState.isDistracted = False
Plant.driverMonitoringState.send(dmon_state.to_bytes())
pandaState = messaging.new_message('pandaState')
pandaState.pandaState.safetyModel = car.CarParams.SafetyModel.hondaNidec
pandaState.pandaState.controlsAllowed = True
Plant.pandaState.send(pandaState.to_bytes())
deviceState = messaging.new_message('deviceState')
deviceState.deviceState.freeSpacePercent = 1.
deviceState.deviceState.batteryPercent = 100
Plant.deviceState.send(deviceState.to_bytes())
live_location_kalman = messaging.new_message('liveLocationKalman')
live_location_kalman.liveLocationKalman.inputsOK = True
live_location_kalman.liveLocationKalman.gpsOK = True
Plant.live_location_kalman.send(live_location_kalman.to_bytes())
# ******** publish a fake model going straight and fake calibration ********
# note that this is worst case for MPC, since model will delay long mpc by one time step
if publish_model and self.frame % 5 == 0:
md = messaging.new_message('modelV2')
cal = messaging.new_message('liveCalibration')
md.modelV2.frameId = 0
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
v_rel = v_lead - speed
prob = 1.0
else:
d_rel = 200.
v_rel = 0.
prob = 0.0
lead = log.ModelDataV2.LeadDataV2.new_message()
lead.xyva = [float(d_rel), 0.0, float(v_rel), 0.0]
lead.xyvaStd = [1.0, 1.0, 1.0, 1.0]
lead.prob = prob
md.modelV2.leads = [lead, lead]
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
cal.liveCalibration.rpyCalib = [0.] * 3
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
for s in Plant.pm.sock.keys():
try:
Plant.pm.send(s, messaging.new_message(s))
except Exception:
Plant.pm.send(s, messaging.new_message(s, 1))
Plant.logcan.send(can_list_to_can_capnp(can_msgs))
# ******** update prevs ********
self.frame += 1
if self.response_seen:
self.rk.monitor_time()
self.speed = speed
self.distance = distance
self.distance_lead = distance_lead
self.speed_prev = speed
self.distance_prev = distance
self.distance_lead_prev = distance_lead
else:
# Don't advance time when controlsd is not yet ready
self.rk.keep_time()
self.rk._frame = 0
return {
"distance": distance,
"speed": speed,
"acceleration": acceleration,
"distance_lead": distance_lead,
"brake": brake,
"gas": gas,
"steer_torque": steer_torque,
"fcw": fcw,
"controls_state_msgs": controls_state_msgs,
}
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(Priority.CTRL_LOW)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
# *** publish radarState and liveTracks
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, RI.delay)
# TODO: always log leads once we can hide them conditionally
enable_lead = CP.openpilotLongitudinalControl or not CP.radarOffCan
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
# This looks like a useless tesla hack. See if it can be removed?
if CP.carName == "tesla":
rr, rrext, ahbCarDetected = RI.update(can_strings, v_ego=0)
else:
rr = RI.update(can_strings)
if rr is None:
continue
sm.update(0)
dat = RD.update(rk.frame, sm, rr, enable_lead)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
def radard_thread(sm=None, pm=None, can_sock=None):
set_realtime_priority(2)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")
CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
use_tesla_radar = CarSettings().get_value("useTeslaRadar")
mocked = (CP.carName == "mock") or ((CP.carName == "tesla")
and not use_tesla_radar)
cloudlog.info("radard got CarParams")
# import the radar from the fingerprint
cloudlog.info("radard is importing %s", CP.carName)
RadarInterface = importlib.import_module(
'selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
if can_sock is None:
can_sock = messaging.sub_sock('can')
if sm is None:
sm = messaging.SubMaster(
['model', 'controlsState', 'liveParameters', 'pathPlan'])
# *** publish radarState and liveTracks
if pm is None:
pm = messaging.PubMaster(['radarState', 'liveTracks'])
icLeads = messaging.pub_sock('uiIcLeads')
ahbInfo = messaging.pub_sock('ahbInfo')
RI = RadarInterface(CP)
rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None)
RD = RadarD(CP.radarTimeStep, mocked, RI, use_tesla_radar, RI.delay)
has_radar = not CP.radarOffCan or mocked
last_md_ts = 0.
v_ego = 0.
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
sm.update(0)
if sm.updated['controlsState']:
v_ego = sm['controlsState'].vEgo
rr, rrext, ahbCarDetected = RI.update(can_strings, v_ego)
if rr is None:
continue
dat, datext = RD.update(rk.frame, sm, rr, has_radar, rrext)
dat.radarState.cumLagMs = -rk.remaining * 1000.
pm.send('radarState', dat)
icLeads.send(datext.to_bytes())
ahbInfoMsg = tesla.AHBinfo.new_message()
ahbInfoMsg.source = 0
ahbInfoMsg.radarCarDetected = ahbCarDetected
ahbInfoMsg.cameraCarDetected = False
ahbInfo.send(ahbInfoMsg.to_bytes())
# *** publish tracks for UI debugging (keep last) ***
tracks = RD.tracks
dat = messaging.new_message('liveTracks', len(tracks))
for cnt, ids in enumerate(sorted(tracks.keys())):
dat.liveTracks[cnt] = {
"trackId": ids,
"dRel": float(tracks[ids].dRel),
"yRel": float(tracks[ids].yRel),
"vRel": float(tracks[ids].vRel),
}
pm.send('liveTracks', dat)
rk.monitor_time()
MINY = -1.0
MAXY = 1.0
def get_rrext_by_trackId(rrext, trackId):
if rrext is not None:
for p in rrext:
if p.trackId == trackId:
return p
return None
if __name__ == "__main__":
CP = None
RI = RadarInterface(CP)
can_sock = messaging.sub_sock('can')
while 1:
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
rr, rrext, ahb = RI.update(can_strings, 0.)
if (rr is None) or (rrext is None):
continue
print(chr(27) + "[2J")
for pt in rr.points:
if (pt.dRel >= MINX) and (pt.dRel <= MAXX) and (
pt.yRel >= MINY) and (pt.yRel <= MAXY):
extpt = get_rrext_by_trackId(rrext, pt.trackId)
print(pt, extpt)
def can_sync_thread():
can_sock = messaging.sub_sock('can', conflate=True, timeout=100)
can_pub = None
os.environ["ZMQ"] = "1"
pub_context = messaging_pyx.Context()
can_pub = messaging_pyx.PubSocket()
can_pub.connect(pub_context, 'testJoystick')
del os.environ["ZMQ"]
rk = Ratekeeper(100.0, print_delay_threshold=None)
cc_main = 0
cc_status = 0
speed = 0.0
dbc_file = "toyota_yaris"
signals = [
# sig_name, sig_address, default
("GEAR", "GEAR_PACKET", 0),
("BRAKE_PRESSED", "BRAKE_MODULE2", 0),
("GAS_PEDAL", "GAS_PEDAL", 0),
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 0),
("DOOR_OPEN_FL", "SEATS_DOORS", 0),
("DOOR_OPEN_FR", "SEATS_DOORS", 0),
("DOOR_OPEN_RL", "SEATS_DOORS", 0),
("DOOR_OPEN_RR", "SEATS_DOORS", 0),
("MAIN_ON", "PCM_CRUISE_SM", 0),
("CRUISE_CONTROL_STATE", "PCM_CRUISE_SM", 0),
("TURN_SIGNALS", "STEERING_LEVERS", 0), # 3 is no blinkers
("ENGINE_RPM", "POWERTRAIN", 0),
("SPEED", "SPEED", 0),
("MAY_CONTAIN_LIGHTS", "BOOLS", 0),
("CHANGES_EACH_RIDE", "SLOW_VARIABLE_INFOS", 0),
("INCREASING_VALUE_FUEL", "SLOW_VARIABLE_INFOS", 0)
]
checks = []
parser = CANParser(dbc_file, signals, checks, 0)
play_time = 0
while True:
try:
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
parser.update_strings(can_strs)
# print (parser.vl)
cc_main = parser.vl['PCM_CRUISE_SM']['MAIN_ON']
cc_status = parser.vl['PCM_CRUISE_SM']['CRUISE_CONTROL_STATE']
speed = parser.vl['SPEED']['SPEED']
doorOpen = any([
parser.vl["SEATS_DOORS"]['DOOR_OPEN_FL'],
parser.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
parser.vl["SEATS_DOORS"]['DOOR_OPEN_RL'],
parser.vl["SEATS_DOORS"]['DOOR_OPEN_RR']
])
seatbeltUnlatched = parser.vl["SEATS_DOORS"][
'SEATBELT_DRIVER_UNLATCHED'] != 0
light = parser.vl["BOOLS"]["MAY_CONTAIN_LIGHTS"]
a = parser.vl["SLOW_VARIABLE_INFOS"]["CHANGES_EACH_RIDE"]
b = parser.vl["SLOW_VARIABLE_INFOS"]["INCREASING_VALUE_FUEL"]
if doorOpen:
play_time = play_sound(SOUND_PATH + 'door_open.wav', play_time,
3)
if seatbeltUnlatched:
play_time = play_sound(SOUND_PATH + 'seatbelt.wav', play_time,
3)
if light != 0:
play_time = play_sound(SOUND_PATH + 'turn_signal.wav',
play_time, 3)
cc_main_v = 0
if cc_main:
cc_main_v = 1
can_dict = {
'cc_main': cc_main_v,
'cc_status': cc_status,
'speed': speed,
'light': light,
'a': a,
'b': b
}
json_str = json.dumps(can_dict)
json_str = json_str.replace('\'', '"')
can_pub.send(json_str)
except messaging_pyx.MessagingError:
print('MessagingError error happens')
rk.keep_time()
def fingerprint(logcan, sendcan):
fixed_fingerprint = os.environ.get('FINGERPRINT', "")
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
if not fixed_fingerprint and not skip_fw_query:
# Vin query only reliably works thorugh OBDII
bus = 1
cached_params = Params().get("CarParamsCache")
if cached_params is not None:
cached_params = car.CarParams.from_bytes(cached_params)
if cached_params.carName == "mock":
cached_params = None
if cached_params is not None and len(
cached_params.carFw
) > 0 and cached_params.carVin is not VIN_UNKNOWN:
cloudlog.warning("Using cached CarParams")
vin = cached_params.carVin
car_fw = list(cached_params.carFw)
else:
cloudlog.warning("Getting VIN & FW versions")
_, vin = get_vin(logcan, sendcan, bus)
car_fw = get_fw_versions(logcan, sendcan)
exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
else:
vin = VIN_UNKNOWN
exact_fw_match, fw_candidates, car_fw = True, set(), []
if len(vin) != 17:
cloudlog.event("Malformed VIN", vin=vin, error=True)
vin = VIN_UNKNOWN
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
finger = gen_empty_fingerprint()
candidate_cars = {i: all_legacy_fingerprint_cars()
for i in [0, 1]
} # attempt fingerprint on both bus 0 and 1
frame = 0
frame_fingerprint = 25 # 0.25s
car_fingerprint = None
done = False
# drain CAN socket so we always get the latest messages
messaging.drain_sock_raw(logcan)
while not done:
a = get_one_can(logcan)
for can in a.can:
# The fingerprint dict is generated for all buses, this way the car interface
# can use it to detect a (valid) multipanda setup and initialize accordingly
if can.src < 128:
if can.src not in finger:
finger[can.src] = {}
finger[can.src][can.address] = len(can.dat)
for b in candidate_cars:
# Ignore extended messages and VIN query response.
if can.src == b and can.address < 0x800 and can.address not in (
0x7df, 0x7e0, 0x7e8):
candidate_cars[b] = eliminate_incompatible_cars(
can, candidate_cars[b])
# if we only have one car choice and the time since we got our first
# message has elapsed, exit
for b in candidate_cars:
if len(candidate_cars[b]) == 1 and frame > frame_fingerprint:
# fingerprint done
car_fingerprint = candidate_cars[b][0]
# bail if no cars left or we've been waiting for more than 2s
failed = (all(len(cc) == 0 for cc in candidate_cars.values())
and frame > frame_fingerprint) or frame > 200
succeeded = car_fingerprint is not None
done = failed or succeeded
frame += 1
exact_match = True
source = car.CarParams.FingerprintSource.can
# If FW query returns exactly 1 candidate, use it
if len(fw_candidates) == 1:
car_fingerprint = list(fw_candidates)[0]
source = car.CarParams.FingerprintSource.fw
exact_match = exact_fw_match
if fixed_fingerprint:
car_fingerprint = fixed_fingerprint
source = car.CarParams.FingerprintSource.fixed
cloudlog.event("fingerprinted",
car_fingerprint=car_fingerprint,
source=source,
fuzzy=not exact_match,
fw_count=len(car_fw))
return car_fingerprint, finger, vin, car_fw, source, exact_match