mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-15 06:12:11 +08:00
let can packer handle counter (#25240)
* let can packer handle counter * diff is expected * update refs * clean that up * bump opendbc * fix sim old-commit-hash: 0ca62bf7df0fe81829c744fd8689c9b97cb6a36b
This commit is contained in:
+1
-1
Submodule opendbc updated: e2465cc701...4195e8f4c9
+1
-1
Submodule panda updated: 1dfee5973b...a1686ca3ca
@@ -1,9 +1,7 @@
|
||||
def create_control(packer, torque_l, torque_r, idx):
|
||||
can_bus = 0
|
||||
|
||||
def create_control(packer, torque_l, torque_r):
|
||||
values = {
|
||||
"TORQUE_L": torque_l,
|
||||
"TORQUE_R": torque_r,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("TORQUE_CMD", can_bus, values, idx)
|
||||
return packer.make_can_msg("TORQUE_CMD", 0, values)
|
||||
|
||||
@@ -80,7 +80,7 @@ class CarController:
|
||||
torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
|
||||
|
||||
can_sends = []
|
||||
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r, self.frame // 2))
|
||||
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r))
|
||||
|
||||
new_actuators = CC.actuators.copy()
|
||||
new_actuators.accel = torque_l
|
||||
|
||||
@@ -70,8 +70,7 @@ class CarController:
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
idx = self.frame // 2
|
||||
can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit, idx))
|
||||
can_sends.append(create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit))
|
||||
|
||||
self.frame += 1
|
||||
if not lkas_control_bit and self.lkas_control_bit_prev:
|
||||
|
||||
@@ -52,19 +52,20 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au
|
||||
return packer.make_can_msg("DAS_6", 0, values)
|
||||
|
||||
|
||||
def create_lkas_command(packer, CP, apply_steer, lkas_control_bit, frame):
|
||||
def create_lkas_command(packer, CP, apply_steer, lkas_control_bit):
|
||||
# LKAS_COMMAND Lane-keeping signal to turn the wheel
|
||||
enabled_val = 2 if CP.carFingerprint in RAM_CARS else 1
|
||||
values = {
|
||||
"STEERING_TORQUE": apply_steer,
|
||||
"LKAS_CONTROL_BIT": enabled_val if lkas_control_bit else 0,
|
||||
}
|
||||
return packer.make_can_msg("LKAS_COMMAND", 0, values, frame % 0x10)
|
||||
return packer.make_can_msg("LKAS_COMMAND", 0, values)
|
||||
|
||||
|
||||
def create_cruise_buttons(packer, frame, bus, cancel=False, resume=False):
|
||||
values = {
|
||||
"ACC_Cancel": cancel,
|
||||
"ACC_Resume": resume,
|
||||
"COUNTER": frame % 0x10,
|
||||
}
|
||||
return packer.make_can_msg("CRUISE_BUTTONS", bus, values, frame % 0x10)
|
||||
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
|
||||
|
||||
@@ -155,9 +155,8 @@ class CarController:
|
||||
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
|
||||
|
||||
# Send steering command.
|
||||
idx = self.frame % 4
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint,
|
||||
idx, CS.CP.openpilotLongitudinalControl))
|
||||
CS.CP.openpilotLongitudinalControl))
|
||||
|
||||
# wind brake from air resistance decel at high speed
|
||||
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
|
||||
@@ -190,18 +189,16 @@ class CarController:
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message
|
||||
idx = self.frame // 2
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint, idx))
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint))
|
||||
# If using stock ACC, spam cancel command to kill gas when OP disengages.
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, self.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint))
|
||||
elif CC.cruiseControl.resume:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, self.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if self.frame % 2 == 0:
|
||||
idx = self.frame // 2
|
||||
ts = self.frame * DT_CTRL
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
@@ -210,7 +207,7 @@ class CarController:
|
||||
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas,
|
||||
idx, stopping, self.CP.carFingerprint))
|
||||
stopping, self.CP.carFingerprint))
|
||||
else:
|
||||
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
|
||||
apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
|
||||
@@ -218,7 +215,7 @@ class CarController:
|
||||
|
||||
pcm_override = True
|
||||
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, fcw_display, idx,
|
||||
pcm_override, pcm_cancel_cmd, fcw_display,
|
||||
self.CP.carFingerprint, CS.stock_brake))
|
||||
self.apply_brake_last = apply_brake
|
||||
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
|
||||
@@ -234,14 +231,13 @@ class CarController:
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
|
||||
else:
|
||||
self.gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if self.frame % 10 == 0:
|
||||
idx = (self.frame // 10) % 4
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
|
||||
hud_control.lanesVisible, fcw_display, acc_alert, steer_required)
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.stock_hud))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
self.speed = pcm_speed
|
||||
|
||||
@@ -20,7 +20,7 @@ def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
|
||||
return 0
|
||||
|
||||
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, stock_brake):
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake):
|
||||
# TODO: do we loose pressure if we keep pump off for long?
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
@@ -42,10 +42,10 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
|
||||
"AEB_STATUS": 0,
|
||||
}
|
||||
bus = get_pt_bus(car_fingerprint)
|
||||
return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
|
||||
return packer.make_can_msg("BRAKE_COMMAND", bus, values)
|
||||
|
||||
|
||||
def create_acc_commands(packer, enabled, active, accel, gas, idx, stopping, car_fingerprint):
|
||||
def create_acc_commands(packer, enabled, active, accel, gas, stopping, car_fingerprint):
|
||||
commands = []
|
||||
bus = get_pt_bus(car_fingerprint)
|
||||
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
|
||||
@@ -67,7 +67,7 @@ def create_acc_commands(packer, enabled, active, accel, gas, idx, stopping, car_
|
||||
"STANDSTILL": standstill,
|
||||
"STANDSTILL_RELEASE": standstill_release,
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values, idx))
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values))
|
||||
|
||||
acc_control_on_values = {
|
||||
"SET_TO_3": 0x03,
|
||||
@@ -76,21 +76,21 @@ def create_acc_commands(packer, enabled, active, accel, gas, idx, stopping, car_
|
||||
"SET_TO_75": 0x75,
|
||||
"SET_TO_30": 0x30,
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values, idx))
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, radar_disabled):
|
||||
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, radar_disabled):
|
||||
values = {
|
||||
"STEER_TORQUE": apply_steer if lkas_active else 0,
|
||||
"STEER_TORQUE_REQUEST": lkas_active,
|
||||
}
|
||||
bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled)
|
||||
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
|
||||
return packer.make_can_msg("STEERING_CONTROL", bus, values)
|
||||
|
||||
|
||||
def create_bosch_supplemental_1(packer, car_fingerprint, idx):
|
||||
def create_bosch_supplemental_1(packer, car_fingerprint):
|
||||
# non-active params
|
||||
values = {
|
||||
"SET_ME_X04": 0x04,
|
||||
@@ -98,10 +98,10 @@ def create_bosch_supplemental_1(packer, car_fingerprint, idx):
|
||||
"SET_ME_X10": 0x10,
|
||||
}
|
||||
bus = get_lkas_cmd_bus(car_fingerprint)
|
||||
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values, idx)
|
||||
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
|
||||
|
||||
|
||||
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, idx, stock_hud):
|
||||
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, stock_hud):
|
||||
commands = []
|
||||
bus_pt = get_pt_bus(CP.carFingerprint)
|
||||
radar_disabled = CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl
|
||||
@@ -129,7 +129,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, idx, stoc
|
||||
acc_hud_values['FCM_OFF_2'] = stock_hud['FCM_OFF_2']
|
||||
acc_hud_values['FCM_PROBLEM'] = stock_hud['FCM_PROBLEM']
|
||||
acc_hud_values['ICONS'] = stock_hud['ICONS']
|
||||
commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx))
|
||||
commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values))
|
||||
|
||||
lkas_hud_values = {
|
||||
'SET_ME_X41': 0x41,
|
||||
@@ -146,29 +146,29 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, idx, stoc
|
||||
lkas_hud_values['SET_ME_X48'] = 0x48
|
||||
|
||||
if CP.flags & HondaFlags.BOSCH_EXT_HUD and not CP.openpilotLongitudinalControl:
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_A', bus_lkas, lkas_hud_values, idx))
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_B', bus_lkas, lkas_hud_values, idx))
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_A', bus_lkas, lkas_hud_values))
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_B', bus_lkas, lkas_hud_values))
|
||||
else:
|
||||
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
|
||||
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values))
|
||||
|
||||
if radar_disabled and CP.carFingerprint in HONDA_BOSCH:
|
||||
radar_hud_values = {
|
||||
'CMBS_OFF': 0x01,
|
||||
'SET_TO_1': 0x01,
|
||||
}
|
||||
commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
|
||||
commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values))
|
||||
|
||||
if CP.carFingerprint == CAR.CIVIC_BOSCH:
|
||||
commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {}, idx))
|
||||
commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {}))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def spam_buttons_command(packer, button_val, idx, car_fingerprint):
|
||||
def spam_buttons_command(packer, button_val, car_fingerprint):
|
||||
values = {
|
||||
'CRUISE_BUTTONS': button_val,
|
||||
'CRUISE_SETTING': 0,
|
||||
}
|
||||
# send buttons to camera on radarless cars
|
||||
bus = 2 if car_fingerprint in HONDA_BOSCH_RADARLESS else get_pt_bus(car_fingerprint)
|
||||
return packer.make_can_msg("SCM_BUTTONS", bus, values, idx)
|
||||
return packer.make_can_msg("SCM_BUTTONS", bus, values)
|
||||
|
||||
@@ -73,10 +73,10 @@ class CarController:
|
||||
|
||||
if self.CP.carFingerprint in HDA2_CAR:
|
||||
# steering control
|
||||
can_sends.append(hda2can.create_lkas(self.packer, CC.enabled, self.frame, CC.latActive, apply_steer))
|
||||
can_sends.append(hda2can.create_lkas(self.packer, CC.enabled, CC.latActive, apply_steer))
|
||||
|
||||
if self.frame % 5 == 0:
|
||||
can_sends.append(hda2can.create_cam_0x2a4(self.packer, self.frame, CS.cam_0x2a4))
|
||||
can_sends.append(hda2can.create_cam_0x2a4(self.packer, CS.cam_0x2a4))
|
||||
|
||||
# cruise cancel
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
def create_lkas(packer, enabled, frame, lat_active, apply_steer):
|
||||
def create_lkas(packer, enabled, lat_active, apply_steer):
|
||||
values = {
|
||||
"LKA_MODE": 2,
|
||||
"LKA_ICON": 2 if enabled else 1,
|
||||
@@ -10,17 +10,18 @@ def create_lkas(packer, enabled, frame, lat_active, apply_steer):
|
||||
"NEW_SIGNAL_1": 0,
|
||||
"NEW_SIGNAL_2": 0,
|
||||
}
|
||||
return packer.make_can_msg("LKAS", 4, values, frame % 255)
|
||||
return packer.make_can_msg("LKAS", 4, values)
|
||||
|
||||
def create_cam_0x2a4(packer, frame, camera_values):
|
||||
def create_cam_0x2a4(packer, camera_values):
|
||||
camera_values.update({
|
||||
"BYTE7": 0,
|
||||
})
|
||||
return packer.make_can_msg("CAM_0x2a4", 4, camera_values, frame % 255)
|
||||
return packer.make_can_msg("CAM_0x2a4", 4, camera_values)
|
||||
|
||||
def create_buttons(packer, cnt, btn):
|
||||
values = {
|
||||
"COUNTER": cnt,
|
||||
"SET_ME_1": 1,
|
||||
"CRUISE_BUTTONS": btn,
|
||||
}
|
||||
return packer.make_can_msg("CRUISE_BUTTONS", 5, values, cnt % 0xf)
|
||||
return packer.make_can_msg("CRUISE_BUTTONS", 5, values)
|
||||
|
||||
@@ -33,7 +33,7 @@ class CarController:
|
||||
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
|
||||
|
||||
if CC.latActive:
|
||||
# # windup slower
|
||||
# windup slower
|
||||
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
|
||||
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
|
||||
else:
|
||||
@@ -60,7 +60,7 @@ class CarController:
|
||||
self.last_angle = apply_angle
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd:
|
||||
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, self.frame))
|
||||
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg))
|
||||
|
||||
# TODO: Find better way to cancel!
|
||||
# For some reason spamming the cancel button is unreliable on the Leaf
|
||||
|
||||
@@ -2,17 +2,17 @@ import copy
|
||||
import crcmod
|
||||
from selfdrive.car.nissan.values import CAR
|
||||
|
||||
# TODO: add this checksum to the CANPacker
|
||||
nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torque):
|
||||
idx = (frame % 16)
|
||||
values = {
|
||||
"COUNTER": frame % 0x10,
|
||||
"DESIRED_ANGLE": apply_steer,
|
||||
"SET_0x80_2": 0x80,
|
||||
"SET_0x80": 0x80,
|
||||
"MAX_TORQUE": lkas_max_torque if steer_on else 0,
|
||||
"COUNTER": idx,
|
||||
"LKA_ACTIVE": steer_on,
|
||||
}
|
||||
|
||||
@@ -22,7 +22,7 @@ def create_steering_control(packer, apply_steer, frame, steer_on, lkas_max_torqu
|
||||
return packer.make_can_msg("LKAS", 0, values)
|
||||
|
||||
|
||||
def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg, frame):
|
||||
def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg):
|
||||
values = copy.copy(cruise_throttle_msg)
|
||||
can_bus = 2
|
||||
|
||||
@@ -35,7 +35,6 @@ def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg, frame):
|
||||
values["SET_BUTTON"] = 0
|
||||
values["RES_BUTTON"] = 0
|
||||
values["FOLLOW_DISTANCE_BUTTON"] = 0
|
||||
values["COUNTER"] = (frame % 4)
|
||||
|
||||
return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values)
|
||||
|
||||
|
||||
@@ -39,9 +39,9 @@ class CarController:
|
||||
apply_steer = 0
|
||||
|
||||
if self.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, self.frame, self.p.STEER_STEP))
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer))
|
||||
else:
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, self.frame, self.p.STEER_STEP))
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer))
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
@@ -74,7 +74,9 @@ class CarController:
|
||||
self.es_distance_cnt = CS.es_distance_msg["COUNTER"]
|
||||
|
||||
if self.es_lkas_cnt != CS.es_lkas_msg["COUNTER"]:
|
||||
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.enabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart))
|
||||
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.enabled, hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
|
||||
self.es_lkas_cnt = CS.es_lkas_msg["COUNTER"]
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
|
||||
@@ -3,19 +3,15 @@ from cereal import car
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def create_steering_control(packer, apply_steer, frame, steer_step):
|
||||
|
||||
idx = (frame / steer_step) % 16
|
||||
|
||||
def create_steering_control(packer, apply_steer):
|
||||
values = {
|
||||
"LKAS_Output": apply_steer,
|
||||
"LKAS_Request": 1 if apply_steer != 0 else 0,
|
||||
"SET_1": 1
|
||||
}
|
||||
return packer.make_can_msg("ES_LKAS", 0, values)
|
||||
|
||||
return packer.make_can_msg("ES_LKAS", 0, values, idx)
|
||||
|
||||
def create_steering_status(packer, apply_steer, frame, steer_step):
|
||||
def create_steering_status(packer):
|
||||
return packer.make_can_msg("ES_LKAS_State", 0, {})
|
||||
|
||||
def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd):
|
||||
@@ -66,17 +62,14 @@ def subaru_preglobal_checksum(packer, values, addr):
|
||||
dat = packer.make_can_msg(addr, 0, values)[2]
|
||||
return (sum(dat[:7])) % 256
|
||||
|
||||
def create_preglobal_steering_control(packer, apply_steer, frame, steer_step):
|
||||
|
||||
idx = (frame / steer_step) % 8
|
||||
|
||||
def create_preglobal_steering_control(packer, apply_steer):
|
||||
values = {
|
||||
"LKAS_Command": apply_steer,
|
||||
"LKAS_Active": 1 if apply_steer != 0 else 0
|
||||
}
|
||||
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_LKAS")
|
||||
|
||||
return packer.make_can_msg("ES_LKAS", 0, values, idx)
|
||||
return packer.make_can_msg("ES_LKAS", 0, values)
|
||||
|
||||
def create_preglobal_es_distance(packer, cruise_button, es_distance_msg):
|
||||
|
||||
|
||||
@@ -83,7 +83,7 @@ class CarController:
|
||||
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
|
||||
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
|
||||
# on consecutive messages
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, self.frame))
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req))
|
||||
if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
|
||||
can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2))
|
||||
|
||||
|
||||
@@ -1,10 +1,9 @@
|
||||
def create_steer_command(packer, steer, steer_req, raw_cnt):
|
||||
def create_steer_command(packer, steer, steer_req):
|
||||
"""Creates a CAN message for the Toyota Steer Command."""
|
||||
|
||||
values = {
|
||||
"STEER_REQUEST": steer_req,
|
||||
"STEER_TORQUE_CMD": steer,
|
||||
"COUNTER": raw_cnt,
|
||||
"SET_ME_1": 1,
|
||||
}
|
||||
return packer.make_can_msg("STEERING_LKA", 0, values)
|
||||
|
||||
@@ -69,9 +69,7 @@ class CarController:
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
idx = (self.frame / P.HCA_STEP) % 16
|
||||
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer,
|
||||
idx, hcaEnabled))
|
||||
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hcaEnabled))
|
||||
|
||||
# **** HUD Controls ***************************************************** #
|
||||
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
# CAN controls for MQB platform Volkswagen, Audi, Skoda and SEAT.
|
||||
# CAN controls for MQB platform Volkswagen, Audi, Skoda, and SEAT.
|
||||
# PQ35/PQ46/NMS, and any future MLB, to come later.
|
||||
|
||||
def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled):
|
||||
def create_mqb_steering_control(packer, bus, apply_steer, lkas_enabled):
|
||||
values = {
|
||||
"SET_ME_0X3": 0x3,
|
||||
"Assist_Torque": abs(apply_steer),
|
||||
@@ -13,7 +13,7 @@ def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled):
|
||||
"SET_ME_0XFE": 0xFE,
|
||||
"SET_ME_0X07": 0x07,
|
||||
}
|
||||
return packer.make_can_msg("HCA_01", bus, values, idx)
|
||||
return packer.make_can_msg("HCA_01", bus, values)
|
||||
|
||||
def create_mqb_hud_control(packer, bus, enabled, steering_pressed, hud_alert, left_lane_visible, right_lane_visible,
|
||||
ldw_stock_values, left_lane_depart, right_lane_depart):
|
||||
@@ -34,6 +34,7 @@ def create_mqb_hud_control(packer, bus, enabled, steering_pressed, hud_alert, le
|
||||
|
||||
def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
|
||||
values = {
|
||||
"COUNTER": idx,
|
||||
"GRA_Hauptschalter": CS.graHauptschalter,
|
||||
"GRA_Abbrechen": buttonStatesToSend["cancel"],
|
||||
"GRA_Tip_Setzen": buttonStatesToSend["setCruise"],
|
||||
@@ -46,4 +47,4 @@ def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
|
||||
"GRA_Tip_Stufe_2": CS.graTipStufe2,
|
||||
"GRA_ButtonTypeInfo": CS.graButtonTypeInfo
|
||||
}
|
||||
return packer.make_can_msg("GRA_ACC_01", bus, values, idx)
|
||||
return packer.make_can_msg("GRA_ACC_01", bus, values)
|
||||
|
||||
@@ -1 +1 @@
|
||||
c428493b2a46cb996a7565ea4395d7ad6cd79786
|
||||
658b1097663a8deea2c4d77c4c00a514739980bb
|
||||
+25
-25
@@ -33,48 +33,48 @@ def can_function(pm, speed, angle, idx, cruise_button, is_engaged):
|
||||
# *** powertrain bus ***
|
||||
|
||||
speed = speed * 3.6 # convert m/s to kph
|
||||
msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}, idx))
|
||||
msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed}))
|
||||
msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0, {
|
||||
"WHEEL_SPEED_FL": speed,
|
||||
"WHEEL_SPEED_FR": speed,
|
||||
"WHEEL_SPEED_RL": speed,
|
||||
"WHEEL_SPEED_RR": speed
|
||||
}, -1))
|
||||
}))
|
||||
|
||||
msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": cruise_button}, idx))
|
||||
msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": cruise_button}))
|
||||
|
||||
values = {"COUNTER_PEDAL": idx & 0xF}
|
||||
checksum = crc8_pedal(packer.make_can_msg("GAS_SENSOR", 0, {"COUNTER_PEDAL": idx & 0xF}, -1)[2][:-1])
|
||||
checksum = crc8_pedal(packer.make_can_msg("GAS_SENSOR", 0, {"COUNTER_PEDAL": idx & 0xF})[2][:-1])
|
||||
values["CHECKSUM_PEDAL"] = checksum
|
||||
msg.append(packer.make_can_msg("GAS_SENSOR", 0, values, -1))
|
||||
msg.append(packer.make_can_msg("GAS_SENSOR", 0, values))
|
||||
|
||||
msg.append(packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8}, idx))
|
||||
msg.append(packer.make_can_msg("GAS_PEDAL_2", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1}, idx))
|
||||
msg.append(packer.make_can_msg("STEER_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": angle}, idx))
|
||||
msg.append(packer.make_can_msg("VSA_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if speed >= 1.0 else 0}, idx))
|
||||
msg.append(packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("EPB_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("DOORS_STATUS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("CRUISE_PARAMS", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("CRUISE", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}, idx))
|
||||
msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)}, idx))
|
||||
msg.append(packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8}))
|
||||
msg.append(packer.make_can_msg("GAS_PEDAL_2", 0, {}))
|
||||
msg.append(packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1}))
|
||||
msg.append(packer.make_can_msg("STEER_STATUS", 0, {}))
|
||||
msg.append(packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": angle}))
|
||||
msg.append(packer.make_can_msg("VSA_STATUS", 0, {}))
|
||||
msg.append(packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if speed >= 1.0 else 0}))
|
||||
msg.append(packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {}))
|
||||
msg.append(packer.make_can_msg("EPB_STATUS", 0, {}))
|
||||
msg.append(packer.make_can_msg("DOORS_STATUS", 0, {}))
|
||||
msg.append(packer.make_can_msg("CRUISE_PARAMS", 0, {}))
|
||||
msg.append(packer.make_can_msg("CRUISE", 0, {}))
|
||||
msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}))
|
||||
msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)}))
|
||||
msg.append(packer.make_can_msg("HUD_SETTING", 0, {}))
|
||||
msg.append(packer.make_can_msg("CAR_SPEED", 0, {}, idx))
|
||||
msg.append(packer.make_can_msg("CAR_SPEED", 0, {}))
|
||||
|
||||
# *** cam bus ***
|
||||
msg.append(packer.make_can_msg("STEERING_CONTROL", 2, {}, idx))
|
||||
msg.append(packer.make_can_msg("ACC_HUD", 2, {}, idx))
|
||||
msg.append(packer.make_can_msg("BRAKE_COMMAND", 2, {}, idx))
|
||||
msg.append(packer.make_can_msg("STEERING_CONTROL", 2, {}))
|
||||
msg.append(packer.make_can_msg("ACC_HUD", 2, {}))
|
||||
msg.append(packer.make_can_msg("BRAKE_COMMAND", 2, {}))
|
||||
|
||||
# *** radar bus ***
|
||||
if idx % 5 == 0:
|
||||
msg.append(rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79}, -1))
|
||||
msg.append(rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79}))
|
||||
for i in range(16):
|
||||
msg.append(rpacker.make_can_msg("TRACK_%d" % i, 1, {"LONG_DIST": 255.5}, -1))
|
||||
msg.append(rpacker.make_can_msg("TRACK_%d" % i, 1, {"LONG_DIST": 255.5}))
|
||||
|
||||
pm.send('can', can_list_to_can_capnp(msg))
|
||||
|
||||
|
||||
Reference in New Issue
Block a user