diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index bda8ef22cd..f84ea3e322 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -60,6 +60,7 @@ class CarController(): self.apply_steer_last = 0 self.lka_icon_status_last = (False, False) self.steer_rate_limited = False + self.fcw_frames = 0 self.params = CarControllerParams() @@ -74,6 +75,10 @@ class CarController(): # Send CAN commands. can_sends = [] + # FCW: trigger FCWAlert for 100 frames (4 seconds) + if hud_alert == VisualAlert.fcw: + self.fcw_frames = 100 + ### STEER ### if (frame % P.STEER_STEP) == 0: @@ -122,7 +127,13 @@ class CarController(): # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) + # Send FCW if applicable + send_fcw = 0 + if self.fcw_frames > 0: + send_fcw = 0x3 + self.fcw_frames -= 1 + + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 23b521f55b..ba13df9219 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -60,7 +60,7 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, near_stop, at_f return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) -def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lead_car_in_sight): +def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lead_car_in_sight, fcw): # Not a bit shift, dash can round up based on low 4 bits. target_speed = int(target_speed_kph * 16) & 0xfff @@ -71,7 +71,8 @@ def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lea "ACCGapLevel" : 3 * acc_engaged, # 3 "far", 0 "inactive" "ACCCmdActive" : acc_engaged, "ACCAlwaysOne2" : 1, - "ACCLeadCar" : lead_car_in_sight + "ACCLeadCar" : lead_car_in_sight, + "FCWAlert": fcw } return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values)