diff --git a/panda b/panda index 69215887dc..893a81aa82 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 69215887dc5a28170b558f3f7d4f2d87eba42995 +Subproject commit 893a81aa823254cd72c9817886d4b4039ea04a7e diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 2c1f55e7de..cf43b8ef00 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -9,7 +9,6 @@ class CarController: self.CP = CP self.frame = 0 self.last_angle = 0 - self.long_control_counter = 0 self.packer = CANPacker(dbc_name) self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) @@ -41,15 +40,15 @@ class CarController: self.last_angle = apply_angle can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame)) - # Longitudinal control (40Hz) - if self.CP.openpilotLongitudinalControl and self.frame % 5 in (0, 2): + # Longitudinal control (in sync with stock message, about 40Hz) + if self.CP.openpilotLongitudinalControl: 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 - can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, self.long_control_counter)) - self.long_control_counter += 1 + while len(CS.das_control_counters) > 0: + can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft())) # Cancel on user steering override, since there is no steering torque blending if hands_on_fault: diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 435beedc9c..0f373842f2 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -1,4 +1,5 @@ import copy +from collections import deque from cereal import car from common.conversions import Conversions as CV from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS @@ -17,6 +18,7 @@ class CarState(CarStateBase): self.hands_on_level = 0 self.steer_warning = None self.acc_state = 0 + self.das_control_counters = deque(maxlen=32) def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -87,9 +89,13 @@ class CarState(CarStateBase): # TODO: blindspot + # AEB + ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1) + # Messages needed by carcontroller self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"]) self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"] + self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"]) return ret @@ -177,6 +183,8 @@ class CarState(CarStateBase): signals = [ # sig_name, sig_address ("DAS_accState", "DAS_control"), + ("DAS_aebEvent", "DAS_control"), + ("DAS_controlCounter", "DAS_control"), ] checks = [ # sig_address, frequency diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 17a3b11985..e5d904f80e 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -51,7 +51,7 @@ class TeslaCAN: "DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX, "DAS_accelMin": min_accel, "DAS_accelMax": max_accel, - "DAS_controlCounter": (cnt % 8), + "DAS_controlCounter": cnt, "DAS_controlChecksum": 0, }