diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index be0ca64cc..71277cf2a 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -26,7 +26,11 @@ class CarControllerParams(): self.STEER_DRIVER_FACTOR = 100 # from dbc self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop - self.ADAS_KEEPALIVE_STEP = 10 + # Takes case of "Service Adaptive Cruise" and "Service Front Camera" + # dashboard messages. + self.ADAS_KEEPALIVE_STEP = 100 + self.CAMERA_KEEPALIVE_STEP = 100 + # pedal lookups, only for Volt MAX_GAS = 3072 # Only a safety limit ZERO_GAS = 2048 @@ -59,12 +63,11 @@ class CarController(object): self.pedal_steady = 0. self.start_time = sec_since_boot() self.chime = 0 - self.lkas_active = False - self.inhibit_steer_for = 0 self.steer_idx = 0 self.apply_steer_last = 0 self.car_fingerprint = car_fingerprint self.allow_controls = allow_controls + self.lka_icon_status_last = (False, False) # Setup detection helper. Routes commands to # an appropriate CAN bus number. @@ -158,10 +161,21 @@ class CarController(object): can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx)) can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx)) - # Send ADAS keepalive, 10hz if frame % P.ADAS_KEEPALIVE_STEP == 0: can_sends += gmcan.create_adas_keepalive(canbus.powertrain) + # Show green icon when LKA torque is applied, and + # alarming orange icon when approaching torque limit. + # If not sent again, LKA icon disappears in about 5 seconds. + # Conveniently, sending camera message periodically also works as a keepalive. + lka_active = CS.lkas_status == 1 + lka_critical = lka_active and abs(actuators.steer) > 0.9 + lka_icon_status = (lka_active, lka_critical) + if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ + or lka_icon_status != self.lka_icon_status_last: + can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical)) + self.lka_icon_status_last = lka_icon_status + # Send chimes if self.chime != chime: duration = 0x3c diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 47b0d8b38..910cb1d95 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -134,6 +134,16 @@ def create_chime_command(bus, chime_type, duration, repeat_cnt): dat = [chime_type, duration, repeat_cnt, 0xff, 0] return [0x10400060, 0, "".join(map(chr, dat)), bus] +def create_lka_icon_command(bus, active, critical): + if active: + if critical: + dat = "\x40\xc0\x14" + else: + dat = "\x40\x40\x18" + else: + dat = "\x00\x00\x00" + return [0x104c006c, 0, dat, bus] + # TODO: WIP ''' def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, at_full_stop): diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 471464495..451183802 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -44,6 +44,11 @@ def get_can_parser(CP): ("CF_Clu_AmpInfo", "CLU11", 0), ("CF_Clu_AliveCnt1", "CLU11", 0), + ("CF_Clu_InhibitD", "CLU15", 0), + ("CF_Clu_InhibitP", "CLU15", 0), + ("CF_Clu_InhibitN", "CLU15", 0), + ("CF_Clu_InhibitR", "CLU15", 0), + ("CF_Lvr_Gear","LVR12",0), ("ACCEnable", "TCS13", 0), @@ -204,7 +209,7 @@ class CarState(object): self.pedal_gas = cp.vl["EMS12"]['TPS'] self.car_gas = cp.vl["EMS12"]['TPS'] - # Gear Selecton - This should be compatible with all Kia/Hyundai with Auto's + # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with gear = cp.vl["LVR12"]["CF_Lvr_Gear"] if gear == 5: self.gear_shifter = "drive" @@ -217,6 +222,18 @@ class CarState(object): else: self.gear_shifter = "unknown" + # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method. + if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1: + self.gear_shifter_cluster = "drive" + elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1: + self.gear_shifter_cluster = "neutral" + elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1: + self.gear_shifter_cluster = "park" + elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1: + self.gear_shifter_cluster = "reverse" + else: + self.gear_shifter_cluster = "unknown" + # save the entire LKAS11 and CLU11 self.lkas11 = cp_cam.vl["LKAS11"] self.clu11 = cp.vl["CLU11"] diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5f9d75c94..96174ef09 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -95,14 +95,15 @@ class CarInterface(object): ret.steerKpV, ret.steerKiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. elif candidate == CAR.ELANTRA: - ret.steerKf = 0.00004 + ret.steerKf = 0.00006 ret.steerRateCost = 0.5 ret.mass = 1275 + std_cargo ret.wheelbase = 2.7 - ret.steerRatio = 16.9 + ret.steerRatio = 13.73 #Spec + tire_stiffness_factor = 0.385 ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.20], [0.01]] - ret.minSteerSpeed = 35 * CV.MPH_TO_MS + ret.steerKpV, ret.steerKiV = [[0.25], [0.05]] + ret.minSteerSpeed = 32 * CV.MPH_TO_MS elif candidate == CAR.GENESIS: ret.steerKf = 0.00005 ret.steerRateCost = 0.5 @@ -190,7 +191,10 @@ class CarInterface(object): ret.wheelSpeeds.rr = self.CS.v_wheel_rr # gear shifter - ret.gearShifter = self.CS.gear_shifter + if self.CP.carFingerprint == CAR.ELANTRA: + ret.gearShifter = self.CS.gear_shifter_cluster + else: + ret.gearShifter = self.CS.gear_shifter # gas pedal ret.gas = self.CS.car_gas