From 64f8cbd3871a63febda282242fe9caf472eea472 Mon Sep 17 00:00:00 2001 From: Vasily Tarasov Date: Fri, 12 Oct 2018 17:37:13 -0700 Subject: [PATCH 1/2] GM: LKA dashboard icon support (#389) * GM: LKA dashboard icon support * Decrease camera keepalive interval * Use tuple for LKA icon status --- selfdrive/car/gm/carcontroller.py | 22 ++++++++++++++++++---- selfdrive/car/gm/gmcan.py | 10 ++++++++++ 2 files changed, 28 insertions(+), 4 deletions(-) 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): From ce13309d74f7b487e3d21088905a41816bd84db9 Mon Sep 17 00:00:00 2001 From: Andrew Frahn Date: Mon, 15 Oct 2018 01:30:18 +1100 Subject: [PATCH 2/2] Revert to using CLU15 message for Gear Selection for Compatability Reasons (#362) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * hyundai WIP * steer_driver_factor is 1 * removed unnecessary file * removed unnecessary code * Update carcontroller.py bug fix * safety tuning and fixed interface stiffness * better lateral tuning, some fixes * Fix set speed * added camera state reading, autoresume from stop, cancel on accel, hud alerts * WIP * Updated for Kia Sorento *WIP* * Cleanup * clean2 * Bug Fixes * pre-merge * Add all the cars! * Panda to auto-detect Camera Bus * Move Checksum Check * Final Sorento Tuning * Make CAN3 for Cam default * Update README.md * update panda, minor aesthetic updates * few other minor changes * added steer not allowed alert * bup panda version to force panda update * fixed camera alerts * Revert to using CLU15 for Gear Selection * Missing Defines Didn’t realise this was removed as well! * Change Gear Selection Definitions Add logic to use transmission message rather than cluster message for all gear selections UNLESS the car in question does not have the known good transmission messages * Revert Camera CAN Bus * self.candidate * fixed fingerpint, tested on Sorento * Update for Elantra on Hyundai-Dev Branch Latest Elantra values that correlate to progress Community branch. --- selfdrive/car/hyundai/carstate.py | 19 ++++++++++++++++++- selfdrive/car/hyundai/interface.py | 14 +++++++++----- 2 files changed, 27 insertions(+), 6 deletions(-) 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