def test_panda_safety_carstate(self):
"""
Assert that panda safety matches openpilot's carState
"""
if self.CP.dashcamOnly:
self.skipTest("no need to check panda safety for dashcamOnly")
CC = car.CarControl.new_message()
# warm up pass, as initial states may be different
for can in self.can_msgs[:300]:
for msg in can_capnp_to_can_list(can.can, src_filter=range(64)):
to_send = package_can_msg(msg)
self.safety.safety_rx_hook(to_send)
self.CI.update(CC, (can_list_to_can_capnp([msg, ]), ))
checks = defaultdict(lambda: 0)
for can in self.can_msgs:
CS = self.CI.update(CC, (can.as_builder().to_bytes(), ))
for msg in can_capnp_to_can_list(can.can, src_filter=range(64)):
to_send = package_can_msg(msg)
ret = self.safety.safety_rx_hook(to_send)
self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}")
# TODO: check rest of panda's carstate (steering, ACC main on, etc.)
# TODO: make the interceptor thresholds in openpilot and panda match, then remove this exception
gas_pressed = CS.gasPressed
if self.CP.enableGasInterceptor and gas_pressed and not self.safety.get_gas_pressed_prev():
# panda intentionally has a higher threshold
if self.CP.carName == "toyota" and 15 < CS.gas < 15*1.5:
gas_pressed = False
if self.CP.carName == "honda":
gas_pressed = False
checks['gasPressed'] += gas_pressed != self.safety.get_gas_pressed_prev()
# TODO: remove this exception once this mismatch is resolved
brake_pressed = CS.brakePressed
if CS.brakePressed and not self.safety.get_brake_pressed_prev():
if self.CP.carFingerprint in (HONDA.PILOT, HONDA.PASSPORT, HONDA.RIDGELINE) and CS.brake > 0.05:
brake_pressed = False
checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev()
if self.CP.pcmCruise:
checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed()
if self.CP.carName == "honda":
checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on()
# TODO: add flag to toyota safety
if self.CP.carFingerprint == TOYOTA.SIENNA and checks['brakePressed'] < 25:
checks['brakePressed'] = 0
# Honda Nidec uses button enable in panda, but pcm enable in openpilot
if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH and checks['controlsAllowed'] < 25:
checks['controlsAllowed'] = 0
failed_checks = {k: v for k, v in checks.items() if v > 0}
self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}")
def boardd_proxy_loop(rate=100, address="192.168.2.251"):
rk = Ratekeeper(rate)
can_init()
# *** subscribes can
logcan = messaging.sub_sock('can', addr=address)
# *** publishes to can send
sendcan = messaging.pub_sock('sendcan')
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
can_send_many(cl)
rk.keep_time()
def boardd_mock_loop():
can_init()
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
logcan = messaging.sub_sock('can')
sendcan = messaging.pub_sock('sendcan')
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
snd = []
for s in snds:
snd += s
snd = list(filter(lambda x: x[-1] <= 2, snd))
snd_0 = len(list(filter(lambda x: x[-1] == 0, snd)))
snd_1 = len(list(filter(lambda x: x[-1] == 1, snd)))
snd_2 = len(list(filter(lambda x: x[-1] == 2, snd)))
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
got_0 = len(list(filter(lambda x: x[-1] == 0 + 0x80, can_msgs)))
got_1 = len(list(filter(lambda x: x[-1] == 1 + 0x80, can_msgs)))
got_2 = len(list(filter(lambda x: x[-1] == 2 + 0x80, can_msgs)))
print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" %
(len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1,
got_2))
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan')
sendcan.send(m.to_bytes())
def send_thread(sender_serial):
while True:
try:
if jungle:
sender = PandaJungle(sender_serial)
else:
sender = Panda(sender_serial)
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
sender.set_can_loopback(False)
can_sock = messaging.sub_sock('can')
while True:
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = list(filter(lambda x: x[-1] <= 2, snd))
try:
sender.can_send_many(snd)
except usb1.USBErrorTimeout:
pass
# Drain panda message buffer
sender.can_recv()
except Exception:
traceback.print_exc()
time.sleep(1)
def update(self):
canMonoTimes = []
if NEW_CAN:
#print "================================ new_can" # didn't enter during simulation -- Hasnat
updated_messages = set()
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
if 0x445 in updated_messages:
break
else:
can_pub_radar = []
# TODO: can hang if no packets show up
while 1:
for a in messaging.drain_sock(self.logcan, wait_for_one=True):
canMonoTimes.append(a.logMonoTime)
can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3])) # receiving radar message from CAN (sent from Plant.py) -- Hasnat
# only run on the 0x445 packets, used for timing
if any(x[0] == 0x445 for x in can_pub_radar):
break
updated_messages = self.rcp.update_can(can_pub_radar)
ret = car.RadarState.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("notValid")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
for ii in updated_messages:
cpt = self.rcp.vl[ii]
#print cpt
if cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
else:
if ii in self.pts:
del self.pts[ii]
ret.points = self.pts.values()
return ret
def send_thread(serial):
panda = Panda(serial)
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
panda.set_can_loopback(False)
can_sock = messaging.sub_sock(service_list['can'].port)
while True:
# Send messages one bus 0 and 1
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = filter(lambda x: x[-1] <= 2, snd)
panda.can_send_many(snd)
# Drain panda message buffer
panda.can_recv()
def boardd_loop(rate=200):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** publishes can and health
logcan = messaging.pub_sock(context, service_list['can'].port)
health_sock = messaging.pub_sock(context, service_list['health'].port)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 1hz
if (rk.frame % rate) == 0:
health = can_health()
msg = messaging.new_message()
msg.init('health')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.started = health['started']
msg.health.controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs).to_bytes()
logcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
def boardd_loop(rate=100):
rk = Ratekeeper(rate)
can_init()
# *** publishes can and health
logcan = messaging.pub_sock('can')
health_sock = messaging.pub_sock('pandaStates')
# *** subscribes to can send
sendcan = messaging.sub_sock('sendcan')
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 2hz
if (rk.frame % (rate // 2)) == 0:
health = can_health()
msg = messaging.new_message('pandaStates', 1)
# store the health to be logged
msg.pandaState[0].voltage = health['voltage']
msg.pandaState[0].current = health['current']
msg.pandaState[0].ignitionLine = health['ignition_line']
msg.pandaState[0].ignitionCan = health['ignition_can']
msg.pandaState[0].controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs).to_bytes()
logcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
def send_thread(serial):
try:
panda = Panda(serial)
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
panda.set_can_loopback(False)
can_sock = messaging.sub_sock('can')
while True:
# Send messages one bus 0 and 1
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = list(filter(lambda x: x[-1] <= 2, snd))
panda.can_send_many(snd)
# Drain panda message buffer
panda.can_recv()
except Exception:
traceback.print_exc()
def update(self):
canMonoTimes = []
can_pub_radar = []
# TODO: can hang if no packets show up
while 1:
for a in messaging.drain_sock(self.logcan, wait_for_one=True):
canMonoTimes.append(a.logMonoTime)
can_pub_radar.extend(can_capnp_to_can_list(a.can, [1, 3]))
# only run on the 0x445 packets, used for timing
if any(x[0] == 0x445 for x in can_pub_radar):
break
self.rcp.update_can(can_pub_radar)
ret = car.RadarState.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("notValid")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
for ii in self.rcp.msgs_upd:
cpt = self.rcp.vl[ii]
if cpt['LONG_DIST'] < 255:
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = car.RadarState.RadarPoint.new_message()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt[
'LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = float('nan')
else:
if ii in self.pts:
del self.pts[ii]
ret.points = self.pts.values()
return ret
def fingerprint(logcan):
print "waiting for fingerprint..."
brake_only = True
finger = {}
st = None
while 1:
possible_cars = []
for a in messaging.drain_sock(logcan, wait_for_one=True):
if st is None:
st = sec_since_boot()
for adr, _, msg, idx in can_capnp_to_can_list(a.can):
# pedal
if adr == 0x201 and idx == 0:
brake_only = False
if idx == 0:
finger[adr] = len(msg)
# check for a single match
for f in fingerprints:
is_possible = True
for adr in finger:
# confirm all messages we have seen match
if adr not in fingerprints[
f] or fingerprints[f][adr] != finger[adr]:
#print "mismatch", f, adr
is_possible = False
break
if is_possible:
possible_cars.append(f)
# if we only have one car choice and it's been 100ms since we got our first message, exit
if len(possible_cars) == 1 and st is not None and (sec_since_boot() -
st) > 0.1:
break
elif len(possible_cars) == 0:
print finger
raise Exception("car doesn't match any fingerprints")
print "fingerprinted", possible_cars[0]
return brake_only, possible_cars[0]
def boardd_mock_loop():
context = zmq.Context()
can_init()
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
snd = []
for s in snds:
snd += s
snd = filter(lambda x: x[-1] <= 1, snd)
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
print("sent %d got %d" % (len(snd), len(can_msgs)))
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan')
sendcan.send(m.to_bytes())
def step(self,
v_lead=0.0,
cruise_buttons=None,
grade=0.0,
publish_model=True):
# dbc_f, sgs, ivs, msgs, cks_msgs, frqs = initialize_can_struct(self.civic, self.brake_only)
cp2 = get_can_parser(fake_car_params())
sgs = cp2._sgs
msgs = cp2._msgs
cks_msgs = cp2.msgs_ck
# ******** get messages sent to the car ********
can_msgs = []
for a in messaging.drain_sock(Plant.sendcan):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2]))
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live_msgs = []
for a in messaging.drain_sock(Plant.live100):
live_msgs.append(a.live100)
if self.cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']:
brake = self.cp.vl[0x1fa]['COMPUTER_BRAKE']
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)
standstill = (speed == 0)
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.
a_rel = 0
lateral_pos_rel = 0.
# print at 5hz
if (self.rk.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 = [
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.speed_sensor(speed),
self.angle_steer,
0,
self.gear_choice,
speed != 0,
0,
0,
0,
0,
self.v_cruise,
not self.seatbelt,
self.seatbelt,
self.brake_pressed,
self.user_gas,
cruise_buttons,
self.esp_disabled,
0,
self.user_brake,
self.steer_error,
self.speed_sensor(speed),
self.brake_error,
self.brake_error,
self.gear_shifter,
self.main_on,
self.acc_status,
self.pedal_gas,
self.cruise_setting,
# left_blinker, right_blinker, counter
0,
0,
0,
# interceptor_gas
0,
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]] = vls[i]
if msg in cks_msgs:
msg_struct["COUNTER"] = self.rk.frame % 4
msg_data = acura.encode(msg, msg_struct)
if msg in cks_msgs:
msg_data = fix(msg_data, msg)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
def to_3_byte(x):
return struct.pack("!H", int(x)).encode("hex")[1:]
def to_3s_byte(x):
return struct.pack("!h", int(x)).encode("hex")[1:]
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)) + \
"0f00000"
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
# ******** publish a fake model going straight ********
if publish_model:
md = messaging.new_message()
md.init('model')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0] * 50
x.prob = 1.0
x.std = 1.0
# fake values?
Plant.model.send(md.to_bytes())
# ******** update prevs ********
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
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas,
steer_torque, live_msgs)
def step(self,
v_lead=0.0,
cruise_buttons=None,
grade=0.0,
publish_model=True):
gen_dbc, 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_msgs = []
for a in messaging.drain_sock(Plant.sendcan):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2]))
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live100_msgs = []
for a in messaging.drain_sock(Plant.live100):
live100_msgs.append(a.live100)
fcw = None
for a in messaging.drain_sock(Plant.plan):
if a.plan.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.rk.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 = [
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,
0,
0,
0, # Doors
0,
0, # Blinkers
0, # Cruise speed offset
self.gear_choice,
speed != 0,
self.brake_error,
self.brake_error,
self.v_cruise,
not self.seatbelt,
self.seatbelt, # Seatbelt
self.brake_pressed,
0.,
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.user_gas,
self.main_on,
0, # EPB State
0, # Brake hold
0, # Interceptor feedback
# 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]] = vls[i]
if "COUNTER" in honda.get_signals(msg):
msg_struct["COUNTER"] = self.rk.frame % 4
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)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
if self.rk.frame % 5 == 0:
radar_state_msg = '\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)) + \
"0f00000"
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).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.rk.frame % 5 == 0:
md = messaging.new_message()
cal = messaging.new_message()
md.init('model')
cal.init('liveCalibration')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0] * 50
x.prob = 1.0
x.std = 1.0
md.model.lead.dist = float(d_rel)
md.model.lead.prob = 1.
md.model.lead.std = 0.1
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
# ******** update prevs ********
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
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas,
steer_torque, fcw, live100_msgs)
def update(self):
# ******************* do can recv *******************
can_pub_main = []
canMonoTimes = []
for a in messaging.drain_sock(self.logcan):
canMonoTimes.append(a.logMonoTime)
can_pub_main.extend(can_capnp_to_can_list(a.can, [0, 2]))
if self.CS.accord:
self.accord_msg.extend(can_capnp_to_can_list(a.can, [9]))
self.accord_msg = self.accord_msg[-1:]
self.CS.update(can_pub_main)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.wheelSpeeds.fl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
ret.wheelSpeeds.fr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
ret.wheelSpeeds.rl = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
ret.wheelSpeeds.rr = self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
# gas pedal
ret.gas = self.CS.car_gas / 256.0
if not self.CP.enableGas:
ret.gasPressed = self.CS.pedal_gas > 0
else:
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
# steering wheel
# TODO: units
ret.steeringAngle = self.CS.angle_steers
if self.CS.accord:
# TODO: move this into the CAN parser
ret.steeringTorque = 0
if len(self.accord_msg) > 0:
aa = map(lambda x: ord(x) & 0x7f, self.accord_msg[0][2])
if len(aa) != 5 or (
-(aa[0] + aa[1] + aa[2] + aa[3])) & 0x7f != aa[4]:
print "ACCORD MSG BAD LEN OR CHECKSUM!"
# TODO: throw an error here?
else:
st = ((aa[0] & 0xF) << 5) + (aa[1] & 0x1F)
if st >= 256:
st = -(512 - st)
ret.steeringTorque = st
ret.steeringPressed = abs(ret.steeringTorque) > 20
else:
ret.steeringTorque = self.CS.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = 'accelCruise'
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
if self.CS.cruise_setting != 0:
be.pressed = True
but = self.CS.cruise_setting
else:
be.pressed = False
but = self.CS.prev_cruise_setting
if but == 1:
be.type = 'altButton1'
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# errors
# TODO: I don't like the way capnp does enums
# These strings aren't checked at compile time
errors = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
errors.append('commIssue')
else:
self.can_invalid_count = 0
if self.CS.steer_error:
errors.append('steerUnavailable')
elif self.CS.steer_not_allowed:
errors.append('steerTemporarilyUnavailable')
if self.CS.brake_error:
errors.append('brakeUnavailable')
if not self.CS.gear_shifter_valid:
errors.append('wrongGear')
if not self.CS.door_all_closed:
errors.append('doorOpen')
if not self.CS.seatbelt:
errors.append('seatbeltNotLatched')
if self.CS.esp_disabled:
errors.append('espDisabled')
if not self.CS.main_on:
errors.append('wrongCarMode')
if self.CS.gear_shifter == 2:
errors.append('reverseGear')
ret.errors = errors
ret.canMonoTimes = canMonoTimes
# cast to reader so it can't be modified
#print ret
return ret.as_reader()
def test_panda_safety_carstate(self):
"""
Assert that panda safety matches openpilot's carState
"""
if self.CP.dashcamOnly:
self.skipTest("no need to check panda safety for dashcamOnly")
CC = car.CarControl.new_message()
# warm up pass, as initial states may be different
for can in self.can_msgs[:300]:
for msg in can_capnp_to_can_list(can.can, src_filter=range(64)):
to_send = package_can_msg(msg)
self.safety.safety_rx_hook(to_send)
self.CI.update(CC, (can_list_to_can_capnp([
msg,
]), ))
if not self.CP.pcmCruise:
self.safety.set_controls_allowed(0)
controls_allowed_prev = False
CS_prev = car.CarState.new_message()
checks = defaultdict(lambda: 0)
for can in self.can_msgs:
CS = self.CI.update(CC, (can.as_builder().to_bytes(), ))
for msg in can_capnp_to_can_list(can.can, src_filter=range(64)):
to_send = package_can_msg(msg)
ret = self.safety.safety_rx_hook(to_send)
self.assertEqual(1, ret,
f"safety rx failed ({ret=}): {to_send}")
# TODO: check rest of panda's carstate (steering, ACC main on, etc.)
checks[
'gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev(
)
# TODO: remove this exception once this mismatch is resolved
brake_pressed = CS.brakePressed
if CS.brakePressed and not self.safety.get_brake_pressed_prev():
if self.CP.carFingerprint in (
HONDA.PILOT, HONDA.PASSPORT,
HONDA.RIDGELINE) and CS.brake > 0.05:
brake_pressed = False
checks[
'brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev(
)
if self.CP.pcmCruise:
# On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state.
# On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but
# openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages).
if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH:
# only the rising edges are expected to match
if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
checks[
'controlsAllowed'] += not self.safety.get_controls_allowed(
)
else:
checks[
'controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed(
)
else:
# Check for enable events on rising edge of controls allowed
button_enable = any(evt.enable for evt in CS.events)
mismatch = button_enable != (
self.safety.get_controls_allowed()
and not controls_allowed_prev)
checks['controlsAllowed'] += mismatch
controls_allowed_prev = self.safety.get_controls_allowed()
if button_enable and not mismatch:
self.safety.set_controls_allowed(False)
if self.CP.carName == "honda":
checks[
'mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on(
)
# TODO: fix standstill mismatches for other makes
checks[
'standstill'] += CS.standstill == self.safety.get_vehicle_moving(
)
CS_prev = CS
# TODO: add flag to toyota safety
if self.CP.carFingerprint == TOYOTA.SIENNA and checks[
'brakePressed'] < 25:
checks['brakePressed'] = 0
failed_checks = {k: v for k, v in checks.items() if v > 0}
self.assertFalse(
len(failed_checks),
f"panda safety doesn't agree with openpilot: {failed_checks}")
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_msgs = []
for a in messaging.drain_sock(Plant.sendcan):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2]))
self.cp.update_can(can_msgs)
# ******** get live100 messages for plotting ***
live100_msgs = []
for a in messaging.drain_sock(Plant.live100):
live100_msgs.append(a.live100)
fcw = None
for a in messaging.drain_sock(Plant.plan):
if a.plan.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.rk.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',
'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',
])
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, #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
)
# 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.rk.frame % 4
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)
can_msgs.append([msg, 0, msg_data, 0])
# add the radar message
# TODO: use the DBC
if self.rk.frame % 5 == 0:
radar_state_msg = '\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)) + \
"0f00000"
can_msgs.append([0x400, 0, radar_state_msg, 1])
can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1])
# add camera msg so controlsd thinks it's alive
msg_struct["COUNTER"] = self.rk.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])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).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.rk.frame % 5 == 0:
md = messaging.new_message()
cal = messaging.new_message()
md.init('model')
cal.init('liveCalibration')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0] * 50
x.prob = 1.0
x.std = 1.0
md.model.lead.dist = float(d_rel)
md.model.lead.prob = 1.
md.model.lead.std = 0.1
cal.liveCalibration.calStatus = 1
cal.liveCalibration.calPerc = 100
# fake values?
Plant.model.send(md.to_bytes())
Plant.cal.send(cal.to_bytes())
# ******** update prevs ********
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
self.rk.keep_time()
return (distance, speed, acceleration, distance_lead, brake, gas,
steer_torque, fcw, live100_msgs)
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_msgs = []
for a in messaging.drain_sock(Plant.sendcan,
wait_for_one=self.response_seen):
can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0, 2]))
# After the first response the car is done fingerprinting, so we can run in lockstep with controlsd
if can_msgs:
self.response_seen = True
self.cp.update_can(can_msgs)
# ******** 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.plan.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',
])
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)
# 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()
live_parameters.init('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())
driver_monitoring = messaging.new_message()
driver_monitoring.init('driverMonitoring')
driver_monitoring.driverMonitoring.faceOrientation = [0.] * 3
driver_monitoring.driverMonitoring.facePosition = [0.] * 2
Plant.driverMonitoring.send(driver_monitoring.to_bytes())
health = messaging.new_message()
health.init('health')
health.health.controlsAllowed = True
Plant.health.send(health.to_bytes())
thermal = messaging.new_message()
thermal.init('thermal')
thermal.thermal.freeSpace = 1.
thermal.thermal.batteryPercent = 100
Plant.thermal.send(thermal.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()
cal = messaging.new_message()
md.init('model')
cal.init('liveCalibration')
md.model.frameId = 0
for x in [md.model.path, md.model.leftLane, md.model.rightLane]:
x.points = [0.0] * 50
x.prob = 1.0
x.std = 1.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
md.model.lead.dist = float(d_rel)
md.model.lead.prob = prob
md.model.lead.relY = 0.0
md.model.lead.relYStd = 1.
md.model.lead.relVel = float(v_rel)
md.model.lead.relVelStd = 1.
md.model.lead.relA = 0.0
md.model.lead.relAStd = 10.
md.model.lead.std = 1.0
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())
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,
}
from panda import Panda
try:
from panda_jungle import PandaJungle # pylint: disable=import-error
except Exception:
PandaJungle = None # type: ignore
ROUTE = "77611a1fac303767/2020-03-24--09-50-38"
NUM_SEGS = 10 # route has 82 segments available
print("Loading log...")
CAN_MSGS = []
for i in tqdm(list(range(1, NUM_SEGS))):
log_url = f"https://commadataci.blob.core.windows.net/openpilotci/{ROUTE}/{i}/rlog.bz2"
lr = LogReader(log_url)
CAN_MSGS += [
can_capnp_to_can_list(m.can) for m in lr if m.which() == 'can'
]
def send_thread(sender, core):
config_realtime_process(core, 55)
if "Jungle" in str(type(sender)):
sender.set_ignition(False)
time.sleep(3)
sender.set_ignition(True)
else:
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
sender.set_can_loopback(False)
log_idx = 0
from tools.lib.logreader import LogReader
from panda import Panda
try:
from panda_jungle import PandaJungle # pylint: disable=import-error
except Exception:
PandaJungle = None # type: ignore
print("Loading log...")
ROUTE = "77611a1fac303767/2020-03-24--09-50-38"
REPLAY_SEGS = list(range(10, 16)) # route has 82 segments available
CAN_MSGS = []
for i in tqdm(REPLAY_SEGS):
log_url = f"https://commadataci.blob.core.windows.net/openpilotci/{ROUTE}/{i}/rlog.bz2"
lr = LogReader(log_url)
CAN_MSGS += [can_capnp_to_can_list(m.can) for m in lr if m.which() == 'can']
# set both to cycle ignition
IGN_ON = int(os.getenv("ON", "0"))
IGN_OFF = int(os.getenv("OFF", "0"))
ENABLE_IGN = IGN_ON > 0 and IGN_OFF > 0
if ENABLE_IGN:
print(f"Cycling ignition: on for {IGN_ON}s, off for {IGN_OFF}s")
def send_thread(s, flock):
if "Jungle" in str(type(s)):
if "FLASH" in os.environ:
with flock:
s.flash()
