diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 48255866c7..c63c05cb1a 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -31,8 +31,6 @@ class CarController: if CP.spFlags & SubaruFlagsSP.SP_SUBARU_SNG: self.subaru_sng = True self.manual_parking_brake = self.param_s.get_bool("SubaruManualParkingBrakeSng") - self.throttle_cnt = -1 - self.brake_pedal_cnt = -1 self.prev_close_distance = 0 self.prev_standstill = False self.standstill_start = 0 @@ -121,7 +119,7 @@ class CarController: can_sends.append(subarucan.create_preglobal_es_distance(self.packer, cruise_button, CS.es_distance_msg)) if self.subaru_sng: - can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg, throttle_cmd)) + can_sends.append(subarucan.create_preglobal_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, throttle_cmd)) else: if self.frame % 10 == 0: @@ -136,10 +134,10 @@ class CarController: can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert)) if self.subaru_sng: - can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg, throttle_cmd)) + can_sends.append(subarucan.create_throttle(self.packer, CS.throttle_msg["COUNTER"] + 1, CS.throttle_msg, throttle_cmd)) if self.frame % 2 == 0: - can_sends.append(subarucan.create_brake_pedal(self.packer, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd)) + can_sends.append(subarucan.create_brake_pedal(self.packer, self.frame // 2, CS.brake_pedal_msg, speed_cmd, pcm_cancel_cmd)) if self.CP.openpilotLongitudinalControl: if self.frame % 5 == 0: diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 411b034323..bf847a6d23 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -293,9 +293,8 @@ def create_preglobal_es_distance(packer, cruise_button, es_distance_msg): return packer.make_can_msg("ES_Distance", CanBus.main, values) -def create_brake_pedal(packer, brake_pedal_msg, speed_cmd, brake_cmd): +def create_brake_pedal(packer, frame, brake_pedal_msg, speed_cmd, brake_cmd): values = {s: brake_pedal_msg[s] for s in [ - "COUNTER", "Signal1", "Speed", "Signal2", @@ -305,6 +304,8 @@ def create_brake_pedal(packer, brake_pedal_msg, speed_cmd, brake_cmd): "Signal4", ]} + values["COUNTER"] = frame % 0x10 + if speed_cmd: values["Speed"] = 3 if brake_cmd: @@ -314,10 +315,9 @@ def create_brake_pedal(packer, brake_pedal_msg, speed_cmd, brake_cmd): return packer.make_can_msg("Brake_Pedal", CanBus.camera, values) -def create_throttle(packer, throttle_msg, throttle_cmd): +def create_throttle(packer, frame, throttle_msg, throttle_cmd): values = {s: throttle_msg[s] for s in [ "CHECKSUM", - "COUNTER", "Signal1", "Engine_RPM", "Signal2", @@ -328,16 +328,17 @@ def create_throttle(packer, throttle_msg, throttle_cmd): "Off_Accel", ]} + values["COUNTER"] = frame % 0x10 + if throttle_cmd: values["Throttle_Pedal"] = 5 return packer.make_can_msg("Throttle", 2, values) -def create_preglobal_throttle(packer, throttle_msg, throttle_cmd): +def create_preglobal_throttle(packer, frame, throttle_msg, throttle_cmd): values = {s: throttle_msg[s] for s in [ "Throttle_Pedal", - "COUNTER", "Signal1", "Not_Full_Throttle", "Signal2", @@ -351,6 +352,8 @@ def create_preglobal_throttle(packer, throttle_msg, throttle_cmd): "Signal4", ]} + values["COUNTER"] = frame % 0x10 + if throttle_cmd: values["Throttle_Pedal"] = 5