diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 2018d73306..9b8e68fee4 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -44,11 +44,11 @@ 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() self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) + self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) def update(self, enabled, CS, frame, actuators, @@ -59,10 +59,6 @@ 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: lkas_enabled = enabled and not CS.out.steerWarning and CS.out.vEgo > P.MIN_STEER_SPEED @@ -98,18 +94,11 @@ class CarController(): at_full_stop = enabled and CS.out.standstill near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) - - at_full_stop = enabled and CS.out.standstill can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: - # Send FCW if applicable - send_fcw = 0 - if self.fcw_frames > 0: - send_fcw = 0x3 - self.fcw_frames -= 1 - + send_fcw = hud_alert == VisualAlert.fcw 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), @@ -120,7 +109,7 @@ class CarController(): if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) - can_sends.append(gmcan.create_adas_headlights_status(CanBus.OBSTACLE)) + can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 2eb3ff7cdb..7f45e7ea9e 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -5,8 +5,7 @@ from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \ - CruiseButtons, is_eps_status_ok, \ - STEER_THRESHOLD + CruiseButtons, STEER_THRESHOLD class CarState(CarStateBase): @@ -58,18 +57,17 @@ class CarState(CarStateBase): ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] - regen_pressed = False + ret.brakePressed = ret.brake > 1e-5 + # Regen braking is braking if self.car_fingerprint == CAR.VOLT: - regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) + ret.brakePressed = ret.brakePressed or bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) - # Regen braking is braking - ret.brakePressed = ret.brake > 1e-5 or regen_pressed ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] - ret.steerWarning = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) + ret.steerWarning = self.lkas_status not in [0, 1] return ret diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 5cd81ead11..9dd78e4df8 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -72,7 +72,7 @@ def create_acc_dashboard_command(packer, bus, acc_engaged, target_speed_kph, lea "ACCCmdActive" : acc_engaged, "ACCAlwaysOne2" : 1, "ACCLeadCar" : lead_car_in_sight, - "FCWAlert": fcw + "FCWAlert": 0x3 if fcw else 0 } return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values) @@ -104,8 +104,12 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx): dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] return make_can_msg(0x308, bytes(dat), bus) -def create_adas_headlights_status(bus): - return make_can_msg(0x310, b"\x42\x04", bus) +def create_adas_headlights_status(packer, bus): + values = { + "Always42": 0x42, + "Always4": 0x4, + } + return packer.make_can_msg("ASCMHeadlight", bus, values) def create_lka_icon_command(bus, active, critical, steer): if active and steer == 1: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index f63f8d7741..1328b11fe0 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -34,6 +34,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.444 # not optimized yet # Start with a baseline lateral tuning for all GM vehicles. Override tuning as needed in each model section below. + ret.minSteerSpeed = 7 * CV.MPH_TO_MS 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 @@ -156,6 +157,8 @@ class CarInterface(CarInterfaceBase): events.add(EventName.resumeRequired) if self.CS.pcm_acc_status == AccState.FAULTED: events.add(EventName.controlsFailed) + if ret.vEgo < self.CP.minSteerSpeed: + events.add(car.CarEvent.EventName.belowSteerSpeed) # handle button presses for b in ret.buttonEvents: diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index cb77b09705..efe696f8a6 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -32,9 +32,6 @@ class CanBus: CHASSIS = 2 SW_GMLAN = 3 -def is_eps_status_ok(eps_status, car_fingerprint): - return eps_status in [0, 1] - FINGERPRINTS = { # Astra BK MY17, ASCM unplugged CAR.HOLDEN_ASTRA: [{ diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5e03e2ab90..3423ee6519 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -d21758939420dba7d9dba842739914432a77e024 \ No newline at end of file +fe829cbe7ff161f098f680f3bfe3b8daeba2f220 \ No newline at end of file