Release 260111

opendbc_repo/opendbc/car/byd/interface.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python3
+import os
+from math import fabs, exp
+
+from opendbc.car import get_safety_config, get_friction, structs
+from opendbc.car.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+from opendbc.car.byd.carcontroller import CarController
+from opendbc.car.byd.carstate import CarState
+from opendbc.car.byd.radar_interface import RadarInterface
+from opendbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel
+from opendbc.car.byd.values import CAR, CanBus, BydFlags, BydSafetyFlags, MPC_ACC_CAR, TORQUE_LAT_CAR, EXP_LONG_CAR, PT_RADAR_CAR, RADAR_CAR,\
+                PLATFORM_TANG_DMI, PLATFORM_SONG_PLUS_DMI, PLATFORM_QIN_PLUS_DMI, PLATFORM_YUAN_PLUS_DMI_ATTO3, PLATFORM_SEAL, PLATFORM_TENGSHI, PLATFORM_HAN_DMI
+
+from opendbc.car.byd.tuning import Tuning
+
+ButtonType = structs.CarState.ButtonEvent.Type
+GearShifter = structs.CarState.GearShifter
+TransmissionType = structs.CarParams.TransmissionType
+NetworkLocation = structs.CarParams.NetworkLocation
+
+class CarInterface(CarInterfaceBase):
+  CarState = CarState
+  CarController = CarController
+  RadarInterface = RadarInterface
+
+  def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs, torque_params: structs.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)
+    #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 = Tuning.LAT_SIGLIN_TABLE
+    steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c)
+    return float(steer_torque / torque_params.latAccelFactor + friction)
+
+  def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
+    if Params().get_bool("BydLatUseSiglin"):
+      return self.torque_from_lateral_accel_siglin
+    else:
+      return self.torque_from_lateral_accel_linear
+
+  @staticmethod
+  def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
+    ret.brand = "byd"
+
+    if Params().get_bool("UseRedPanda"):
+      ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput),get_safety_config(structs.CarParams.SafetyModel.byd)]
+    else:
+      ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.byd)]
+
+    ret.dashcamOnly = False
+
+    radar_car = candidate in (PT_RADAR_CAR | RADAR_CAR)
+    ret.radarUnavailable = not radar_car
+
+    ret.enableBsm = 0x418 in fingerprint[CanBus.ESC]
+    ret.transmissionType = TransmissionType.direct
+
+    valid_safety_index = 1 if Params().get_bool("UseRedPanda") else 0
+    if candidate in PLATFORM_TANG_DMI:
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.TANG_DMI.value
+
+    elif candidate in PLATFORM_HAN_DMI:
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.HAN_DMI.value
+
+    elif candidate in PLATFORM_SONG_PLUS_DMI:
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.SONG_PLUS_DMI.value
+
+    elif candidate in PLATFORM_QIN_PLUS_DMI:
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.QIN_PLUS_DMI.value
+
+    elif candidate in PLATFORM_YUAN_PLUS_DMI_ATTO3:
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.YUAN_PLUS_DMI_ATTO3.value
+
+    elif candidate in PLATFORM_TENGSHI:
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.TENGSHI.value
+
+    elif candidate in PLATFORM_SEAL:
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.SEAL.value
+
+    else: #汉dm，唐dm，宋Pro
+      ret.safetyConfigs[valid_safety_index].safetyParam |= BydSafetyFlags.HAN_TANG_DMEV.value
+
+    if candidate in MPC_ACC_CAR:
+      ret.networkLocation = NetworkLocation.fwdCamera
+
+    if candidate in TORQUE_LAT_CAR:
+      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+    else:
+      ret.steerControlType = structs.CarParams.SteerControlType.angle
+      ret.flags |= BydFlags.ANGLE_CONTROL.value
+
+    ret.openpilotLongitudinalControl = candidate in EXP_LONG_CAR
+
+    ret.longitudinalTuning.kpBP, ret.longitudinalTuning.kiBP = [[0.],  [0.]]
+    ret.longitudinalTuning.kpV, ret.longitudinalTuning.kiV   = [[1.],  [0.]]
+    ret.longitudinalTuning.kf = 1.0
+
+    #car specified settings
+    if candidate in TORQUE_LAT_CAR:
+      ret.minEnableSpeed = -1.
+      ret.minSteerSpeed = 0.1 * CV.KPH_TO_MS
+      ret.autoResumeSng = True
+      ret.startingState = True
+      ret.startAccel = 0.8
+      ret.stopAccel = -0.5
+      ret.vEgoStarting = 0.1
+      ret.vEgoStopping = 0.1
+      ret.longitudinalActuatorDelay = 0.5
+      ret.steerActuatorDelay = 0.2  # 转向执行器延迟，测量是0.4，但是在torqued.py里55行会加上0.2
+      ret.steerLimitTimer = 0.6
+    elif candidate in (PLATFORM_SEAL | PLATFORM_TENGSHI):
+      ret.minEnableSpeed = -1.
+      ret.minSteerSpeed = 0.1 * CV.KPH_TO_MS
+      ret.autoResumeSng = True
+      ret.startingState = True
+      ret.startAccel = 0.8
+      ret.stopAccel = -0.5
+      ret.vEgoStarting = 0.1
+      ret.vEgoStopping = 0.1
+      ret.longitudinalActuatorDelay = 0.5
+      ret.steerActuatorDelay = 0.1  # 转向执行器延迟，测量是0.4，但是在torqued.py里55行会加上0.2
+      ret.steerLimitTimer = 1.0
+    else:
+      ret.dashcamOnly = True
+
+
+    return ret