diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0ccab439b4..92f699abef 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -137,7 +137,7 @@ class CarController(CarControllerBase): can_sends.extend(hyundaicanfd.create_radar_aux_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: if can_canfd_hybrid: - can_sends.extend(hyundaicanfd.create_acc_commands_can_canfd_hybrid(self.packer, self.CAN, CC.enabled, accel, + can_sends.extend(hyundaicanfd.create_acc_commands_can_canfd_hybrid(self.packer, self.CAN, CC.enabled, accel, self.accel_last, int(self.frame / 2), hud_control.leadVisible, set_speed_in_units, stopping, CC.cruiseControl.override, hud_control)) else: diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index f48787fccb..5757f7303b 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -192,15 +192,22 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_acc_commands_can_canfd_hybrid(packer, CAN, enabled, accel, idx, lead_visible, set_speed, stopping, long_override, hud_control): +def create_acc_commands_can_canfd_hybrid(packer, CAN, enabled, accel, accel_last, idx, lead_visible, set_speed, stopping, long_override, hud_control): + jerk = 5 + jn = jerk / 50 + if not enabled or long_override: + a_val, a_raw = 0, 0 + else: + a_raw = accel + a_val = clip(accel, accel_last - jn, accel_last + jn) ret = [] msg_values = [ ("SCC11", { - "aReqRaw": accel, - "aReqValue": accel, + "aReqRaw": a_raw, + "aReqValue": a_val, "JerkUpperLimit": 3.0, - "JerkLowerLimit": 5 if enabled else 1, + "JerkLowerLimit": jerk if enabled else 1, }), ("SCC12", { @@ -222,7 +229,7 @@ def create_acc_commands_can_canfd_hybrid(packer, CAN, enabled, accel, idx, lead_ ("FCA11", { "BYTE4": 0xC0, "BYTE5": 0x3F, - "BYTE6": 0x7F + "BYTE6": 0x7F, }), ]