diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 263e317063..d0f4ace97f 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -6,10 +6,6 @@ from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - @staticmethod - def compute_gb(accel, speed): - return float(accel) / 3.0 - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index f5923515ef..f62f89f652 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -7,11 +7,6 @@ from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - - @staticmethod - def compute_gb(accel, speed): - return float(accel) / 3.0 - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 0f87836ece..c94f693c72 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -46,18 +46,13 @@ class CarController(): can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) - # GAS/BRAKE - # no output if not enabled, but keep sending keepalive messages - # treat pedals as one - final_pedal = actuators.gas - actuators.brake - if not enabled: # Stock ECU sends max regen when not enabled. apply_gas = P.MAX_ACC_REGEN apply_brake = 0 else: - apply_gas = int(round(interp(final_pedal, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) - apply_brake = int(round(interp(final_pedal, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) + apply_gas = int(round(interp(actuators.accel, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V))) + apply_brake = int(round(interp(actuators.accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V))) # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 85578cd98e..0362daf4bc 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -11,10 +11,6 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): - @staticmethod - def compute_gb(accel, speed): - return float(accel) / 4.0 - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 6f6b0d722a..244dacd16d 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -26,9 +26,9 @@ class CarControllerParams(): ZERO_GAS = 2048 MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle - self.GAS_LOOKUP_BP = [-0.25, 0., 0.5] + self.GAS_LOOKUP_BP = [-1.0, 0., 2.0] self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS] - self.BRAKE_LOOKUP_BP = [-1., -0.25] + self.BRAKE_LOOKUP_BP = [-4., -1.0] self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0] class CAR: diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 0d26a18852..e923e7dd57 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -11,7 +11,29 @@ from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert -# TODO: not clear this does anything useful +def compute_gb_honda_bosch(accel, speed): + #TODO returns 0s, is unused + return 0.0, 0.0 + + +def compute_gb_honda_nidec(accel, speed): + creep_brake = 0.0 + creep_speed = 2.3 + creep_brake_value = 0.15 + if speed < creep_speed: + creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value + gb = float(accel) / 4.8 - creep_brake + return clip(gb, 0.0, 1.0), clip(-gb, 0.0, 1.0) + + +def compute_gas_brake(accel, speed, fingerprint): + if fingerprint in HONDA_BOSCH: + return compute_gb_honda_bosch(accel, speed) + else: + return compute_gb_honda_nidec(accel, speed) + + +#TODO not clear this does anything useful def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): # hyst params brake_hyst_on = 0.02 # to activate brakes exceed this value @@ -94,8 +116,15 @@ class CarController(): P = self.params + if enabled: + accel = actuators.accel + gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) + else: + accel = 0.0 + gas, brake = 0.0, 0.0 + # *** apply brake hysteresis *** - brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) + pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** if not enabled and CS.out.cruiseState.enabled: @@ -107,7 +136,7 @@ class CarController(): pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise # *** rate limit after the enable check *** - self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL) + self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL) # vehicle hud display, wait for one update from 10Hz 0x304 msg if hud_show_lanes: @@ -147,18 +176,13 @@ class CarController(): lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl)) - accel = actuators.gas - actuators.brake # TODO: pass in LoC.long_control_state and use that to decide starting/stoppping stopping = accel < 0 and CS.out.vEgo < 0.3 starting = accel > 0 and CS.out.vEgo < 0.3 # Prevent rolling backwards - accel = -1.0 if stopping else accel - if CS.CP.carFingerprint in HONDA_BOSCH: - apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V) - else: - apply_accel = interp(accel, P.NIDEC_ACCEL_LOOKUP_BP, P.NIDEC_ACCEL_LOOKUP_V) + accel = -4.0 if stopping else accel # 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]) @@ -167,14 +191,15 @@ class CarController(): pcm_accel = int(clip(pcm_accel, 0, 1) * 0xc6) else: max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V) - pcm_accel = int(clip(apply_accel/max_accel, 0.0, 1.0) * 0xc6) + # 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] pcm_speed_V = [0.0, - clip(CS.out.vEgo + apply_accel/2.0 - 2.0, 0.0, 100.0), - clip(CS.out.vEgo + apply_accel/2.0 + 2.0, 0.0, 100.0)] - pcm_speed = interp(accel, pcm_speed_BP, pcm_speed_V) + 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)] + pcm_speed = interp(-brake, pcm_speed_BP, pcm_speed_V) if not CS.CP.openpilotLongitudinalControl: if (frame % 2) == 0: @@ -193,8 +218,8 @@ class CarController(): ts = frame * DT_CTRL if CS.CP.carFingerprint in HONDA_BOSCH: - apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) - can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint)) + bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V) + can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) @@ -209,7 +234,7 @@ class CarController(): gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0]) # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. # This prevents unexpected pedal range rescaling - apply_gas = clip(gas_mult * actuators.gas, 0., 1.) + apply_gas = clip(gas_mult * gas, 0., 1.) can_sends.append(create_gas_command(self.packer, apply_gas, idx)) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 5e08d48103..6784414065 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -15,38 +15,7 @@ EventName = car.CarEvent.EventName TransmissionType = car.CarParams.TransmissionType -def compute_gb_honda_bosch(accel, speed): - return float(accel) / 3.5 - - -def compute_gb_honda_nidec(accel, speed): - creep_brake = 0.0 - creep_speed = 2.3 - creep_brake_value = 0.15 - if speed < creep_speed: - creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value - return float(accel) / 4.8 - creep_brake - - -def compute_gb_acura(accel, speed): - GB_VALUES = [-2., 0.0, 0.8] - GB_BP = [-5., 0.0, 4.0] - return interp(accel, GB_BP, GB_VALUES) - - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController, CarState): - super().__init__(CP, CarController, CarState) - - if self.CS.CP.carFingerprint in HONDA_BOSCH: - self.compute_gb = compute_gb_honda_bosch - else: - self.compute_gb = compute_gb_honda_nidec - - @staticmethod - def compute_gb(accel, speed): # pylint: disable=method-hidden - raise NotImplementedError - @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): @@ -333,17 +302,6 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - if candidate in HONDA_BOSCH: - ret.gasMaxBP = [0.] # m/s - ret.gasMaxV = [0.6] - ret.brakeMaxBP = [0.] # m/s - ret.brakeMaxV = [1.] # max brake allowed, 3.5m/s^2 - else: - ret.gasMaxBP = [0.] # m/s - ret.gasMaxV = [0.6] # max gas allowed - ret.brakeMaxBP = [5., 20.] # m/s - ret.brakeMaxV = [1., 0.8] # max brake allowed - ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 @@ -422,7 +380,7 @@ class CarInterface(CarInterfaceBase): # we engage when pcm is active (rising edge) if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled: events.add(EventName.pcmEnable) - elif not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): + elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl): # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if ret.vEgo < self.CP.minEnableSpeed + 2.: diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index cfc3bfd8dc..c747c6291b 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -22,10 +22,7 @@ class CarControllerParams(): self.NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6] self.NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.] - - self.BOSCH_ACCEL_LOOKUP_BP = [-1., 0., 0.6] - self.BOSCH_ACCEL_LOOKUP_V = [-3.5, 0., 2.] - self.BOSCH_GAS_LOOKUP_BP = [0., 0.6] + self.BOSCH_GAS_LOOKUP_BP = [0., 2.0] # 2m/s^2 self.BOSCH_GAS_LOOKUP_V = [0, 2000] diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c09e1872da..1fee6e58bc 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -6,11 +6,6 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - - @staticmethod - def compute_gb(accel, speed): - return float(accel) / 3.0 - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 34c6c5a543..e2ace9c057 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -45,10 +45,6 @@ class CarInterfaceBase(): def calc_accel_override(a_ego, a_target, v_ego, v_target): return 1. - @staticmethod - def compute_gb(accel, speed): - raise NotImplementedError - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): raise NotImplementedError @@ -72,15 +68,11 @@ class CarInterfaceBase(): ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA - ret.gasMaxBP = [0.] - ret.gasMaxV = [.5] # half max brake - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] ret.openpilotLongitudinalControl = False ret.startAccel = 0.0 ret.minSpeedCan = 0.3 - ret.stoppingBrakeRate = 0.2 # brake_travel/s while trying to stop - ret.startingBrakeRate = 0.8 # brake_travel/s while releasing on restart + ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop + ret.startingAccelRate = 3.2 # brake_travel/s while releasing on restart ret.stoppingControl = True ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index d85abd7703..3edcbd0f8c 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -9,10 +9,6 @@ class CarInterface(CarInterfaceBase): super().__init__(CP, CarController, CarState) self.cp_adas = self.CS.get_adas_can_parser(CP) - @staticmethod - def compute_gb(accel, speed): - return float(accel) / 4.0 - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 436dda8d1b..260933cb25 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -6,10 +6,6 @@ from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - @staticmethod - def compute_gb(accel, speed): - return float(accel) / 4.0 - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index b3564c10c4..2078fa38b4 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -6,11 +6,6 @@ from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - @staticmethod - def compute_gb(accel, speed): - # TODO: is this correct? - return accel - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 42c47f1b01..94cc81406e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,5 +1,5 @@ from cereal import car -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ @@ -7,7 +7,6 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams from opendbc.can.packer import CANPacker - VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -45,7 +44,9 @@ class CarController(): # gas and brake interceptor_gas_cmd = 0. - pcm_accel_cmd = actuators.gas - actuators.brake + # TODO this is needed to preserve behavior, but doesn't make sense + # This can all be cleaned up + pcm_accel_cmd = actuators.accel / CarControllerParams.ACCEL_SCALE if CS.CP.enableGasInterceptor: # handle hysteresis when around the minimum acc speed @@ -57,8 +58,9 @@ class CarController(): if self.use_interceptor and enabled: # only send negative accel when using interceptor. gas handles acceleration # +0.06 offset to reduce ABS pump usage when OP is engaged - interceptor_gas_cmd = clip(actuators.gas, 0., 1.) - pcm_accel_cmd = 0.06 - actuators.brake + MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5]) + interceptor_gas_cmd = clip(actuators.accel/CarControllerParams.ACCEL_SCALE, 0., MAX_INTERCEPTOR_GAS) + pcm_accel_cmd = 0.06 - clip(-actuators.accel/CarControllerParams.ACCEL_SCALE, 0., 1.) pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled) pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 73aa1942e4..9a22934c17 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -9,10 +9,6 @@ EventName = car.CarEvent.EventName class CarInterface(CarInterfaceBase): - @staticmethod - def compute_gb(accel, speed): - return float(accel) / CarControllerParams.ACCEL_SCALE - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value ret = CarInterfaceBase.get_std_params(candidate, fingerprint) @@ -316,9 +312,6 @@ class CarInterface(CarInterfaceBase): ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu if ret.enableGasInterceptor: - # Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap - ret.gasMaxBP = [0., MIN_ACC_SPEED] - ret.gasMaxV = [0.2, 0.5] ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] @@ -331,8 +324,8 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7] ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.] ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1] - ret.stoppingBrakeRate = 0.1 # reach stopping target smoothly - ret.startingBrakeRate = 2.0 # release brakes fast + ret.stoppingDecelRate = 0.3 # reach stopping target smoothly + ret.startingAccelRate = 6.0 # release brakes fast ret.startAccel = 1.2 # Accelerate from 0 faster else: # Default longitudinal tune @@ -363,7 +356,7 @@ class CarInterface(CarInterfaceBase): events.add(EventName.lowSpeedLockout) if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: events.add(EventName.belowEngageSpeed) - if c.actuators.gas > 0.1: + if c.actuators.accel > 0.3: # some margin on the actuator to not false trigger cancellation while stopping events.add(EventName.speedTooLow) if ret.vEgo < 0.001: diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 802b5e27ba..0f3fa8a4a0 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -20,10 +20,6 @@ class CarInterface(CarInterfaceBase): self.ext_bus = CANBUS.cam self.cp_ext = self.cp_cam - @staticmethod - def compute_gb(accel, speed): - return float(accel) / 4.0 - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1f0225e0a7..4a0c13f473 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -118,7 +118,7 @@ class Controls: self.AM = AlertManager() self.events = Events() - self.LoC = LongControl(self.CP, self.CI.compute_gb) + self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) if self.CP.steerControlType == car.CarParams.SteerControlType.angle: @@ -459,8 +459,8 @@ class Controls: self.LoC.reset(v_pid=CS.vEgo) if not self.joystick_mode: - # Gas/Brake PID loop - actuators.gas, actuators.brake, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan) + # accel PID loop + actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan) # Steering PID loop and lateral MPC desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, @@ -472,8 +472,7 @@ class Controls: else: lac_log = log.ControlsState.LateralDebugState.new_message() if self.sm.rcv_frame['testJoystick'] > 0 and self.active: - gb = clip(self.sm['testJoystick'].axes[0], -1, 1) - actuators.gas, actuators.brake = max(gb, 0), max(-gb, 0) + actuators.accel = 4.0*clip(self.sm['testJoystick'].axes[0], -1, 1) steer = clip(self.sm['testJoystick'].axes[1], -1, 1) # max angle is 45 for angle-based cars @@ -528,7 +527,7 @@ class Controls: # TODO remove car specific stuff in controls # Some override values for Honda # brake discount removes a sharp nonlinearity - brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) + brake_discount = (1.0 - clip(-actuators.accel * (3.0/4.0), 0.0, 1.0)) speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) CC.cruiseControl.speedOverride = float(speed_override if self.CP.pcmCruise else 0.0) CC.cruiseControl.accelOverride = float(self.CI.calc_accel_override(CS.aEgo, self.a_target, diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 359f992667..2335f0885f 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -6,19 +6,23 @@ from selfdrive.modeld.constants import T_IDXS LongCtrlState = log.ControlsState.LongControlState +ACCEL_MAX = 2.0 +ACCEL_MIN = -4.0 +ACCEL_SCALE = 4.0 STOPPING_EGO_SPEED = 0.5 STOPPING_TARGET_SPEED_OFFSET = 0.01 STARTING_TARGET_SPEED = 0.5 -BRAKE_THRESHOLD_TO_PID = 0.2 +DECEL_THRESHOLD_TO_PID = 0.8 -BRAKE_STOPPING_TARGET = 0.5 # apply at least this amount of brake to maintain the vehicle stationary +DECEL_STOPPING_TARGET = 2.0 # apply at least this amount of brake to maintain the vehicle stationary RATE = 100.0 DEFAULT_LONG_LAG = 0.15 +# TODO this logic isn't really car independent, does not belong here def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, - output_gb, brake_pressed, cruise_standstill, min_speed_can): + output_accel, brake_pressed, cruise_standstill, min_speed_can): """Update longitudinal control state machine""" stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET stopping_condition = (v_ego < 2.0 and cruise_standstill) or \ @@ -47,22 +51,23 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, elif long_control_state == LongCtrlState.starting: if stopping_condition: long_control_state = LongCtrlState.stopping - elif output_gb >= -BRAKE_THRESHOLD_TO_PID: + elif output_accel >= -DECEL_THRESHOLD_TO_PID: long_control_state = LongCtrlState.pid return long_control_state class LongControl(): - def __init__(self, CP, compute_gb): + def __init__(self, CP): self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=RATE, - sat_limit=0.8, - convert=compute_gb) + sat_limit=0.8) + self.pid.pos_limit = ACCEL_MAX + self.pid.neg_limit = ACCEL_MIN self.v_pid = 0.0 - self.last_output_gb = 0.0 + self.last_output_accel = 0.0 def reset(self, v_pid): """Reset PID controller and change setpoint""" @@ -83,55 +88,48 @@ class LongControl(): a_target = 0.0 - # Actuation limits - gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV) - brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV) - # Update state machine - output_gb = self.last_output_gb + output_accel = self.last_output_accel self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo, - v_target_future, self.v_pid, output_gb, + v_target_future, self.v_pid, output_accel, CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan) v_ego_pid = max(CS.vEgo, CP.minSpeedCan) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 if self.long_control_state == LongCtrlState.off or CS.gasPressed: self.reset(v_ego_pid) - output_gb = 0. + output_accel = 0. # tracking objects and driving elif self.long_control_state == LongCtrlState.pid: self.v_pid = v_target - self.pid.pos_limit = gas_max - self.pid.neg_limit = - brake_max # 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 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) - output_gb = 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=prevent_overshoot) if prevent_overshoot: - output_gb = min(output_gb, 0.0) + output_accel = min(output_accel, 0.0) # Intention is to stop, switch to a different brake control until we stop elif self.long_control_state == LongCtrlState.stopping: # Keep applying brakes until the car is stopped - if not CS.standstill or output_gb > -BRAKE_STOPPING_TARGET: - output_gb -= CP.stoppingBrakeRate / RATE - output_gb = clip(output_gb, -brake_max, gas_max) + if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET: + output_accel -= CP.stoppingDecelRate / RATE + output_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX) self.reset(CS.vEgo) # Intention is to move again, release brake fast before handing control to PID elif self.long_control_state == LongCtrlState.starting: - if output_gb < -0.2: - output_gb += CP.startingBrakeRate / RATE + if output_accel < -DECEL_THRESHOLD_TO_PID: + output_accel += CP.startingAccelRate / RATE self.reset(CS.vEgo) - self.last_output_gb = output_gb - final_gas = clip(output_gb, 0., gas_max) - final_brake = -clip(output_gb, -brake_max, 0.) + self.last_output_accel = output_accel + final_accel = clip(output_accel, ACCEL_MIN, ACCEL_MAX) - return final_gas, final_brake, v_target, a_target + return final_accel, v_target, a_target diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 916b95c9ea..9a32c83583 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -11,7 +11,7 @@ def apply_deadzone(error, deadzone): return error class PIController(): - def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): + def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain self.k_f = k_f # feedforward gain @@ -23,7 +23,6 @@ class PIController(): self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate self.sat_limit = sat_limit - self.convert = convert self.reset() @@ -68,9 +67,6 @@ class PIController(): i = self.i + error * self.k_i * self.i_rate control = self.p + self.f + i - if self.convert is not None: - control = self.convert(control, speed=self.speed) - # Update when changing i will move the control away from the limits # or when i will move towards the sign of the error if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or @@ -79,9 +75,6 @@ class PIController(): self.i = i control = self.p + self.f + self.i - if self.convert is not None: - control = self.convert(control, speed=self.speed) - self.saturated = self._check_saturation(control, check_saturation, error) self.control = clip(control, self.neg_limit, self.pos_limit) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 6a092baead..086db9fadb 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b9d80fdb3942a1c43621349dafe0763b03717c10 \ No newline at end of file +b845a62e02d23f734dad2811b6a96d17447a933f \ No newline at end of file diff --git a/tools/replay/ui.py b/tools/replay/ui.py index d778e48c27..2809390292 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -10,6 +10,7 @@ import numpy as np import pygame # pylint: disable=import-error import cereal.messaging as messaging +from common.numpy_fast import clip from common.basedir import BASEDIR from selfdrive.config import UIParams as UP from tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, _FULL_FRAME_SIZE, @@ -146,10 +147,12 @@ def ui_thread(addr, frame_address): plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas - plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas + # TODO gas is deprecated + plot_arr[-1, name_to_arr_idx['computer_gas']] = clip(sm['carControl'].actuators.accel/4.0, 0.0, 1.0) plot_arr[-1, name_to_arr_idx['user_brake']] = sm['carState'].brake plot_arr[-1, name_to_arr_idx['steer_torque']] = sm['carControl'].actuators.steer * ANGLE_SCALE - plot_arr[-1, name_to_arr_idx['computer_brake']] = sm['carControl'].actuators.brake + # TODO brake is deprecated + plot_arr[-1, name_to_arr_idx['computer_brake']] = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0) plot_arr[-1, name_to_arr_idx['v_ego']] = sm['carState'].vEgo plot_arr[-1, name_to_arr_idx['v_pid']] = sm['controlsState'].vPid plot_arr[-1, name_to_arr_idx['v_override']] = sm['carControl'].cruiseControl.speedOverride diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 49082dd1a0..2bb89807fc 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -11,6 +11,7 @@ from typing import Any import cereal.messaging as messaging from common.params import Params +from common.numpy_fast import clip from common.realtime import Ratekeeper, DT_DMON from lib.can import can_function from selfdrive.car.honda.values import CruiseButtons @@ -295,9 +296,10 @@ def bridge(q): if is_openpilot_engaged: sm.update(0) - throttle_op = sm['carControl'].actuators.gas #[0,1] - brake_op = sm['carControl'].actuators.brake #[0,1] - steer_op = sm['carControl'].actuators.steeringAngleDeg + # TODO gas and brake is deprecated + throttle_op = clip(sm['carControl'].actuators.accel/4.0, 0.0, 1.0) + brake_op = clip(-sm['carControl'].actuators.accel/4.0, 0.0, 1.0) + steer_op = sm['carControl'].actuators.steeringAngleDeg throttle_out = throttle_op steer_out = steer_op