diff --git a/panda b/panda index fe72dda172..0a2db31fcc 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit fe72dda17244cf794eebb0b857776e69052159a1 +Subproject commit 0a2db31fcc8f392684f4d5a7c7ddd6c418c7fb47 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 33631ece46..63015c10ce 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -13,7 +13,6 @@ def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - self.last_right_stalk_press = 0 self.pcm_cancel_cmd = False # Must be latching because of frame rate self.acc_mismatch_start_nanos = None @@ -41,17 +40,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 hands_on_fault and not CS.params_list.enable_mads: self.pcm_cancel_cmd = True @@ -69,15 +57,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 diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 8693c36eee..03801ce542 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -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 = { @@ -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)