From f5a966c56498ea99585b81e3685c4d01eb6ad3d6 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sun, 30 Jun 2024 17:38:09 -0700 Subject: [PATCH] Temporary HKG longitudinal fixes --- cereal/car.capnp | 4 +- cereal/log.capnp | 2 +- selfdrive/car/hyundai/interface.py | 8 ++ selfdrive/controls/controlsd.py | 13 ++- selfdrive/controls/lib/longcontrol.py | 109 +++++++++++++++++++++++++- 5 files changed, 129 insertions(+), 7 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 34c021d18..4d822fd99 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -579,8 +579,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/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index b39f24e90..23d6607ea 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -82,8 +82,16 @@ class CarInterface(CarInterfaceBase): # *** longitudinal control *** if candidate in CANFD_CAR: + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] + ret.longitudinalTuning.kpV = [0.1] + ret.longitudinalTuning.kiV = [0.0] ret.experimentalLongitudinalAvailable = candidate not in (CANFD_UNSUPPORTED_LONGITUDINAL_CAR | CANFD_RADAR_SCC_CAR) else: + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] + ret.longitudinalTuning.kpV = [0.5] + ret.longitudinalTuning.kiV = [0.0] ret.experimentalLongitudinalAvailable = candidate not in (UNSUPPORTED_LONGITUDINAL_CAR | CAMERA_SCC_CAR) ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable ret.pcmCruise = not ret.openpilotLongitudinalControl diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f63fb9919..877949917 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -205,6 +205,7 @@ class Controls: self.speed_check = False self.speed_limit_changed = False self.update_toggles = False + self.use_old_long = self.CP.carName == "hyundai" self.vCruise69_alert_played = False self.display_timer = 0 @@ -625,12 +626,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] @@ -835,6 +843,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 50bfcad47..ab5ae68bc 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 @@ -42,6 +42,45 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, 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 @@ -49,6 +88,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): @@ -84,3 +124,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