From e5e212c8ca25d0afb3b486b4ea1362c1a0f06b2c Mon Sep 17 00:00:00 2001 From: Eric Brown Date: Fri, 28 Feb 2025 18:23:13 -0700 Subject: [PATCH] CC long tweaks --- selfdrive/car/car_specific.py | 6 +++++- selfdrive/car/card.py | 10 ++++++++-- selfdrive/car/cruise.py | 9 ++++++--- selfdrive/controls/controlsd.py | 2 ++ 4 files changed, 21 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index c5edb484a..3b4ce2b3a 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -1,6 +1,7 @@ from cereal import car, log import cereal.messaging as messaging from opendbc.car import DT_CTRL, structs +from opendbc.car.gm.values import GMFlags from opendbc.car.interfaces import MAX_CTRL_SPEED from opendbc.car.volkswagen.values import CarControllerParams as VWCarControllerParams @@ -103,11 +104,14 @@ class CarSpecificEvents: if CS.vEgo < self.CP.minEnableSpeed and not (CS.standstill and CS.brake >= 20 and self.CP.networkLocation == NetworkLocation.fwdCamera): events.add(EventName.belowEngageSpeed) - if CS.cruiseState.standstill: + if CS.cruiseState.standstill and not self.CP.autoResumeSng: events.add(EventName.resumeRequired) if CS.vEgo < self.CP.minSteerSpeed: events.add(EventName.belowSteerSpeed) + if (self.CP.flags & GMFlags.CC_LONG) and CS.vEgo < self.CP.minEnableSpeed and CS.cruiseState.enabled: + events.add(EventName.speedTooLow) + elif self.CP.brand == 'volkswagen': events = self.create_common_events(CS, CS_prev, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=self.CP.pcmCruise) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 502daaa01..9ed0767f0 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -11,7 +11,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper from openpilot.common.swaglog import cloudlog, ForwardingHandler -from opendbc.car import DT_CTRL, structs +from opendbc.car import DT_CTRL, structs, ButtonType from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable from opendbc.car.carlog import carlog from opendbc.car.fw_versions import ObdCallback @@ -73,6 +73,7 @@ class Car: self.CC_prev = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() self.initialized_prev = False + self.resume_prev_button = False self.last_actuators_output = structs.CarControl.Actuators() @@ -185,12 +186,17 @@ class Car: self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric) if self.sm['carControl'].enabled and not self.CC_prev.enabled: # Use CarState w/ buttons from the step selfdrived enables on - self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode) + self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.resume_prev_button) # TODO: mirror the carState.cruiseState struct? CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) + if any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents): + self.resume_prev_button = True + elif any(be.type in (ButtonType.decelCruise, ButtonType.setCruise) for be in CS.buttonEvents): + self.resume_prev_button = False + return CS, RD def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None): diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index b825808ac..e6b921744 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -2,6 +2,7 @@ import math import numpy as np from cereal import car +from opendbc.car.gm.values import CC_ONLY_CAR, GMFlags from openpilot.common.conversions import Conversions as CV @@ -31,6 +32,7 @@ CRUISE_INTERVAL_SIGN = { class VCruiseHelper: def __init__(self, CP): self.CP = CP + self.gm_cc_only = self.CP.carFingerprint in CC_ONLY_CAR and self.CP.flags & GMFlags.CC_LONG.value self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET self.v_cruise_kph_last = 0 @@ -123,14 +125,15 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, resume_prev_button: bool) -> None: # initializing is handled by the PCM - if self.CP.pcmCruise: + if self.CP.pcmCruise and not self.gm_cc_only: return initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL - if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized: + if (any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) + and self.v_cruise_initialized or (self.gm_cc_only and resume_prev_button)): self.v_cruise_kph = self.v_cruise_kph_last else: self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5ecff1ccd..6ff1df92c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -113,6 +113,8 @@ class Controls: pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)) + if len(long_plan.speeds): + actuators.speed = long_plan.speeds[-1] # Steering PID loop and lateral MPC # Reset desired curvature to current to avoid violating the limits on engage new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature