#!/usr/bin/env python3 import os from cereal import car from math import fabs, exp from panda import Panda from openpilot.common.basedir import BASEDIR from openpilot.selfdrive.car import get_safety_config, get_friction from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel TransmissionType = car.CarParams.TransmissionType NetworkLocation = car.CarParams.NetworkLocation NON_LINEAR_TORQUE_PARAMS = { CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772], CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122] } NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/neural_ff_weights.json') class CarInterface(CarInterfaceBase): @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. @staticmethod def get_steer_feedforward_volt(desired_angle, v_ego): desired_angle *= 0.02904609 sigmoid = desired_angle / (1 + fabs(desired_angle)) return 0.10006696 * sigmoid * (v_ego + 3.12485927) def get_steer_feedforward_function(self): if self.CP.carFingerprint == CAR.CHEVROLET_VOLT: return self.get_steer_feedforward_volt else: return CarInterfaceBase.get_steer_feedforward_default 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_neural(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) inputs = list(latcontrol_inputs) if gravity_adjusted: inputs[0] += inputs[1] return float(self.neural_ff_model.predict(inputs)) + friction def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_EUV: self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint) return self.torque_from_lateral_accel_neural elif 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): ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN] if candidate in EV_CAR: ret.transmissionType = TransmissionType.direct else: ret.transmissionType = TransmissionType.automatic ret.longitudinalTuning.kiBP = [5., 35.] if candidate in CAMERA_ACC_CAR: ret.experimentalLongitudinalAvailable = True ret.networkLocation = NetworkLocation.fwdCamera ret.radarUnavailable = True # no radar ret.pcmCruise = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM ret.minEnableSpeed = 5 * CV.KPH_TO_MS ret.minSteerSpeed = 10 * CV.KPH_TO_MS # Tuning for experimental long ret.longitudinalTuning.kiV = [2.0, 1.5] ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 if experimental_long: ret.pcmCruise = False ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG else: # ASCM, OBD-II harness ret.openpilotLongitudinalControl = True ret.networkLocation = NetworkLocation.gateway ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs ret.pcmCruise = False # stock non-adaptive cruise control is kept off # supports stop and go, but initial engage must (conservatively) be above 18mph ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.minSteerSpeed = 7 * CV.MPH_TO_MS # Tuning ret.longitudinalTuning.kiV = [2.4, 1.5] # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/car/tests/routes.py, we can remove it from this list. ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.CHEVROLET_MALIBU, CAR.BUICK_REGAL} or \ (ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable) # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]] ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking if candidate == CAR.CHEVROLET_VOLT: ret.lateralTuning.pid.kpBP = [0., 40.] ret.lateralTuning.pid.kpV = [0., 0.17] ret.lateralTuning.pid.kiBP = [0.] ret.lateralTuning.pid.kiV = [0.] ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt() ret.steerActuatorDelay = 0.2 elif candidate == CAR.GMC_ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.BUICK_LACROSSE: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.CADILLAC_ESCALADE: ret.minEnableSpeed = -1. # engage speed is decided by pcm CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate in (CAR.CADILLAC_ESCALADE_ESV, CAR.CADILLAC_ESCALADE_ESV_2019): ret.minEnableSpeed = -1. # engage speed is decided by pcm if candidate == CAR.CADILLAC_ESCALADE_ESV: ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]] ret.lateralTuning.pid.kf = 0.000045 else: ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.CHEVROLET_BOLT_EUV: ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.CHEVROLET_SILVERADO: # On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop # with foot on brake to allow engagement, but this platform only has that check in the camera. # TODO: check if this is split by EV/ICE with more platforms in the future if ret.openpilotLongitudinalControl: ret.minEnableSpeed = -1. CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.CHEVROLET_EQUINOX: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.CHEVROLET_TRAILBLAZER: ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) return ret