def update_can(self, can_recv):
msgs_upd = []
cn_vl_max = 5 # no more than 5 wrong counter checks
self.sec_since_boot_cached = sec_since_boot()
# we are subscribing to PID_XXX, else data from USB
for msg, ts, cdat, _ in can_recv:
idxs = self._message_indices[msg]
if idxs:
msgs_upd.append(msg)
# read the entire message
out = self.can_dbc.decode((msg, 0, cdat))[1]
# checksum check
self.ck[msg] = True
if "CHECKSUM" in out.keys() and msg in self.msgs_ck:
# remove checksum (half byte)
ck_portion = cdat[:-1] + (cdat[-1] & 0xF0).to_bytes(
1, 'little')
# recalculate checksum
msg_vl = fix(ck_portion, msg)
# compare recalculated vs received checksum
if msg_vl != cdat:
print("CHECKSUM FAIL: {0}".format(hex(msg)))
self.ck[msg] = False
self.ok[msg] = False
# counter check
cn = 0
if "COUNTER" in out.keys():
cn = out["COUNTER"]
# check counter validity if it's a relevant message
if cn != (
(self.cn[msg] + 1) %
4) and msg in self.msgs_ck and "COUNTER" in out.keys():
#print("FAILED COUNTER: {0}".format(hex(msg)()
self.cn_vl[msg] += 1 # counter check failed
else:
self.cn_vl[msg] -= 1 # counter check passed
# message status is invalid if we received too many wrong counter values
if self.cn_vl[msg] >= cn_vl_max:
print("COUNTER WRONG: {0}".format(hex(msg)))
self.ok[msg] = False
# update msg time stamps and counter value
self.ct[msg] = self.sec_since_boot_cached
self.cn[msg] = cn
self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
# set msg valid status if checksum is good and wrong counter counter is zero
if self.ck[msg] and self.cn_vl[msg] == 0:
self.ok[msg] = True
# update value of signals in the
for ii in idxs:
sg = self._sgs[ii]
self.vl[msg][sg] = out[sg]
self.ts[msg][sg] = ts
# for each message, check if it's too long since last time we received it
self._check_dead_msgs()
# assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
self.can_valid = True
if False in self.ok.values():
#print("CAN INVALID!")
self.can_valid = False
return msgs_upd
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_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,
}
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 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)
