Mazda Interface

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()