diff --git a/common/params.cc b/common/params.cc index 9b4077347..990be48b2 100644 --- a/common/params.cc +++ b/common/params.cc @@ -180,10 +180,12 @@ std::unordered_map keys = { {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, {"OpenpilotEnabledToggle", PERSISTENT}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"OpenpilotLongitudinalControl", PERSISTENT}, {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"Passive", PERSISTENT}, {"PrimeType", PERSISTENT}, {"RecordFront", PERSISTENT}, + {"RecordRoad", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"ShouldDoUpdate", CLEAR_ON_MANAGER_START}, diff --git a/common/realtime.py b/common/realtime.py index 05e19e770..5bd3c31f7 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -93,3 +93,88 @@ class Ratekeeper: 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/assets/img_spinner_comma.png b/selfdrive/assets/img_spinner_comma.png index 16109557f..dd32c2d5d 100644 Binary files a/selfdrive/assets/img_spinner_comma.png and b/selfdrive/assets/img_spinner_comma.png differ diff --git a/selfdrive/assets/img_spinner_comma1.png b/selfdrive/assets/img_spinner_comma1.png new file mode 100644 index 000000000..16109557f Binary files /dev/null and b/selfdrive/assets/img_spinner_comma1.png differ diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 0ec33c1a2..073320fdc 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -380,6 +380,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector ps.setSpiChecksumErrorCount(health.spi_checksum_error_count); ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); + ps.setTorqueInterceptorDetected(health.torque_interceptor_detected_pkt); std::array cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index c90ae50ab..7019f0a83 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -76,6 +76,25 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, str]: return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc} +#alternate settings when using torque interceptor. May or may not be useful to some users/branches. +def apply_ti_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): + + # limits due to driver torque + driver_max_torque = LIMITS.TI_STEER_MAX + (LIMITS.TI_STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER + driver_min_torque = -LIMITS.TI_STEER_MAX + (-LIMITS.TI_STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER + max_steer_allowed = max(min(LIMITS.TI_STEER_MAX, driver_max_torque), 0) + min_steer_allowed = min(max(-LIMITS.TI_STEER_MAX, driver_min_torque), 0) + apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed) + + # slow rate if steer torque increases in magnitude + if apply_torque_last > 0: + apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.TI_STEER_DELTA_DOWN, -LIMITS.TI_STEER_DELTA_UP), + apply_torque_last + LIMITS.TI_STEER_DELTA_UP) + else: + apply_torque = clip(apply_torque, apply_torque_last - LIMITS.TI_STEER_DELTA_UP, + min(apply_torque_last + LIMITS.TI_STEER_DELTA_DOWN, LIMITS.TI_STEER_DELTA_UP)) + + return int(round(float(apply_torque))) def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 5c1234258..26fc61751 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -15,6 +15,7 @@ import cereal.messaging as messaging from openpilot.selfdrive.car import gen_empty_fingerprint FRAME_FINGERPRINT = 100 # 1s +from openpilot.selfdrive.global_ti import TI EventName = car.CarEvent.EventName @@ -23,7 +24,7 @@ def get_startup_event(car_recognized, controller_available, fw_seen): if is_comma_remote() and is_tested_branch(): event = EventName.startup else: - event = EventName.startupMaster + event = EventName.startup if not car_recognized: if fw_seen: @@ -189,6 +190,8 @@ def fingerprint(logcan, sendcan, num_pandas): cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached, fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, fingerprints=finger, fw_query_time=fw_query_time, error=True) + TI.saved_candidate = car_fingerprint + TI.saved_finger = finger return car_fingerprint, finger, vin, car_fw, source, exact_match @@ -205,5 +208,13 @@ def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): CP.carFw = car_fw CP.fingerprintSource = source CP.fuzzyFingerprint = not exact_match + + TI.saved_CarInterface = CarInterface return CarInterface(CP, CarController, CarState), CP + +def get_ti(): + print("get_ti, entering get_params") + car_params = TI.saved_CarInterface.get_params(TI.saved_candidate, TI.saved_finger, list(), False, False) + + return car_params diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index d11ba4ccf..435247f18 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -100,26 +100,7 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { } FW_QUERY_CONFIG = FwQueryConfig( - requests=[ - # CAN and CAN FD queries are combined. - # FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus - # TODO: properly handle auxiliary requests to separate queries and add back whitelists - Request( - [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], - [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], - # whitelist_ecus=[Ecu.engine], - ), - Request( - [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], - [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], - # whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire], - bus=0, - auxiliary=True, - ), - ], - extra_ecus=[ - (Ecu.shiftByWire, 0x732, None), - ], + requests=[], ) FW_VERSIONS = { diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 1b68b1dbc..130770e0d 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -19,7 +19,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter EventName = car.CarEvent.EventName -TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, bool], float] +TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, float, float, float, float, bool], float] MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS ACCEL_MAX = 2.0 @@ -131,7 +131,8 @@ class CarInterfaceBase(ABC): return self.get_steer_feedforward_default def torque_from_lateral_accel_linear(self, lateral_accel_value: float, torque_params: car.CarParams.LateralTorqueTuning, - lateral_accel_error: float, lateral_accel_deadzone: float, friction_compensation: bool) -> float: + lateral_accel_error: float, lateral_accel_deadzone: float, + steering_angle: float, vego: float, friction_compensation: bool) -> float: # The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction) friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) return (lateral_accel_value / float(torque_params.latAccelFactor)) + friction @@ -175,6 +176,10 @@ class CarInterfaceBase(ABC): ret.longitudinalActuatorDelayLowerBound = 0.15 ret.longitudinalActuatorDelayUpperBound = 0.15 ret.steerLimitTimer = 1.0 + + # No Torque Interceptor by default + ret.enableTorqueInterceptor = False + return ret @staticmethod diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 320ad19bb..d8ab5bde2 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,65 +1,114 @@ from cereal import car from opendbc.can.packer import CANPacker -from openpilot.selfdrive.car import apply_driver_steer_torque_limits +from openpilot.selfdrive.car import apply_driver_steer_torque_limits, apply_ti_steer_torque_limits from openpilot.selfdrive.car.mazda import mazdacan -from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons +from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons, GEN1 +from openpilot.common.realtime import ControlsTimer as Timer VisualAlert = car.CarControl.HUDControl.VisualAlert +LongCtrlState = car.CarControl.Actuators.LongControlState class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 + self.ti_apply_steer_last = 0 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) + self.cancel_delay = Timer(0.07) # 70ms delay to try to avoid a race condition with stock system + def update(self, CC, CS, now_nanos): can_sends = [] apply_steer = 0 + ti_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)) + if CS.CP.enableTorqueInterceptor: + if CS.ti_lkas_allowed: + ti_new_steer = int(round(CC.actuators.steer * self.params.TI_STEER_MAX)) + ti_apply_steer = apply_ti_steer_torque_limits(ti_new_steer, self.ti_apply_steer_last, + CS.out.steeringTorque, self.params) + + 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 + self.ti_apply_steer_last = ti_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 + steer_required = CS.out.steerFaultTemporary + can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) + + # send steering command if GEN1 + #The ti cannot be detected unless OP sends a can message to it because the ti only transmits when it + #sees the signature key in the designated address range. + can_sends.append(mazdacan.create_ti_steering_control(self.packer, self.CP.carFingerprint, + self.frame, ti_apply_steer)) + can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint, + self.frame, apply_steer, CS.cam_lkas)) + else: + 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, + 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 1f7846ca0..bdea32f7d 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -1,9 +1,11 @@ +import copy + from cereal import car from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1 +from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1, GEN2, TI_STATE, CAR, CarControllerParams class CarState(CarStateBase): def __init__(self, CP): @@ -17,8 +19,69 @@ class CarState(CarStateBase): self.low_speed_alert = False self.lkas_allowed_speed = False self.lkas_disabled = False + self.cam_lkas = 0 + self.params = CarControllerParams(CP) + + self.ti_ramp_down = False + self.ti_version = 1 + self.ti_state = TI_STATE.RUN + self.ti_violation = 0 + self.ti_error = 0 + self.ti_lkas_allowed = False + self.update = self.update_gen1 + if CP.carFingerprint in GEN1: + self.update = self.update_gen1 + if CP.carFingerprint in GEN2: + self.update = self.update_gen2 - def update(self, cp, cp_cam): + 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"]) + + return ret + # end GEN2 + + + def update_gen1(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() ret.wheelSpeeds = self.get_wheel_speeds( @@ -43,9 +106,23 @@ class CarState(CarStateBase): ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) - ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] - ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] - ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD + if self.CP.enableTorqueInterceptor: + ret.steeringTorque = cp_body.vl["TI_FEEDBACK"]["TI_TORQUE_SENSOR"] + + self.ti_version = cp_body.vl["TI_FEEDBACK"]["VERSION_NUMBER"] + self.ti_state = cp_body.vl["TI_FEEDBACK"]["STATE"] # DISCOVER = 0, OFF = 1, DRIVER_OVER = 2, RUN=3 + self.ti_violation = cp_body.vl["TI_FEEDBACK"]["VIOL"] # 0 = no violation + self.ti_error = cp_body.vl["TI_FEEDBACK"]["ERROR"] # 0 = no error + if self.ti_version > 1: + self.ti_ramp_down = (cp_body.vl["TI_FEEDBACK"]["RAMP_DOWN"] == 1) + + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.TI_STEER_THRESHOLD + self.ti_lkas_allowed = not self.ti_ramp_down and self.ti_state == TI_STATE.RUN + else: + ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"] + ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD + + ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"] ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"] ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"] @@ -82,16 +159,11 @@ class CarState(CarStateBase): ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1 ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS - if ret.cruiseState.enabled: - if not self.lkas_allowed_speed and self.acc_active_last: - self.low_speed_alert = True - else: - self.low_speed_alert = False - - # Check if LKAS is disabled due to lack of driver torque when all other states indicate - # it should be enabled (steer lockout). Don't warn until we actually get lkas active - # and lose it again, i.e, after initial lkas activation - ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked + # On if no driver torque the last 5 seconds + if self.CP.carFingerprint not in (CAR.CX5_2022, CAR.CX9_2021): + ret.steerFaultTemporary = cp.vl["STEER_RATE"]["HANDS_OFF_5_SECONDS"] == 1 + else: + ret.steerFaultTemporary = False self.acc_active_last = ret.cruiseState.enabled @@ -104,19 +176,39 @@ class CarState(CarStateBase): ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 return ret - + + @staticmethod + def get_ti_messages(CP): + messages = [] + if CP.enableTorqueInterceptor and CP.carFingerprint in GEN1: + messages += [ + ("TI_FEEDBACK", 50), + ] + elif CP.carFingerprint in GEN2: + messages += [ + ("EPS_FEEDBACK", 50), + ("EPS_FEEDBACK2", 50), + ("EPS_FEEDBACK3", 50), + ] + return messages + @staticmethod def get_can_parser(CP): - messages = [ - # sig_address, frequency - ("BLINK_INFO", 10), - ("STEER", 67), - ("STEER_RATE", 83), - ("STEER_TORQUE", 83), - ("WHEEL_SPEEDS", 100), - ] + messages = [] + + if CP.carFingerprint not in GEN2: + messages += [ + # sig_address, frequency + ("BLINK_INFO", 10), + ("STEER", 67), + ("STEER_RATE", 83), + ("STEER_TORQUE", 83), + ("WHEEL_SPEEDS", 100), + ] if CP.carFingerprint in GEN1: + # get real driver torque if we are using a torque interceptor + messages += CarState.get_ti_messages(CP) messages += [ ("ENGINE_DATA", 100), ("CRZ_CTRL", 50), @@ -129,6 +221,16 @@ class CarState(CarStateBase): ("GEAR", 20), ("BSM", 10), ] + + if CP.carFingerprint in GEN2: + messages += [ + ("BRAKE_PEDAL", 20), + ("CRUZE_STATE", 10), + ("BLINK_INFO", 10), + ("ACC", 50), + ("CRZ_BTNS", 10), + ("SYSTEM_SETTINGS", 10), + ] return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @@ -143,4 +245,18 @@ class CarState(CarStateBase): ("CAM_LKAS", 16), ] + if CP.carFingerprint in GEN2: + messages += [ + ("ENGINE_DATA", 100), + ("STEER_TORQUE", 100), + ("GEAR", 40), + ("WHEEL_SPEEDS", 100), + ("STEER", 100), + ("SPEED", 50), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) + + @staticmethod + def get_body_can_parser(CP): + return CANParser(DBC[CP.carFingerprint]["pt"], CarState.get_ti_messages(CP), 1) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 7ac93d9de..ddfbf4677 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,24 +1,59 @@ #!/usr/bin/env python3 from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS +from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS, GEN2, GEN1 from openpilot.selfdrive.car import get_safety_config -from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD +from openpilot.selfdrive.controls.lib.drive_helpers import get_friction +from openpilot.selfdrive.global_ti import TI ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): + + @staticmethod + def torque_from_lateral_accel_mazda(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 float(lateral_accel_value / lat_factor) + friction + + def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: + if self.CP.carFingerprint in GEN2: + return self.torque_from_lateral_accel_mazda + 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)] + ret.steerActuatorDelay = 0.1 + + if candidate in GEN2: + ret.experimentalLongitudinalAvailable = True + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda2019)] + ret.openpilotLongitudinalControl = experimental_long + 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.steerActuatorDelay = 0.3 + ret.radarUnavailable = True - ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) + ret.dashcamOnly = False - ret.steerActuatorDelay = 0.1 + ret.steerLimitTimer = 0.8 ret.tireStiffnessFactor = 0.70 # not optimized yet @@ -40,8 +75,23 @@ class CarInterface(CarInterfaceBase): ret.mass = 3443 * CV.LB_TO_KG ret.wheelbase = 2.83 ret.steerRatio = 15.5 + elif candidate in CAR.MAZDA3_2019: + ret.mass = 3000 * CV.LB_TO_KG + ret.wheelbase = 2.725 + ret.steerRatio = 17.0 + ret.lateralTuning.torque.latAngleFactor = .14 + elif candidate in (CAR.CX_30, CAR.CX_50): + ret.mass = 3375 * CV.LB_TO_KG + ret.wheelbase = 2.814 # Measured + ret.steerRatio = 15.5 + ret.lateralTuning.torque.latAngleFactor = .14 + elif candidate in (CAR.CX_60, CAR.CX_80, CAR.CX_70, CAR.CX_90): + ret.mass = 4217 * CV.LB_TO_KG + ret.wheelbase = 3.1 + ret.steerRatio = 17.6 + ret.lateralTuning.torque.latAngleFactor = .14 - 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 ret.centerToFront = ret.wheelbase * 0.41 @@ -50,15 +100,24 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): - ret = self.CS.update(self.cp, self.cp_cam) + if self.CP.carFingerprint in GEN1: + if self.CP.enableTorqueInterceptor and not TI.enabled: + TI.enabled = True + self.cp_body = self.CS.get_body_can_parser(self.CP) + self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback] + + ret = self.CS.update(self.cp, self.cp_cam, self.cp_body) # events events = self.create_common_events(ret) + if self.CP.carFingerprint in GEN1: + if self.CS.lkas_disabled: + events.add(EventName.lkasDisabled) + elif self.CS.low_speed_alert: + events.add(EventName.belowSteerSpeed) - if self.CS.lkas_disabled: - events.add(EventName.lkasDisabled) - elif self.CS.low_speed_alert: - events.add(EventName.belowSteerSpeed) + if not self.CS.acc_active_last and not self.CS.ti_lkas_allowed: + events.add(EventName.steerTempUnavailable) ret.events = events.to_msg() diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index e350c5587..12fe8be5b 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,51 +1,56 @@ from openpilot.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 - - values = {} 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" + values = { "LKAS_REQUEST": apply_steer, "CTR": ctr, @@ -58,8 +63,40 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): "ANGLE_ENABLED": b2, "CHKSUM": csum } + + elif car_fingerprint in GEN2: + bus = 1 + sig_name = "EPS_LKAS" + values = { + "LKAS_REQUEST": apply_steer, + "STEER_FEEL": 12000, + } + + return packer.make_can_msg(sig_name, bus, values) + +def create_ti_steering_control(packer, car_fingerprint, frame, apply_steer): + + key = 3294744160 + chksum = apply_steer + + if car_fingerprint in GEN1: + values = { + "LKAS_REQUEST" : apply_steer, + "CHKSUM" : chksum, + "KEY" : key + } + # TODO + # 1. Add new CAR values for MDARS Mazdas so that we can change the rate of the message. This will take some work. + # 2. Listen for reply's on both CAN buses if not MDARS version of + # Mazda (2021+ or m3 2019+) and warn the user if there is a bad connection + # but do not cause disengagment + + # Write to both buses for *future* redundancy, but we only check bus 1 for a response in carstate and safey_mazda.h for now. + # if (frame % 2 == 0): + # commands.append(packer.make_can_msg("CAM_LKAS2", 0, values)) + + return packer.make_can_msg("CAM_LKAS2", 1, values) - return packer.make_can_msg("CAM_LKAS", 0, values) def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool): @@ -126,3 +163,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("ExperimentalLongitudinalEnabled"): + 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 849cf229a..ff72f8761 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -9,23 +9,43 @@ from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, Ecu = car.CarParams.Ecu - # 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 = 600 # 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 = 40 # weight driver torque + self.STEER_DRIVER_FACTOR = 1 # from dbc + self.STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + + self.TI_STEER_MAX = 600 # theoretical max_steer 2047 + self.TI_STEER_DELTA_UP = 6 # torque increase per refresh + self.TI_STEER_DELTA_DOWN = 15 # torque decrease per refresh + self.TI_STEER_DRIVER_ALLOWANCE = 15 # allowed driver torque before start limiting + self.TI_STEER_DRIVER_MULTIPLIER = 40 # weight driver torque + self.TI_STEER_DRIVER_FACTOR = 1 # from dbc + self.TI_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 TI_STATE: + DISCOVER = 0 + OFF = 1 + DRIVER_OVER = 2 + RUN = 3 + + class CAR(StrEnum): CX5 = "MAZDA CX-5" CX9 = "MAZDA CX-9" @@ -33,6 +53,13 @@ class CAR(StrEnum): 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,13 +75,24 @@ CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { 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-24"), + CAR.MAZDA3_2019: MazdaCarInfo("Mazda 3 2019-24"), + CAR.CX_30: MazdaCarInfo("Mazda CX-30 2019-24"), + CAR.CX_50: MazdaCarInfo("Mazda CX-50 2022-24"), + 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"), } class LKAS_LIMITS: - STEER_THRESHOLD = 15 - DISABLE_SPEED = 45 # kph - ENABLE_SPEED = 52 # kph + STEER_THRESHOLD = 6 + DISABLE_SPEED = 0 # kph + ENABLE_SPEED = 0 # kph + TI_STEER_THRESHOLD = 6 + TI_DISABLE_SPEED = 0 # kph + TI_ENABLE_SPEED = 0 # kph + class Buttons: @@ -63,7 +101,7 @@ class Buttons: SET_MINUS = 2 RESUME = 3 CANCEL = 4 - + TURN_ON = 5 FW_QUERY_CONFIG = FwQueryConfig( requests=[ @@ -71,13 +109,17 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], ), - # Log responses on powertrain bus Request( - [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], - [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], - bus=0, - logging=True, + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.engine], ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + bus=0, + whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire], + ) ], ) @@ -277,31 +319,37 @@ FW_VERSIONS = { b'GBEF-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GBEF-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GFBC-3210X-A-00\000\000\000\000\000\000\000\000\000', + b'GFBC-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'PA34-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX4F-188K2-D\000\000\000\000\000\000\000\000\000\000\000\000', b'PYH7-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYH7-188K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PANX-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'K131-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-E\000\000\000\000\000\000\000\000\000\000\000\000', + b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ b'GBVH-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GBVH-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GDDM-437K2-A\000\000\000\000\000\000\000\000\000\000\000\000', + b'GDDM-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ b'B61L-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-P\000\000\000\000\000\000\000\000\000\000\000\000', + b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b'PA28-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYH3-21PS1-D\000\000\000\000\000\000\000\000\000\000\000\000', b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PA28-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, @@ -334,7 +382,98 @@ FW_VERSIONS = { 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'BDGF-3216X-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BCKA-3216X-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + 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'PA2J-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #b'PX06-188K2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + 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'BDTS-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #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'BFVV-4300F-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'BHCB-4300F-\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + 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-K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + 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'PAM6-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + #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_30 : { + (Ecu.eps, 0x730, None): [ + b'DFR5-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', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX06-188K2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'BDTS-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'B0N2-67XK2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'DEJW-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', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'BDGF-67WK2-K\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', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PX01-21PS1-E\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', + b'VA40-4300F-E\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', + ], + }, } @@ -345,7 +484,15 @@ DBC = { 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/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index a920bd45d..4c457951e 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -40,6 +40,8 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict: params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0) for fw in params['car_fw']] return params +from openpilot.selfdrive.global_ti import TI +from openpilot.selfdrive.car.mazda.values import GEN1 class TestCarInterfaces(unittest.TestCase): @@ -60,6 +62,13 @@ class TestCarInterfaces(unittest.TestCase): car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], experimental_long=args['experimental_long'], docs=False) + if car_name in GEN1: + TI.saved_candidate = car_name + TI.saved_CarInterface = CarInterface + TI.saved_finger = args['fingerprints'] + car_params = TI.saved_CarInterface.get_params(TI.saved_candidate, TI.saved_finger, list(), experimental_long=False, docs=False) + car_params.enableTorqueInterceptor = True + car_interface = CarInterface(car_params, CarController, CarState) assert car_params assert car_interface diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 29e616b10..0c6bb1b07 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -64,6 +64,14 @@ HYUNDAI AZERA HYBRID 6TH GEN: [1.8, 1.8, 0.1] KIA K8 HYBRID 1ST GEN: [2.5, 2.5, 0.1] HYUNDAI CUSTIN 1ST GEN: [2.5, 2.5, 0.1] +MAZDA 3 2019: [1.45, 3.0, .33] +MAZDA CX-30: [1.45, 3.0, .33] +MAZDA CX-50: [1.45, 3.0, .33] +MAZDA CX-60: [1.45, 3.0, .33] +MAZDA CX-70: [1.45, 3.0, .33] +MAZDA CX-80: [1.45, 3.0, .33] +MAZDA CX-90: [1.45, 3.0, .33] + # 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 19ce40793..0cca4d425 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -16,7 +16,7 @@ from panda import ALTERNATIVE_EXPERIENCE from openpilot.system.swaglog import cloudlog from openpilot.system.version import is_release_branch, get_short_branch from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp -from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can +from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can, get_ti from openpilot.selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED @@ -103,6 +103,8 @@ class Controls: if not self.disengage_on_accelerator: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + self.ti_ready = False + # read params self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") @@ -322,6 +324,15 @@ class Controls: if log.PandaState.FaultType.relayMalfunction in pandaState.faults: self.events.add(EventName.relayMalfunction) + if pandaState.torqueInterceptorDetected and not self.ti_ready: + self.ti_ready = True + self.CP.enableTorqueInterceptor = True + #Update CP based on torque_interceptor_ready + self.CP = get_ti() + # set alternative experiences since get_ti() reset it to default. + if not self.disengage_on_accelerator: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + # Handle HW and system malfunctions # Order is very intentional here. Be careful when modifying this. # All events here should at least have NO_ENTRY and SOFT_DISABLE. diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2c7763063..2a825aaf9 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -43,11 +43,12 @@ class LatControlTorque(LatControl): 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(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + 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(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + 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 @@ -63,13 +64,13 @@ class LatControlTorque(LatControl): 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.torque_params, setpoint, - lateral_accel_deadzone, friction_compensation=False) + lateral_accel_deadzone, steering_angle, CS.vEgo, friction_compensation=False) torque_from_measurement = self.torque_from_lateral_accel(measurement, self.torque_params, measurement, - lateral_accel_deadzone, friction_compensation=False) + 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.torque_params, desired_lateral_accel - actual_lateral_accel, - lateral_accel_deadzone, friction_compensation=True) + lateral_accel_deadzone,steering_angle, CS.vEgo, friction_compensation=True) freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, diff --git a/selfdrive/global_ti.py b/selfdrive/global_ti.py new file mode 100644 index 000000000..20c0055f1 --- /dev/null +++ b/selfdrive/global_ti.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 +from selfdrive.car import gen_empty_fingerprint + +class TI: + saved_candidate: str + saved_finger = gen_empty_fingerprint() + saved_CarInterface: object + enabled = False \ No newline at end of file diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 669de5eae..722925894 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -65,6 +65,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { tr("Upload data from the driver facing camera and help improve the driver monitoring algorithm."), "../assets/offroad/icon_monitoring.png", }, + { + "RecordBack", + tr("Record and Upload Road Cameras"), + tr("Upload data from the road cameras."), + "../assets/offroad/icon_monitoring.png", + }, { "IsMetric", tr("Use Metric System"), diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 1e128fb11..5605c98ae 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -32,8 +32,8 @@ FINALIZED = os.path.join(STAGING_ROOT, "finalized") OVERLAY_INIT = Path(os.path.join(BASEDIR, ".overlay_init")) -DAYS_NO_CONNECTIVITY_MAX = 14 # do not allow to engage after this many days -DAYS_NO_CONNECTIVITY_PROMPT = 10 # send an offroad prompt after this many days +DAYS_NO_CONNECTIVITY_MAX = 9999 # do not allow to engage after this many days +DAYS_NO_CONNECTIVITY_PROMPT = 9998 # send an offroad prompt after this many days class WaitTimeHelper: def __init__(self): diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index cfc06c28d..e2674960a 100644 --- a/system/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -58,12 +58,14 @@ public: const EncoderInfo main_road_encoder_info = { .publish_name = "roadEncodeData", .filename = "fcamera.hevc", + .record = Params().getBool("RecordBack"), INIT_ENCODE_FUNCTIONS(RoadEncode), }; const EncoderInfo main_wide_road_encoder_info = { .publish_name = "wideRoadEncodeData", .filename = "ecamera.hevc", + .record = Params().getBool("RecordBack"), INIT_ENCODE_FUNCTIONS(WideRoadEncode), };