Skip to content

Commit

Permalink
Merge branch 'cancel-test-with-mads' into tesla-port-dev
Browse files Browse the repository at this point in the history
  • Loading branch information
carleeno committed Aug 5, 2024
2 parents ad25c13 + 15633ec commit 6a08034
Show file tree
Hide file tree
Showing 3 changed files with 18 additions and 44 deletions.
2 changes: 1 addition & 1 deletion panda
Submodule panda updated from fe72dd to 0a2db3
35 changes: 14 additions & 21 deletions selfdrive/car/tesla/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@ def __init__(self, dbc_name, CP, VM):
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
self.virtual_blending = Params().get_bool("TeslaVirtualTorqueBlending")
self.last_right_stalk_press = 0
self.pcm_cancel_cmd = False # Must be latching because of frame rate
self.acc_mismatch_start_nanos = None

Expand Down Expand Up @@ -81,17 +80,6 @@ def update(self, CC, CS, now_nanos):
use_lka_mode = CS.params_list.enable_mads
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, (self.frame // 2) % 16, use_lka_mode))

# Longitudinal control
if self.CP.openpilotLongitudinalControl:
acc_state = CS.das_control["DAS_accState"]
target_accel = actuators.accel
target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
max_accel = 0 if target_accel < 0 else target_accel
min_accel = 0 if target_accel > 0 else target_accel

counter = CS.das_control["DAS_controlCounter"]
can_sends.append(self.tesla_can.create_longitudinal_commands(acc_state, target_speed, min_accel, max_accel, counter))

if not self.virtual_blending:
# Cancel on user steering override when blending and mads is disabled
if CS.steering_override and not CS.params_list.enable_mads:
Expand All @@ -111,15 +99,20 @@ def update(self, CC, CS, now_nanos):
if not CS.acc_enabled:
self.pcm_cancel_cmd = False

# Send cancel request only if ACC is enabled
if self.frame % 10 == 0 and self.pcm_cancel_cmd:
counter = int(CS.sccm_right_stalk_counter)
# Alternate between 1 and 0 every 10 frames, car needs time to detect falling edge in order to prevent shift to N
value = 1 if self.last_right_stalk_press == 0 else 0
can_sends.append(self.tesla_can.right_stalk_press((counter + 1) % 16 , value))
self.last_right_stalk_press = value
elif self.last_right_stalk_press != 0 and self.frame % 10 == 0:
self.last_right_stalk_press = 0
# Longitudinal control
if self.pcm_cancel_cmd:
# Increment counter so cancel is prioritized even with stock TACC
counter = (CS.das_control["DAS_controlCounter"] + 1) % 8
can_sends.append(self.tesla_can.cancel_acc(counter))
elif self.CP.openpilotLongitudinalControl:
acc_state = CS.das_control["DAS_accState"]
target_accel = actuators.accel
target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
max_accel = 0 if target_accel < 0 else target_accel
min_accel = 0 if target_accel > 0 else target_accel

counter = CS.das_control["DAS_controlCounter"]
can_sends.append(self.tesla_can.create_longitudinal_commands(acc_state, target_speed, min_accel, max_accel, counter))

# TODO: HUD control

Expand Down
25 changes: 3 additions & 22 deletions selfdrive/car/tesla/teslacan.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,15 +17,6 @@ def checksum(msg_id, dat):
ret += sum(dat)
return ret & 0xFF

@staticmethod
def right_stalk_crc(dat):
right_stalk_val = [0x7C, 0xB6, 0xF0, 0x2F, 0x69, 0xA3, 0xDD, 0x1C, 0x56, 0x90, 0xCA, 0x09, 0x43, 0x7D, 0xB7, 0xF1]
cntr = dat[0] & 0xF
crc1_func = crcmod.mkCrcFun(0x12F, initCrc=0x00, xorOut=0xFF, rev=False)
crc1 = crc1_func(dat) & 0xFF
crc2_func = crcmod.mkCrcFun(0x12F, initCrc=crc1, xorOut=0xFF, rev=False)
return crc2_func(bytes([right_stalk_val[cntr]])) & 0xFF

def create_steering_control(self, angle, enabled, counter, use_lka_mode):
control_type = 2 if use_lka_mode else 1
values = {
Expand Down Expand Up @@ -55,16 +46,6 @@ def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, c
values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
return self.packer.make_can_msg("DAS_control", CANBUS.party, values)

def right_stalk_press(self, counter, position):
values = {
"SCCM_rightStalkCrc": 0,
"SCCM_rightStalkCounter": counter,
"SCCM_rightStalkStatus": position,
"SCCM_rightStalkReserved1": 0,
"SCCM_parkButtonStatus": 0,
"SCCM_rightStalkReserved2": 0,
}

data = self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values)[1]
values["SCCM_rightStalkCrc"] = self.right_stalk_crc(data[1:])
return self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values)
def cancel_acc(self, counter):
# 13 = ACC_CANCEL_GENERIC_SILENT
return self.create_longitudinal_commands(13, 0, 0, 0, counter)

0 comments on commit 6a08034

Please sign in to comment.