diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index e5f8c03926..1256e68938 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -183,17 +183,19 @@ class CarController(): # wind brake from air resistance decel at high speed wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) - - # All of this is only relevant for HONDA NIDEC + # all of this is only relevant for HONDA NIDEC max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) # TODO this 1.44 is just to maintain previous behavior pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6) pcm_speed_BP = [-wind_brake, -wind_brake*(3/4), - 0.0] + 0.0, + 0.1] + pcm_speed_V = [0.0, clip(CS.out.vEgo + accel/2.0 - 2.0, 0.0, 100.0), - clip(CS.out.vEgo + accel/2.0 + 2.0, 0.0, 100.0)] + clip(CS.out.vEgo + accel/2.0 + 2.0, 0.0, 100.0), + clip(CS.out.vEgo + 5.0, 0.0, 100.0)] pcm_speed = interp(-brake, pcm_speed_BP, pcm_speed_V) if not CS.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 6784414065..de7bcbe469 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -6,7 +6,7 @@ from common.params import Params from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL from selfdrive.car.honda.hondacan import disable_radar from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint -from selfdrive.car.interfaces import CarInterfaceBase +from selfdrive.car.interfaces import CarInterfaceBase, ACCEL_MAX, ACCEL_MIN from selfdrive.config import Conversions as CV @@ -16,6 +16,14 @@ TransmissionType = car.CarParams.TransmissionType class CarInterface(CarInterfaceBase): + @staticmethod + def get_pid_accel_limits(current_speed, cruise_speed): + # NIDECs don't allow acceleration near cruise_speed, + # so limit limits of pid to prevent windup + ACCEL_MAX_VALS = [ACCEL_MAX, 0.2] + ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2] + return ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS) + @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e2ace9c057..b068ee6009 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -17,6 +17,9 @@ EventName = car.CarEvent.EventName # WARNING: this value was determined based on the model's training distribution, # model predictions above this speed can be unpredictable MAX_CTRL_SPEED = (V_CRUISE_MAX + 4) * CV.KPH_TO_MS # 135 + 4 = 86 mph +ACCEL_MAX = 2.0 +ACCEL_MIN = -4.0 + # generic car and radar interfaces @@ -41,6 +44,10 @@ class CarInterfaceBase(): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP, self.VM) + @staticmethod + def get_pid_accel_limits(current_speed, cruise_speed): + return ACCEL_MIN, ACCEL_MAX + @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): return 1. diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a3242d6028..6aa605682b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -460,7 +460,8 @@ class Controls: if not self.joystick_mode: # accel PID loop - actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan) + pid_accel_limits = self.CI.get_pid_accel_limits(CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS) + actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits) # Steering PID loop and lateral MPC desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 1723c3293b..0c76a0b746 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -6,8 +6,6 @@ from selfdrive.modeld.constants import T_IDXS LongCtrlState = log.ControlsState.LongControlState -ACCEL_MAX = 2.0 -ACCEL_MIN = -4.0 STOPPING_EGO_SPEED = 0.5 STOPPING_TARGET_SPEED_OFFSET = 0.01 STARTING_TARGET_SPEED = 0.5 @@ -63,8 +61,6 @@ class LongControl(): (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=RATE, sat_limit=0.8) - self.pid.pos_limit = ACCEL_MAX - self.pid.neg_limit = ACCEL_MIN self.v_pid = 0.0 self.last_output_accel = 0.0 @@ -73,7 +69,7 @@ class LongControl(): self.pid.reset() self.v_pid = v_pid - def update(self, active, CS, CP, long_plan): + def update(self, active, CS, CP, long_plan, accel_limits): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Interp control trajectory # TODO estimate car specific lag, use .15s for now @@ -86,6 +82,8 @@ class LongControl(): v_target_future = 0.0 a_target = 0.0 + self.pid.neg_limit = accel_limits[0] + self.pid.pos_limit = accel_limits[1] # Update state machine output_accel = self.last_output_accel @@ -107,8 +105,9 @@ class LongControl(): # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV) + freeze_integrator = prevent_overshoot - output_accel = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot) + output_accel = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=freeze_integrator) if prevent_overshoot: output_accel = min(output_accel, 0.0) @@ -118,7 +117,7 @@ class LongControl(): # Keep applying brakes until the car is stopped if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET: output_accel -= CP.stoppingDecelRate / RATE - output_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX) + output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) self.reset(CS.vEgo) @@ -129,6 +128,6 @@ class LongControl(): self.reset(CS.vEgo) self.last_output_accel = output_accel - final_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX) + final_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return final_accel, v_target, a_target