From 4688177cf101046b7a1aa091d5ce28bb1228bdf0 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Fri, 19 May 2023 00:09:33 -0500 Subject: [PATCH 1/5] added new mazda --- common/realtime.py | 85 ++++++++++ selfdrive/car/interfaces.py | 17 ++ selfdrive/car/mazda/carcontroller.py | 94 +++++++---- selfdrive/car/mazda/carstate.py | 201 +++++++++++++++++++++--- selfdrive/car/mazda/interface.py | 55 ++++++- selfdrive/car/mazda/mazdacan.py | 134 ++++++++++------ selfdrive/car/mazda/values.py | 110 +++++++++++-- selfdrive/car/torque_data/override.yaml | 8 + selfdrive/controls/controlsd.py | 3 + selfdrive/controls/lib/latcontrol_ti.py | 92 +++++++++++ selfdrive/ui/qt/offroad/settings.cc | 6 + 11 files changed, 678 insertions(+), 127 deletions(-) create mode 100644 selfdrive/controls/lib/latcontrol_ti.py diff --git a/common/realtime.py b/common/realtime.py index 7dd2eb98a..defd1a1fd 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -94,3 +94,88 @@ def monitor_time(self) -> bool: self._frame += 1 self._remaining = remaining return lagged + +class DurationTimer: + def __init__(self, duration=0, step=DT_CTRL) -> None: + self.step = step + self.duration = duration + self.was_reset = False + self.timer = 0 + self.min = float("-inf") # type: float + self.max = float("inf") # type: float + + def tick_obj(self) -> None: + self.timer += self.step + # reset on overflow + self.reset() if (self.timer == (self.max or self.min)) else None + + def reset(self) -> None: + """Resets this objects timer""" + self.timer = 0 + self.was_reset = True + + def active(self) -> bool: + """Returns true if time since last reset is less than duration""" + return bool(round(self.timer,2) < self.duration) + + def adjust(self, duration) -> None: + """Adjusts the duration of the timer""" + self.duration = duration + + def once_after_reset(self) -> bool: + """Returns true only one time after calling reset()""" + ret = self.was_reset + self.was_reset = False + return ret + + @staticmethod + def interval_obj(rate, frame) -> bool: + if frame % rate == 0: # Highlighting shows "frame" in white + return True + return False + +class ModelTimer(DurationTimer): + frame = 0 # type: int + objects = [] # type: List[DurationTimer] + def __init__(self, duration=0) -> None: + self.step = DT_MDL + super().__init__(duration, self.step) + self.__class__.objects.append(self) + + @classmethod + def tick(cls) -> None: + cls.frame += 1 + for obj in cls.objects: + ModelTimer.tick_obj(obj) + + @classmethod + def reset_all(cls) -> None: + for obj in cls.objects: + obj.reset() + + @classmethod + def interval(cls, rate) -> bool: + return ModelTimer.interval_obj(rate, cls.frame) + +class ControlsTimer(DurationTimer): + frame = 0 + objects = [] # type: List[DurationTimer] + def __init__(self, duration=0) -> None: + self.step = DT_CTRL + super().__init__(duration=duration, step=self.step) + self.__class__.objects.append(self) + + @classmethod + def tick(cls) -> None: + cls.frame += 1 + for obj in cls.objects: + ControlsTimer.tick_obj(obj) + + @classmethod + def reset_all(cls) -> None: + for obj in cls.objects: + obj.reset() + + @classmethod + def interval(cls, rate) -> bool: + return ControlsTimer.interval_obj(rate, cls.frame) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a14dca465..1505aeb64 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -209,6 +209,21 @@ def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_ tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR'] tune.torque.latAccelOffset = 0.0 tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg + + @staticmethod + def configure_ti_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True): + params = get_torque_params(candidate) + + tune.init('ti') + tune.ti.useSteeringAngle = use_steering_angle + tune.ti.kp = 1.0 + tune.ti.kf = 1.0 + tune.ti.ki = 0.1 + tune.ti.friction = params['FRICTION'] + tune.ti.latAccelFactor = params['LAT_ACCEL_FACTOR'] + tune.ti.latAccelOffset = 0.0 + tune.ti.latAngleFactor = .14 + tune.ti.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg @staticmethod def configure_dp_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True): @@ -217,6 +232,8 @@ def configure_dp_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_stee CarInterfaceBase.configure_lqr_tune(tune) elif params.get_bool('dp_lateral_torque'): CarInterfaceBase.configure_torque_tune(candidate, tune, steering_angle_deadzone_deg, use_steering_angle) + elif params.get_bool('dp_lateral_ti'): + CarInterfaceBase.configure_ti_tune(candidate, tune, steering_angle_deadzone_deg, use_steering_angle) @abstractmethod def _update(self, c: car.CarControl) -> car.CarState: diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index cb401a8ab..629f89b5f 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -2,9 +2,11 @@ from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.mazda import mazdacan -from selfdrive.car.mazda.values import CarControllerParams, Buttons +from selfdrive.car.mazda.values import CarControllerParams, Buttons, GEN1 +from common.realtime import ControlsTimer as Timer VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState class CarController: @@ -14,52 +16,82 @@ def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) self.brake_counter = 0 self.frame = 0 + self.params = CarControllerParams(CP) + self.hold_timer = Timer(6.0) + self.hold_delay = Timer(1.0) # delay before we start holding as to not hit the brakes too hard + self.resume_timer = Timer(0.5) def update(self, CC, CS, now_nanos): can_sends = [] apply_steer = 0 - + if CC.latActive: # calculate steer and also set limits due to driver torque - new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX)) + new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX)) apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, - CS.out.steeringTorque, CarControllerParams) - - if CC.cruiseControl.cancel: - # If brake is pressed, let us wait >70ms before trying to disable crz to avoid - # a race condition with the stock system, where the second cancel from openpilot - # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to - # read 3 messages and most likely sync state before we attempt cancel. - self.brake_counter = self.brake_counter + 1 - if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): - # Cancel Stock ACC if it's enabled while OP is disengaged - # Send at a rate of 10hz until we sync with stock ACC state - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL)) - else: - self.brake_counter = 0 - if CC.cruiseControl.resume and self.frame % 5 == 0: - # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds - # Send Resume button when planner wants car to move - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) - + CS.out.steeringTorque, self.params) self.apply_steer_last = apply_steer + + if self.CP.carFingerprint in GEN1: + if CC.cruiseControl.cancel: + # If brake is pressed, let us wait >70ms before trying to disable crz to avoid + # a race condition with the stock system, where the second cancel from openpilot + # will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to + # read 3 messages and most likely sync state before we attempt cancel. + self.brake_counter = self.brake_counter + 1 + if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): + # Cancel Stock ACC if it's enabled while OP is disengaged + # Send at a rate of 10hz until we sync with stock ACC state + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL)) + else: + self.brake_counter = 0 + if CC.cruiseControl.resume and self.frame % 5 == 0: + # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds + # Send Resume button when planner wants car to move + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) - # send HUD alerts - if self.frame % 50 == 0: - ldw = CC.hudControl.visualAlert == VisualAlert.ldw - steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired - # TODO: find a way to silence audible warnings so we can add more hud alerts - steer_required = steer_required and CS.lkas_allowed_speed - can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) - + # send HUD alerts + if self.frame % 50 == 0: + ldw = CC.hudControl.visualAlert == VisualAlert.ldw + steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired + # TODO: find a way to silence audible warnings so we can add more hud alerts + steer_required = steer_required and CS.lkas_allowed_speed + can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) + + else: # gen2 + resume = False + hold = False + if Timer.interval(2): # send ACC command at 50hz + """ + Without this hold/resum logic, the car will only stop momentarily. + It will then start creeping forward again. This logic allows the car to + apply the electric brake to hold the car. The hold delay also fixes a + bug with the stock ACC where it sometimes will apply the brakes too early + when coming to a stop. + """ + if CS.out.standstill: # if we're stopped + if not self.hold_delay.active(): # and we have been stopped for more than hold_delay duration. This prevents a hard brake if we aren't fully stopped. + if (CC.cruiseControl.resume or CC.cruiseControl.override or (CC.actuators.longControlState == LongCtrlState.starting)): # and we want to resume + self.resume_timer.reset() # reset the resume timer so its active + else: # otherwise we're holding + hold = self.hold_timer.active() # hold for 6s. This allows the electric brake to hold the car. + + else: # if we're moving + self.hold_timer.reset() # reset the hold timer so its active when we stop + self.hold_delay.reset() # reset the hold delay + + resume = self.resume_timer.active() # stay on for 0.5s to release the brake. This allows the car to move. + can_sends.append(mazdacan.create_acc_cmd(self, self.packer, CS, CC, hold, resume)) + # send steering command can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint, self.frame, apply_steer, CS.cam_lkas)) new_actuators = CC.actuators.copy() - new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer self.frame += 1 + Timer.tick() return new_actuators, can_sends diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 944d79809..bda59961c 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -1,9 +1,11 @@ +import copy + from cereal import car from common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1 +from selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1, GEN2, CarControllerParams class CarState(CarStateBase): def __init__(self, CP): @@ -17,8 +19,63 @@ def __init__(self, CP): self.low_speed_alert = False self.lkas_allowed_speed = False self.lkas_disabled = False + self.cam_lkas = 0 + self.params = CarControllerParams(CP) + + if CP.carFingerprint in GEN1: + self.update = self.update_gen1 + if CP.carFingerprint in GEN2: + self.update = self.update_gen2 + + def update_gen2(self, cp, cp_cam, cp_body): + ret = car.CarState.new_message() + ret.wheelSpeeds = self.get_wheel_speeds( + cp_cam.vl["WHEEL_SPEEDS"]["FL"], + cp_cam.vl["WHEEL_SPEEDS"]["FR"], + cp_cam.vl["WHEEL_SPEEDS"]["RL"], + cp_cam.vl["WHEEL_SPEEDS"]["RR"], + ) + + + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Doesn't match cluster speed exactly + + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(100, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, + cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + + ret.steeringAngleDeg = cp_cam.vl["STEER"]["STEER_ANGLE"] + + ret.steeringTorque = cp_body.vl["EPS_FEEDBACK"]["STEER_TORQUE_SENSOR"] + can_gear = int(cp_cam.vl["GEAR"]["GEAR"]) + ret.gas = cp_cam.vl["ENGINE_DATA"]["PEDAL_GAS"] + + unit_conversion = CV.MPH_TO_MS if cp.vl["SYSTEM_SETTINGS"]["IMPERIAL_UNIT"] else CV.KPH_TO_MS + + ret.steeringPressed = abs(ret.steeringTorque) > self.params.STEER_DRIVER_ALLOWANCE + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.gasPressed = ret.gas > 0 + ret.seatbeltUnlatched = False # Cruise will not engage if seatbelt is unlatched (handled by car) + ret.doorOpen = False # Cruise will not engage if door is open (handled by car) + ret.brakePressed = cp.vl["BRAKE_PEDAL"]["BRAKE_PEDAL_PRESSED"] == 1 + ret.brake = .1 + ret.steerFaultPermanent = False # TODO locate signal. Car shows light on dash if there is a fault + ret.steerFaultTemporary = False # TODO locate signal. Car shows light on dash if there is a fault + ret.cruiseState.available = True # TODO locate signal. + ret.cruiseState.speed = cp.vl["CRUZE_STATE"]["CRZ_SPEED"] * unit_conversion + ret.cruiseState.enabled = ( (cp.vl["CRUZE_STATE"]["CRZ_ENABLED"] == 1) or (cp.vl["CRUZE_STATE"]["PRE_ENABLE"] == 1) ) + + speed_kph = cp_cam.vl["SPEED"]["SPEED"] * unit_conversion + ret.standstill = speed_kph < .1 + ret.cruiseState.standstill = False + self.cp = cp + self.cp_cam = cp_cam + self.acc = copy.copy(cp.vl["ACC"]) + # end GEN2 - def update(self, cp, cp_cam): + return ret + + + def update_gen1(self, cp, cp_cam, _): ret = car.CarState.new_message() ret.wheelSpeeds = self.get_wheel_speeds( @@ -107,29 +164,32 @@ def update(self, cp, cp_cam): @staticmethod def get_can_parser(CP): - signals = [ - # sig_name, sig_address - ("LEFT_BLINK", "BLINK_INFO"), - ("RIGHT_BLINK", "BLINK_INFO"), - ("HIGH_BEAMS", "BLINK_INFO"), - ("STEER_ANGLE", "STEER"), - ("STEER_ANGLE_RATE", "STEER_RATE"), - ("STEER_TORQUE_SENSOR", "STEER_TORQUE"), - ("STEER_TORQUE_MOTOR", "STEER_TORQUE"), - ("FL", "WHEEL_SPEEDS"), - ("FR", "WHEEL_SPEEDS"), - ("RL", "WHEEL_SPEEDS"), - ("RR", "WHEEL_SPEEDS"), - ] - - checks = [ - # sig_address, frequency - ("BLINK_INFO", 10), - ("STEER", 67), - ("STEER_RATE", 83), - ("STEER_TORQUE", 83), - ("WHEEL_SPEEDS", 100), - ] + signals = [] + checks = [] + if CP.carFingerprint not in GEN2: + signals = [ + # sig_name, sig_address + ("LEFT_BLINK", "BLINK_INFO"), + ("RIGHT_BLINK", "BLINK_INFO"), + ("HIGH_BEAMS", "BLINK_INFO"), + ("STEER_ANGLE", "STEER"), + ("STEER_ANGLE_RATE", "STEER_RATE"), + ("STEER_TORQUE_SENSOR", "STEER_TORQUE"), + ("STEER_TORQUE_MOTOR", "STEER_TORQUE"), + ("FL", "WHEEL_SPEEDS"), + ("FR", "WHEEL_SPEEDS"), + ("RL", "WHEEL_SPEEDS"), + ("RR", "WHEEL_SPEEDS"), + ] + + checks = [ + # sig_address, frequency + ("BLINK_INFO", 10), + ("STEER", 67), + ("STEER_RATE", 83), + ("STEER_TORQUE", 83), + ("WHEEL_SPEEDS", 100), + ] if CP.carFingerprint in GEN1: signals += [ @@ -168,6 +228,57 @@ def get_can_parser(CP): ("BSM", 10), ] + if CP.carFingerprint in GEN2: + signals += [ + ("CRZ_SPEED", "CRUZE_STATE", 0), + ("CRZ_ENABLED", "CRUZE_STATE", 0), + ("PRE_ENABLE", "CRUZE_STATE", 0), + ("BRAKE_PEDAL_PRESSED", "BRAKE_PEDAL", 0), + ("SET_P", "CRZ_BTNS",0), + ("SET_M", "CRZ_BTNS",0), + ("RES", "CRZ_BTNS",0), + ("CAN", "CRZ_BTNS",0), + ("MODE_ACC", "CRZ_BTNS",0), + ("MODE_LKAS", "CRZ_BTNS",0), + ("DISTANCE_P", "CRZ_BTNS",0), + ("DISTANCE_M", "CRZ_BTNS",0), + ("STATIC_0", "CRZ_BTNS",0), + ("STATIC_1", "CRZ_BTNS",0), + ("STATIC_2", "CRZ_BTNS",0), + ("CTR", "CRZ_BTNS", 0), + ("LEFT_BLINK", "BLINK_INFO", 0), + ("RIGHT_BLINK", "BLINK_INFO", 0), + ("IMPERIAL_UNIT", "SYSTEM_SETTINGS", 0), + ("ACCEL_CMD", "ACC", 0), + ("HOLD", "ACC", 0), + ("RESUME", "ACC", 0), + ("NEW_SIGNAL_1", "ACC", 0), + ("NEW_SIGNAL_2", "ACC", 0), + ("NEW_SIGNAL_3", "ACC", 0), + ("NEW_SIGNAL_4", "ACC", 0), + ("NEW_SIGNAL_5", "ACC", 0), + ("NEW_SIGNAL_6", "ACC", 0), + ("NEW_SIGNAL_7", "ACC", 0), + ("NEW_SIGNAL_8", "ACC", 0), + ("NEW_SIGNAL_9", "ACC", 0), + ("NEW_SIGNAL_10", "ACC", 0), + ("NEW_SIGNAL_11", "ACC", 0), + ("NEW_SIGNAL_12", "ACC", 0), + ("NEW_SIGNAL_13", "ACC", 0), + ("ACC_ENABLED", "ACC", 0), + ("ACC_ENABLED_2", "ACC", 0), + ("CHECKSUM", "ACC", 0), + ] + + checks += [ + ("BRAKE_PEDAL", 20), + ("CRUZE_STATE", 10), + ("BLINK_INFO", 10), + ("ACC", 50), + ("CRZ_BTNS", 10), + ("SYSTEM_SETTINGS", 10), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod @@ -187,7 +298,6 @@ def get_cam_can_parser(CP): ("STEERING_ANGLE", "CAM_LKAS"), ("ANGLE_ENABLED", "CAM_LKAS"), ("CHKSUM", "CAM_LKAS"), - ("LINE_VISIBLE", "CAM_LANEINFO"), ("LINE_NOT_VISIBLE", "CAM_LANEINFO"), ("LANE_LINES", "CAM_LANEINFO"), @@ -205,4 +315,43 @@ def get_cam_can_parser(CP): ("CAM_LKAS", 16), ] + if CP.carFingerprint in GEN2: + signals += [ + ("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0), + ("GEAR", "GEAR", 0), + ("PEDAL_GAS", "ENGINE_DATA", 0), + ("FL", "WHEEL_SPEEDS", 0), + ("FR", "WHEEL_SPEEDS", 0), + ("RL", "WHEEL_SPEEDS", 0), + ("RR", "WHEEL_SPEEDS", 0), + ("STEER_ANGLE", "STEER", 0), + ("SPEED", "SPEED", 0), + ] + checks += [ + ("ENGINE_DATA", 100), + ("STEER_TORQUE", 100), + ("GEAR", 40), + ("WHEEL_SPEEDS", 100), + ("STEER", 100), + ("SPEED", 50), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) + + @staticmethod + def get_body_can_parser(CP): + signals = [] + checks = [] + if CP.carFingerprint in GEN2: + signals += [ + ("STEER_TORQUE_SENSOR", "EPS_FEEDBACK", 0), + ("HALL2", "EPS_FEEDBACK2", 0), + ("HALL3", "EPS_FEEDBACK2", 0), + ("HALL4", "EPS_FEEDBACK2", 0), + ] + checks += [ + ("EPS_FEEDBACK", 50), + ("EPS_FEEDBACK2", 50), + ] + + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 28d16a428..5be72f8dc 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,20 +1,52 @@ #!/usr/bin/env python3 from cereal import car from common.conversions import Conversions as CV -from selfdrive.car.mazda.values import CAR, LKAS_LIMITS +from selfdrive.car.mazda.values import CAR, LKAS_LIMITS, GEN2, GEN1 from selfdrive.car import STD_CARGO_KG, scale_tire_stiffness, get_safety_config -from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.controls.lib.drive_helpers import get_friction +from selfdrive.car.interfaces import CarInterfaceBase, FRICTION_THRESHOLD from common.params import Params +from typing import Callable ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName +TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, float, float, bool], float] class CarInterface(CarInterfaceBase): + + @staticmethod + def torque_from_lateral_accel_ti(lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, + lateral_accel_error: float, lateral_accel_deadzone: float, + steering_angle: float, vego: float, friction_compensation: bool) -> float: + steering_angle = abs(steering_angle) + lat_factor = torque_params.latAccelFactor * ((steering_angle * torque_params.latAngleFactor) + 1) + + friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) + return (lateral_accel_value / lat_factor) + friction + + def torque_from_lateral_accel(self): + if self.CP.carFingerprint in GEN2: + return self.torque_from_lateral_accel_ti # different return type + else: + return self.torque_from_lateral_accel_linear @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "mazda" - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] + if candidate in GEN1: + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] + + if candidate in GEN2: + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda2019)] + ret.openpilotLongitudinalControl = True + ret.stopAccel = -.5 + ret.vEgoStarting = .2 + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [0.0, 0.0, 0.0] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.1, 0.1] + ret.startingState = True + ret.radarUnavailable = True ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) and not Params().get_bool('dp_mazda_dashcam_bypass') @@ -24,6 +56,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): tire_stiffness_factor = 0.70 # not optimized yet CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + CarInterfaceBase.configure_ti_tune(candidate, ret.lateralTuning) if candidate in (CAR.CX5, CAR.CX5_2022): ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG @@ -41,8 +74,20 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.mass = 3443 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.83 ret.steerRatio = 15.5 + elif candidate in CAR.MAZDA3_2019: + ret.mass = 3000 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.725 + ret.steerRatio = 17.0 + elif candidate in (CAR.CX_30, CAR.CX_50): + ret.mass = 3375 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.steerRatio = 15.5 + elif candidate in (CAR.CX_60, CAR.CX_80, CAR.CX_70, CAR.CX_90): + ret.mass = 4217 * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 3.1 + ret.steerRatio = 17.6 - if candidate not in (CAR.CX5_2022, ): + if candidate not in (CAR.CX5_2022, CAR.MAZDA3_2019, CAR.CX_30, CAR.CX_50, CAR.CX_60, CAR.CX_70, CAR.CX_80, CAR.CX_90): ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS CarInterfaceBase.configure_dp_tune(candidate, ret.lateralTuning) @@ -58,7 +103,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): # returns a car.CarState def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) ret.cruiseState.enabled, ret.cruiseState.available = self.dp_atl_mode(ret) # events diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index e2ee93e02..04632de75 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,66 +1,79 @@ import copy -from selfdrive.car.mazda.values import GEN1, Buttons +from selfdrive.car.mazda.values import GEN1, GEN2, Buttons +from common.params import Params def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): - tmp = apply_steer + 2048 - - lo = tmp & 0xFF - hi = tmp >> 8 - - # copy values from camera - b1 = int(lkas["BIT_1"]) - er1 = int(lkas["ERR_BIT_1"]) - lnv = 0 - ldw = 0 - er2 = int(lkas["ERR_BIT_2"]) - - # Some older models do have these, newer models don't. - # Either way, they all work just fine if set to zero. - steering_angle = 0 - b2 = 0 - - tmp = steering_angle + 2048 - ahi = tmp >> 10 - amd = (tmp & 0x3FF) >> 2 - amd = (amd >> 4) | (( amd & 0xF) << 4) - alo = (tmp & 0x3) << 2 - - ctr = frame % 16 - # bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ] - csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5) - - # bytes [ 5 ] [ 6 ] [ 7 ] - csum = csum - ahi - amd - alo - b2 - - if ahi == 1: - csum = csum + 15 - - if csum < 0: - if csum < -256: - csum = csum + 512 - else: - csum = csum + 256 - - csum = csum % 256 - if car_fingerprint in GEN1: + tmp = apply_steer + 2048 + + lo = tmp & 0xFF + hi = tmp >> 8 + + # copy values from camera + b1 = int(lkas["BIT_1"]) + er1 = int(lkas["ERR_BIT_1"]) + lnv = 0 + ldw = 0 + er2 = int(lkas["ERR_BIT_2"]) + + # Some older models do have these, newer models don't. + # Either way, they all work just fine if set to zero. + steering_angle = 0 + b2 = 0 + + tmp = steering_angle + 2048 + ahi = tmp >> 10 + amd = (tmp & 0x3FF) >> 2 + amd = (amd >> 4) | (( amd & 0xF) << 4) + alo = (tmp & 0x3) << 2 + + ctr = frame % 16 + # bytes: [ 1 ] [ 2 ] [ 3 ] [ 4 ] + csum = 249 - ctr - hi - lo - (lnv << 3) - er1 - (ldw << 7) - ( er2 << 4) - (b1 << 5) + + # bytes [ 5 ] [ 6 ] [ 7 ] + csum = csum - ahi - amd - alo - b2 + + if ahi == 1: + csum = csum + 15 + + if csum < 0: + if csum < -256: + csum = csum + 512 + else: + csum = csum + 256 + + csum = csum % 256 + + bus = 0 + sig_name = "CAM_LKAS" + + if car_fingerprint in GEN1: + values = { + "LKAS_REQUEST": apply_steer, + "CTR": ctr, + "ERR_BIT_1": er1, + "LINE_NOT_VISIBLE" : lnv, + "LDW": ldw, + "BIT_1": b1, + "ERR_BIT_2": er2, + "STEERING_ANGLE": steering_angle, + "ANGLE_ENABLED": b2, + "CHKSUM": csum + } + + elif car_fingerprint in GEN2: + bus = 1 + sig_name = "EPS_LKAS" values = { "LKAS_REQUEST": apply_steer, - "CTR": ctr, - "ERR_BIT_1": er1, - "LINE_NOT_VISIBLE" : lnv, - "LDW": ldw, - "BIT_1": b1, - "ERR_BIT_2": er2, - "STEERING_ANGLE": steering_angle, - "ANGLE_ENABLED": b2, - "CHKSUM": csum + "STEER_FEEL": 14000, } - return packer.make_can_msg("CAM_LKAS", 0, values) + return packer.make_can_msg(sig_name, bus, values) def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool): @@ -117,3 +130,20 @@ def create_button_cmd(packer, car_fingerprint, counter, button): } return packer.make_can_msg("CRZ_BTNS", 0, values) + +def create_acc_cmd(self, packer, CS, CC, hold, resume): + if self.CP.carFingerprint in GEN2: + values = CS.acc + msg_name = "ACC" + bus = 2 + + if (values["ACC_ENABLED"]): + if Params().get_bool("dp_e2e_conditional") or Params().get_bool("ExperimentalMode"): + values["ACCEL_CMD"] = (CC.actuators.accel * 240) + 2000 + values["HOLD"] = hold + values["RESUME"] = resume + else: + pass + + return packer.make_can_msg(msg_name, bus, values) + diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 8f993e265..57acb7f9b 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -11,20 +11,25 @@ # Steer torque limits - class CarControllerParams: - STEER_MAX = 800 # theoretical max_steer 2047 - STEER_DELTA_UP = 10 # torque increase per refresh - STEER_DELTA_DOWN = 25 # torque decrease per refresh - STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting - STEER_DRIVER_MULTIPLIER = 1 # weight driver torque - STEER_DRIVER_FACTOR = 1 # from dbc - STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor - STEER_STEP = 1 # 100 Hz - def __init__(self, CP): - pass - + self.STEER_STEP = 1 # 100 Hz + if CP.carFingerprint in GEN1: + self.STEER_MAX = 800 # theoretical max_steer 2047 + self.STEER_DELTA_UP = 10 # torque increase per refresh + self.STEER_DELTA_DOWN = 25 # torque decrease per refresh + self.STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 1 # weight driver torque + self.STEER_DRIVER_FACTOR = 1 # from dbc + self.STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + if CP.carFingerprint in GEN2: + self.STEER_MAX = 8000 + self.STEER_DELTA_UP = 45 # torque increase per refresh + self.STEER_DELTA_DOWN = 80 # torque decrease per refresh + self.STEER_DRIVER_ALLOWANCE = 1400 # allowed driver torque before start limiting + self.STEER_DRIVER_MULTIPLIER = 5 # weight driver torque + self.STEER_DRIVER_FACTOR = 1 # from dbc + self.STEER_ERROR_MAX = 3500 # max delta between torque cmd and torque motor class CAR: CX5 = "MAZDA CX-5" @@ -33,6 +38,13 @@ class CAR: MAZDA6 = "MAZDA 6" CX9_2021 = "MAZDA CX-9 2021" CX5_2022 = "MAZDA CX-5 2022" + MAZDA3_2019 = "MAZDA 3 2019" + CX_30 = "MAZDA CX-30" + CX_50 = "MAZDA CX-50" + CX_60 = "MAZDA CX-60" + CX_70 = "MAZDA CX-70" + CX_80 = "MAZDA CX-80" + CX_90 = "MAZDA CX-90" @dataclass @@ -48,6 +60,13 @@ class MazdaCarInfo(CarInfo): CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"), CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-23"), + CAR.MAZDA3_2019: MazdaCarInfo("Mazda 3 2019-23"), + CAR.CX_30: MazdaCarInfo("Mazda CX-30 2019-23"), + CAR.CX_50: MazdaCarInfo("Mazda CX-50 2022-23"), + CAR.CX_60: MazdaCarInfo("Mazda CX-60 unreleased"), + CAR.CX_70: MazdaCarInfo("Mazda CX-70 unreleased"), + CAR.CX_80: MazdaCarInfo("Mazda CX-80 unreleased"), + CAR.CX_90: MazdaCarInfo("Mazda CX-90 2023"), } @@ -317,7 +336,64 @@ class Buttons: b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], - } + }, + + CAR.MAZDA3_2019 : { + (Ecu.eps, 0x730, None): [ + b'BCKA-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BDGF-3216X-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-3216X-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX06-188K2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX08-188K2-L\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4W-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'B0N2-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B0N2-67XK2-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'BCKA-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-4300F-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BFVV-4300F-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'BDGF-67WK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BDGF-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'DFR5-67WK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX03-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4K-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, + + CAR.CX_50 : { + (Ecu.eps, 0x730, None): [ + b'VA40-3216Y-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX06-188K2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX08-188K2-L\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4W-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'VA45-67XK2-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'VA40-4300F-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'VA40-67WK2-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PX01-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX03-21PS1-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX4K-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, } @@ -328,7 +404,15 @@ class Buttons: CAR.MAZDA6: dbc_dict('mazda_2017', None), CAR.CX9_2021: dbc_dict('mazda_2017', None), CAR.CX5_2022: dbc_dict('mazda_2017', None), + CAR.MAZDA3_2019: dbc_dict('mazda_2019', None), + CAR.CX_30: dbc_dict('mazda_2019', None), + CAR.CX_50: dbc_dict('mazda_2019', None), + CAR.CX_60: dbc_dict('mazda_2019', None), + CAR.CX_70: dbc_dict('mazda_2019', None), + CAR.CX_80: dbc_dict('mazda_2019', None), + CAR.CX_90: dbc_dict('mazda_2019', None), } # Gen 1 hardware: same CAN messages and same camera GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022} +GEN2 = {CAR.MAZDA3_2019, CAR.CX_30, CAR.CX_50, CAR.CX_60, CAR.CX_70, CAR.CX_80, CAR.CX_90} diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index afd4624a8..1c26784af 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -48,6 +48,14 @@ KIA NIRO HYBRID 2ND GEN: [2.42, 2.5, 0.12] KIA NIRO EV 2ND GEN: [2.05, 2.5, 0.14] GENESIS GV80 2023: [2.5, 2.5, 0.1] +MAZDA 3 2019: [1.0, 3.0, .19] +MAZDA CX-30: [1.0, 3.0, .21] +MAZDA CX-50: [1.0, 3.0, .21] +MAZDA CX-60: [1.0, 3.0, .21] +MAZDA CX-70: [1.0, 3.0, .21] +MAZDA CX-80: [1.0, 3.0, .21] +MAZDA CX-90: [1.0, 3.0, .21] + # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 652be3165..b322a3237 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -24,6 +24,7 @@ from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR +from selfdrive.controls.lib.latcontrol_ti import LatControlTI from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -166,6 +167,8 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): self.LaC = LatControlTorque(self.CP, self.CI) elif self.params.get_bool("dp_lateral_lqr") and self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP, self.CI) + elif self.CP.lateralTuning.which() == 'ti': + self.LaC = LatControlTI(self.CP, self.CI) elif self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'pid': diff --git a/selfdrive/controls/lib/latcontrol_ti.py b/selfdrive/controls/lib/latcontrol_ti.py new file mode 100644 index 000000000..4f8265dc3 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_ti.py @@ -0,0 +1,92 @@ +import math + +from cereal import log +from common.numpy_fast import interp +from selfdrive.controls.lib.latcontrol import LatControl +from selfdrive.controls.lib.pid import PIDController +from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY + +# At higher speeds (25+mph) we can assume: +# Lateral acceleration achieved by a specific car correlates to +# torque applied to the steering rack. It does not correlate to +# wheel slip, or to speed. + +# This controller applies torque to achieve desired lateral +# accelerations. To compensate for the low speed effects we +# use a LOW_SPEED_FACTOR in the error. Additionally, there is +# friction in the steering wheel that needs to be overcome to +# move it at all, this is compensated for too. + +LOW_SPEED_X = [0, 10, 20, 30] +LOW_SPEED_Y = [15, 13, 10, 5] + + +class LatControlTI(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) + self.ti_params = CP.lateralTuning.ti + self.pid = PIDController(self.ti_params.kp, self.ti_params.ki, + k_f=self.ti_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) + self.torque_from_lateral_accel = CI.torque_from_lateral_accel() + self.use_steering_angle = self.ti_params.useSteeringAngle + self.steering_angle_deadzone_deg = self.ti_params.steeringAngleDeadzoneDeg + + def update_live_ti_params(self, latAccelFactor, latAccelOffset, friction): + self.ti_params.latAccelFactor = latAccelFactor + self.ti_params.latAccelOffset = latAccelOffset + self.ti_params.friction = friction + + def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): + pid_log = log.ControlsState.LateralTorqueState.new_message() + + if not active: + output_torque = 0.0 + pid_log.active = False + else: + steering_angle = CS.steeringAngleDeg - params.angleOffsetDeg + if self.use_steering_angle: + actual_curvature = -VM.calc_curvature(math.radians(steering_angle), CS.vEgo, params.roll) + curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) + else: + actual_curvature_vm = -VM.calc_curvature(math.radians(steering_angle), CS.vEgo, params.roll) + actual_curvature_llk = llk.angularVelocityCalibrated.value[2] / CS.vEgo + actual_curvature = interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk]) + curvature_deadzone = 0.0 + desired_lateral_accel = desired_curvature * CS.vEgo ** 2 + + # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature + # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2 + actual_lateral_accel = actual_curvature * CS.vEgo ** 2 + lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 + + low_speed_factor = interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 + setpoint = desired_lateral_accel + low_speed_factor * desired_curvature + measurement = actual_lateral_accel + low_speed_factor * actual_curvature + gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY + torque_from_setpoint = self.torque_from_lateral_accel(setpoint, self.ti_params, setpoint, + lateral_accel_deadzone, steering_angle, CS.vEgo, friction_compensation=False) + torque_from_measurement = self.torque_from_lateral_accel(measurement, self.ti_params, measurement, + lateral_accel_deadzone, steering_angle, CS.vEgo, friction_compensation=False) + pid_log.error = torque_from_setpoint - torque_from_measurement + ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.ti_params, + desired_lateral_accel - actual_lateral_accel, + lateral_accel_deadzone,0.0, 0.0, friction_compensation=True) + + freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 + output_torque = self.pid.update(pid_log.error, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) + + pid_log.active = True + pid_log.p = self.pid.p + pid_log.i = self.pid.i + pid_log.d = self.pid.d + pid_log.f = self.pid.f + pid_log.output = -output_torque + pid_log.actualLateralAccel = actual_lateral_accel + pid_log.desiredLateralAccel = desired_lateral_accel + pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited) + + # TODO left is positive in this convention + return -output_torque, 0.0, pid_log diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index dc278c0c4..bfd4d1ce4 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -35,6 +35,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { tr("Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off."), "../assets/offroad/icon_openpilot.png", }, + { + "OpenpilotLongitudinalControl", + tr("Override Stock ACC"), + "Use openpilot's longitudinal control instead of the car's built-in ACC. Enabling this will disable Automatic Emergency Braking (AEB).", + "../assets/icon_speed_limit.png", + }, { "ExperimentalMode", tr("Experimental Mode"), From 7f6eeac6d31f6c2961a190c528a68c22321e106b Mon Sep 17 00:00:00 2001 From: MoreTore Date: Fri, 19 May 2023 00:11:22 -0500 Subject: [PATCH 2/5] Bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 78a959916..6123e0e86 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 78a959916db1e1bd74e59dbb24d0bcc0e5dc9feb +Subproject commit 6123e0e864273b38ba2989ef8ca1aeb040c54ebb From db10f70ebcd0c814b37fea8f54c8ba69b93a4a34 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Fri, 19 May 2023 00:41:48 -0500 Subject: [PATCH 3/5] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 6123e0e86..5155ae43f 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 6123e0e864273b38ba2989ef8ca1aeb040c54ebb +Subproject commit 5155ae43ff4b3c2589d832ec7de574216b75662e From 18c5a2c3d5045006583db2a373dcce0794f03308 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Fri, 19 May 2023 00:42:01 -0500 Subject: [PATCH 4/5] bump cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 5c9d37184..d6241f454 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 5c9d3718481805e06561fb3c15f3c248d97a0dfb +Subproject commit d6241f4547af15a408596bc3963396e2d70ad18f From 4137cc97781855fdbf880eb77cc8913a0dc7cff4 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Fri, 19 May 2023 00:42:19 -0500 Subject: [PATCH 5/5] bump opendbc --- opendbc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc b/opendbc index b39935d6c..6483decb5 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit b39935d6ca4cebda458dd781c1e2cf8d3271dcab +Subproject commit 6483decb5a434946395d9057c1347984fdacbb63