diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index 151a8479a..9513f0ba9 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -1,6 +1,7 @@ from cereal import car, log, custom 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 openpilot.selfdrive.selfdrived.events import Events diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 8448af56d..960ff63c2 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -12,7 +12,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 @@ -81,6 +81,7 @@ class Car: self.CS_prev = car.CarState.new_message() self.CS_SP_prev = custom.CarStateSP.new_message() self.initialized_prev = False + self.resume_prev_button = False self.last_actuators_output = structs.CarControl.Actuators() @@ -218,12 +219,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.dynamic_experimental_control) + self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.dynamic_experimental_control, 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, CS_SP, RD def state_publish(self, CS: car.CarState, CS_SP: custom.CarStateSP, RD: structs.RadarDataT | None): diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 01ef2d7c9..4906bb84c 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 from openpilot.sunnypilot.selfdrive.car.cruise_ext import VCruiseHelperSP @@ -33,6 +34,7 @@ class VCruiseHelper(VCruiseHelperSP): def __init__(self, CP): VCruiseHelperSP.__init__(self) 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 @@ -125,15 +127,16 @@ class VCruiseHelper(VCruiseHelperSP): 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, dynamic_experimental_control: bool) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, dynamic_experimental_control, 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_experimental_mode = experimental_mode and not dynamic_experimental_control initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_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 9ef985fcc..f2123fc27 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -129,6 +129,9 @@ class Controls(ControlsExt): 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 diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 51eb694b4..37848e151 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -63,3 +63,24 @@ def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t): psi_target = np.interp(action_t, t_idxs, yaws) psi_rate = yaw_rates[0] return curv_from_psis(psi_target, psi_rate, vego, action_t) + +def interp(x, xp, fp): + N = len(xp) + def get_interp(xv): + hi = 0 + while hi < N and xv > xp[hi]: + hi += 1 + low = hi - 1 + return fp[-1] if hi == N and xv > xp[low] else ( + fp[0] if hi == 0 else + (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) + return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x) + +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < -deadzone: + error += deadzone + else: + error = 0. + return error \ No newline at end of file diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6d4f92246..cd1da42da 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,7 +1,7 @@ import numpy as np from cereal import car from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone, interp from openpilot.common.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants @@ -80,8 +80,10 @@ class LongControl: self.reset() else: # LongCtrlState.pid + deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBPDEPRECATED, self.CP.longitudinalTuning.deadzoneVDEPRECATED) error = a_target - CS.aEgo - output_accel = self.pid.update(error, speed=CS.vEgo, + error_deadzone = apply_deadzone(error, deadzone) + output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=a_target) self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])