From e7c6fb44e71e191ef740288a95725dd7e5147b98 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Sat, 26 Apr 2025 12:56:14 -0500 Subject: [PATCH] Mazda Interface --- selfdrive/car/mazda/interface.py | 109 +++++++++++++++++++++++++++---- 1 file changed, 97 insertions(+), 12 deletions(-) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 5cbe6d266..6765aa44b 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,15 +1,50 @@ #!/usr/bin/env python3 +from math import exp from cereal import car, custom +from panda import Panda 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, MazdaFlags, GEN1, GEN2 from openpilot.selfdrive.car import create_button_events, get_safety_config -from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.car.interfaces import CarInterfaceBase, LatControlInputs, FRICTION_THRESHOLD, TorqueFromLateralAccelCallbackType +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.drive_helpers import get_friction ButtonType = car.CarState.ButtonEvent.Type FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type EventName = car.CarEvent.EventName +NON_LINEAR_TORQUE_PARAMS = { + CAR.MAZDA_3_2019: (3.8818, 0.6873, 0.0999, 0.3605), +} + class CarInterface(CarInterfaceBase): + def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs, torque_params: car.CarParams.LateralTorqueTuning, lateral_accel_error: float, + lateral_accel_deadzone: float, friction_compensation: bool, gravity_adjusted: bool) -> float: + friction = get_friction(lateral_accel_error, lateral_accel_deadzone, FRICTION_THRESHOLD, torque_params, friction_compensation) + + def sig(val): + # https://timvieira.github.io/blog/post/2014/02/11/exp-normalize-trick + if val >= 0: + return 1 / (1 + exp(-val)) - 0.5 + else: + z = exp(val) + return z / (1 + z) - 0.5 + + # The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves + # An important thing to consider is that the slope at 0 should be > 0 (ideally >1) + # This has big effect on the stability about 0 (noise when going straight) + # ToDo: To generalize to other GMs, explore tanh function as the nonlinear + non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint) + assert non_linear_torque_params, "The params are not defined" + a, b, c, _ = non_linear_torque_params + steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c) + return float(steer_torque) + friction + + def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: + if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS: + return self.torque_from_lateral_accel_siglin + else: + return self.torque_from_lateral_accel_linear @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs, frogpilot_toggles): @@ -17,24 +52,68 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True - ret.steerActuatorDelay = 0.1 + ret.dashcamOnly = False + ret.openpilotLongitudinalControl = True + p = Params() + if p.get_bool("ManualTransmission"): + ret.flags |= MazdaFlags.MANUAL_TRANSMISSION.value + ret.transmissionType = car.CarParams.TransmissionType.manual + else: + ret.transmissionType = car.CarParams.TransmissionType.automatic + + if candidate in GEN1: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN1 + if p.get_bool("TorqueInterceptorEnabled"): # Torque Interceptor Installed + print("Torque Interceptor Installed") + ret.flags |= MazdaFlags.TORQUE_INTERCEPTOR.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_TORQUE_INTERCEPTOR + if p.get_bool("RadarInterceptorEnabled"): # Radar Interceptor Installed + ret.flags |= MazdaFlags.RADAR_INTERCEPTOR.value + ret.experimentalLongitudinalAvailable = True + ret.radarUnavailable = False + ret.startingState = True + ret.longitudinalTuning.kpBP = [0., 5., 30.] + ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7] + ret.longitudinalTuning.kiBP = [0., 5., 20., 30.] + ret.longitudinalTuning.kiV = [0.36, 0.23, 0.17, 0.1] + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_RADAR_INTERCEPTOR + + if p.get_bool("NoMRCC"): # No Mazda Radar Cruise Control; Missing CRZ_CTRL signal + ret.flags |= MazdaFlags.NO_MRCC.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_NO_MRCC + if p.get_bool("NoFSC"): # No Front Sensing Camera + ret.flags |= MazdaFlags.NO_FSC.value + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_NO_FSC + + ret.steerActuatorDelay = 0.1 + ret.enableBsm = True + + if candidate in GEN2: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_MAZDA_GEN2 + ret.experimentalLongitudinalAvailable = 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.steerActuatorDelay = 0.3 + ret.steerLimitTimer = 0.8 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if candidate not in (CAR.MAZDA_CX5_2022, ): + if candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_3_2019, CAR.MAZDA_CX_30, CAR.MAZDA_CX_50) and not ret.flags & MazdaFlags.TORQUE_INTERCEPTOR: ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS ret.centerToFront = ret.wheelbase * 0.41 - ret.enableBsm = True - return ret # returns a car.CarState def _update(self, c, frogpilot_toggles): - ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles) - + ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles) # TODO: add button types for inc and dec ret.buttonEvents = [ *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), @@ -44,10 +123,16 @@ class CarInterface(CarInterfaceBase): # events events = self.create_common_events(ret) - if self.CS.lkas_disabled: - events.add(EventName.lkasDisabled) - elif self.CS.low_speed_alert: - events.add(EventName.belowSteerSpeed) + if self.CP.flags & MazdaFlags.GEN1: + 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) + #if (not self.CS.ti_lkas_allowed) and (self.CP.flags & MazdaFlags.TORQUE_INTERCEPTOR): + # events.add(EventName.steerTempUnavailable) # torqueInterceptorTemporaryWarning ret.events = events.to_msg()