From 8699f2966612263d7a0b79bdc0302cd84d4efd2e Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 31 Jul 2024 13:03:33 -0700 Subject: [PATCH] Temporary GM longitudinal fixes --- cereal/car.capnp | 4 +- cereal/log.capnp | 2 +- common/params.cc | 1 + selfdrive/car/gm/interface.py | 62 +++++++--- selfdrive/controls/controlsd.py | 13 ++- selfdrive/controls/lib/longcontrol.py | 109 +++++++++++++++++- .../ui/qt/offroad/vehicle_settings.cc | 1 + .../ui/qt/offroad/vehicle_settings.h | 2 +- system/manager/manager.py | 1 + 9 files changed, 174 insertions(+), 21 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 9d3ddc3e8..d293db138 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -582,8 +582,8 @@ struct CarParams { kiBP @2 :List(Float32); kiV @3 :List(Float32); kf @6 :Float32; - deadzoneBPDEPRECATED @4 :List(Float32); - deadzoneVDEPRECATED @5 :List(Float32); + deadzoneBP @4 :List(Float32); + deadzoneV @5 :List(Float32); } struct LateralINDITuning { diff --git a/cereal/log.capnp b/cereal/log.capnp index 78f1d45cc..1af6e8c1e 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -704,6 +704,7 @@ struct ControlsState @0x97ff69c53601abf1 { personality @66 :LongitudinalPersonality; longControlState @30 :Car.CarControl.Actuators.LongControlState; + vPid @2 :Float32; vTargetLead @3 :Float32; vCruise @22 :Float32; # actual set speed vCruiseCluster @63 :Float32; # set speed to display in the UI @@ -873,7 +874,6 @@ struct ControlsState @0x97ff69c53601abf1 { canMonoTimesDEPRECATED @21 :List(UInt64); desiredCurvatureRateDEPRECATED @62 :Float32; canErrorCounterDEPRECATED @57 :UInt32; - vPidDEPRECATED @2 :Float32; } # All SI units and in device frame diff --git a/common/params.cc b/common/params.cc index 5779b6af9..93c7c35f7 100644 --- a/common/params.cc +++ b/common/params.cc @@ -389,6 +389,7 @@ std::unordered_map keys = { {"MTSCEnabled", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NavigationModels", PERSISTENT}, {"NextMapSpeedLimit", PERSISTENT}, + {"NewLongAPIGM", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES}, {"NNFF", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NNFFLite", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_CONTROLS}, {"NNFFModelName", PERSISTENT}, diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 63e7b3f74..aeaca3f82 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -98,6 +98,8 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params): + use_new_api = params.get_bool("NewLongAPIGM") + ret.carName = "gm" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)] ret.autoResumeSng = False @@ -111,7 +113,14 @@ class CarInterface(CarInterfaceBase): else: ret.transmissionType = TransmissionType.automatic - ret.longitudinalTuning.kiBP = [5., 35.] + if use_new_api: + ret.longitudinalTuning.kiBP = [5., 35.] + else: + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.15] + + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kiBP = [0.] if candidate in CAMERA_ACC_CAR: ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR @@ -123,9 +132,17 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 10 * CV.KPH_TO_MS # Tuning for experimental long - ret.longitudinalTuning.kiV = [2.0, 1.5] - ret.vEgoStopping = 0.1 - ret.vEgoStarting = 0.1 + if use_new_api: + ret.longitudinalTuning.kiV = [2.0, 1.5] + ret.vEgoStopping = 0.1 + ret.vEgoStarting = 0.1 + else: + ret.longitudinalTuning.kpV = [2.0, 1.5] + ret.longitudinalTuning.kiV = [0.72] + + ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling + ret.vEgoStopping = 0.25 + ret.vEgoStarting = 0.25 if experimental_long: ret.pcmCruise = False @@ -133,6 +150,8 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG elif candidate in SDGM_CAR: + if use_new_api: + ret.longitudinalTuning.kiV = [0., 0.] # TODO: tuning ret.experimentalLongitudinalAvailable = False ret.networkLocation = NetworkLocation.fwdCamera ret.pcmCruise = True @@ -151,7 +170,11 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 7 * CV.MPH_TO_MS # Tuning - ret.longitudinalTuning.kiV = [2.4, 1.5] + if use_new_api: + ret.longitudinalTuning.kiV = [2.4, 1.5] + else: + ret.longitudinalTuning.kpV = [2.4, 1.5] + ret.longitudinalTuning.kiV = [0.36] if ret.enableGasInterceptor: # Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits @@ -271,8 +294,14 @@ class CarInterface(CarInterfaceBase): ret.flags |= GMFlags.PEDAL_LONG.value ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_PEDAL_LONG # Note: Low speed, stop and go not tested. Should be fairly smooth on highway - ret.longitudinalTuning.kiBP = [0.0, 5., 35.] - ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5] + if use_new_api: + ret.longitudinalTuning.kiBP = [0.0, 5., 35.] + ret.longitudinalTuning.kiV = [0.0, 0.35, 0.5] + else: + ret.longitudinalTuning.kpBP = [5., 35.] + ret.longitudinalTuning.kpV = [0.35, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.0] + ret.longitudinalTuning.kiV = [0.1, 0.1] ret.longitudinalTuning.kf = 0.15 ret.stoppingDecelRate = 0.8 else: # Pedal used for SNG, ACC for longitudinal control otherwise @@ -290,14 +319,21 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = not disable_openpilot_long ret.pcmCruise = False + if use_new_api: + ret.longitudinalTuning.kiBP = [10.7, 10.8, 28.] + ret.longitudinalTuning.kiV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed + else: + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s + ret.longitudinalActuatorDelay = 1. # TODO: measure this + + ret.longitudinalTuning.kpBP = [10.7, 10.8, 28.] # 10.7 m/s == 24 mph + ret.longitudinalTuning.kpV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.1] + ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate) - ret.longitudinalActuatorDelayLowerBound = 1. # TODO: measure this - ret.longitudinalActuatorDelayUpperBound = 2. - - ret.longitudinalTuning.kiBP = [10.7, 10.8, 28.] - ret.longitudinalTuning.kiV = [0., 20., 20.] # set lower end to 0 since we can't drive below that speed - if candidate in CC_ONLY_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 150173b7b..6160111c0 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -209,6 +209,7 @@ class Controls: self.speed_limit_changed = False self.stopped_for_light = False self.update_toggles = False + self.use_old_long = self.CP.carName == "gm" and not self.params.get_bool("NewLongAPIGM") self.display_timer = 0 self.drive_distance = 0 @@ -628,12 +629,19 @@ class Controls: if not CC.latActive: self.LaC.reset() if not CC.longActive: - self.LoC.reset() + if self.use_old_long: + self.LoC.reset_old_long(v_pid=CS.vEgo) + else: + self.LoC.reset() if not self.joystick_mode: # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS, self.frogpilot_toggles) - actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits) + if self.use_old_long: + t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL + actuators.accel = self.LoC.update_old_long(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) + else: + actuators.accel = 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] @@ -838,6 +846,7 @@ class Controls: controlsState.state = self.state controlsState.engageable = not self.events.contains(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state + controlsState.vPid = float(self.LoC.v_pid) controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph) controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index d298c51ec..0dc7753bd 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,7 +1,7 @@ from cereal import car -from openpilot.common.numpy_fast import clip +from openpilot.common.numpy_fast import clip, interp 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 from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants @@ -46,6 +46,45 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, long_control_state = LongCtrlState.pid return long_control_state +def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_target, + v_target_1sec, brake_pressed, cruise_standstill): + accelerating = v_target_1sec > v_target + planned_stop = (v_target < CP.vEgoStopping and + v_target_1sec < CP.vEgoStopping and + not accelerating) + stay_stopped = (v_ego < CP.vEgoStopping and + (brake_pressed or cruise_standstill)) + stopping_condition = planned_stop or stay_stopped + + starting_condition = (v_target_1sec > CP.vEgoStarting and + accelerating and + not cruise_standstill and + not brake_pressed) + started_condition = v_ego > CP.vEgoStarting + + if not active: + long_control_state = LongCtrlState.off + + else: + if long_control_state in (LongCtrlState.off, LongCtrlState.pid): + long_control_state = LongCtrlState.pid + if stopping_condition: + long_control_state = LongCtrlState.stopping + + elif long_control_state == LongCtrlState.stopping: + if starting_condition and CP.startingState: + long_control_state = LongCtrlState.starting + elif starting_condition: + long_control_state = LongCtrlState.pid + + elif long_control_state == LongCtrlState.starting: + if stopping_condition: + long_control_state = LongCtrlState.stopping + elif started_condition: + long_control_state = LongCtrlState.pid + + return long_control_state + class LongControl: def __init__(self, CP): self.CP = CP @@ -53,6 +92,7 @@ class LongControl: self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) + self.v_pid = 0.0 self.last_output_accel = 0.0 def reset(self): @@ -88,3 +128,68 @@ class LongControl: self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel + + def reset_old_long(self, v_pid): + """Reset PID controller and change setpoint""" + self.pid.reset() + self.v_pid = v_pid + + def update_old_long(self, active, CS, long_plan, accel_limits, t_since_plan): + """Update longitudinal control. This updates the state machine and runs a PID loop""" + # Interp control trajectory + speeds = long_plan.speeds + if len(speeds) == CONTROL_N: + v_target_now = interp(t_since_plan, CONTROL_N_T_IDX, speeds) + a_target_now = interp(t_since_plan, CONTROL_N_T_IDX, long_plan.accels) + + v_target = interp(self.CP.longitudinalActuatorDelay + t_since_plan, CONTROL_N_T_IDX, speeds) + a_target = 2 * (v_target - v_target_now) / self.CP.longitudinalActuatorDelay - a_target_now + + v_target_1sec = interp(self.CP.longitudinalActuatorDelay + t_since_plan + 1.0, CONTROL_N_T_IDX, speeds) + else: + v_target = 0.0 + v_target_now = 0.0 + v_target_1sec = 0.0 + a_target = 0.0 + + self.pid.neg_limit = accel_limits[0] + self.pid.pos_limit = accel_limits[1] + + output_accel = self.last_output_accel + self.long_control_state = long_control_state_trans_old_long(self.CP, active, self.long_control_state, CS.vEgo, + v_target, v_target_1sec, CS.brakePressed, + CS.cruiseState.standstill) + + if self.long_control_state == LongCtrlState.off: + self.reset_old_long(CS.vEgo) + output_accel = 0. + + elif self.long_control_state == LongCtrlState.stopping: + if output_accel > self.CP.stopAccel: + output_accel = min(output_accel, 0.0) + output_accel -= self.CP.stoppingDecelRate * DT_CTRL + self.reset_old_long(CS.vEgo) + + elif self.long_control_state == LongCtrlState.starting: + output_accel = self.CP.startAccel + self.reset_old_long(CS.vEgo) + + elif self.long_control_state == LongCtrlState.pid: + self.v_pid = v_target_now + + # Toyota starts braking more when it thinks you want to stop + # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration + # TODO too complex, needs to be simplified and tested on toyotas + prevent_overshoot = not self.CP.stoppingControl and CS.vEgo < 1.5 and v_target_1sec < 0.7 and v_target_1sec < self.v_pid + deadzone = interp(CS.vEgo, self.CP.longitudinalTuning.deadzoneBP, self.CP.longitudinalTuning.deadzoneV) + freeze_integrator = prevent_overshoot + + error = self.v_pid - CS.vEgo + error_deadzone = apply_deadzone(error, deadzone) + output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, + feedforward=a_target, + freeze_integrator=freeze_integrator) + + self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) + + return self.last_output_accel diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc index 8903d72a1..e2b61fa5a 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -140,6 +140,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil std::vector> vehicleToggles { {"LongPitch", tr("Long Pitch Compensation"), tr("Smoothen out the gas and pedal controls."), ""}, {"VoltSNG", tr("2017 Volt SNG"), tr("Enable the 'Stop and Go' hack for 2017 Chevy Volts."), ""}, + {"NewLongAPIGM", tr("Use comma's New Longitudinal API"), tr("Use comma's new longitudinal controls that have shown great improvement with acceleration and braking, but has a few issues on some GM vehicles."), ""}, {"CrosstrekTorque", tr("Subaru Crosstrek Torque Increase"), tr("Increases the maximum allowed torque for the Subaru Crosstrek."), ""}, diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h index 57f61d2b3..454b685cd 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h @@ -31,7 +31,7 @@ private: QMap carModels; - std::set gmKeys = {"LongPitch", "VoltSNG"}; + std::set gmKeys = {"LongPitch", "NewLongAPIGM", "VoltSNG"}; std::set subaruKeys = {"CrosstrekTorque"}; std::set toyotaKeys = {"ClusterOffset", "SNGHack", "ToyotaDoors", "ToyotaTune"}; diff --git a/system/manager/manager.py b/system/manager/manager.py index 2b5aa4e49..d5f80de45 100644 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -213,6 +213,7 @@ def manager_init() -> None: ("MTSCCurvatureCheck", "0"), ("MTSCEnabled", "1"), ("NavigationModels", ""), + ("NewLongAPIGM", "1"), ("NNFF", "1"), ("NNFFLite", "1"), ("NoLogging", "0"),