diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index be0ca64ccb..71277cf2a7 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 47b0d8b38d..910cb1d956 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):