diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 6d474dcd85..73dd2ca398 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -16,6 +16,7 @@ A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName + def compute_gb_honda(accel, speed): creep_brake = 0.0 creep_speed = 2.3 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 4300e61d67..6b881a8f56 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -96,9 +96,9 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD]: ret.lateralTuning.pid.kf = 0.00006 - ret.mass = 1490. + STD_CARGO_KG #weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx + ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx ret.wheelbase = 2.7 - ret.steerRatio = 13.73 #Spec + ret.steerRatio = 13.73 # Spec tire_stiffness_factor = 0.385 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] @@ -178,7 +178,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]] - # these cars require a special panda safety mode due to missing counters and checksums in the messages if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70, CAR.GENESIS_G80]: @@ -207,7 +206,7 @@ class CarInterface(CarInterfaceBase): ret.canValid = self.cp.can_valid and self.cp_cam.can_valid events = self.create_common_events(ret) - #TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event + # TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 9510fff357..a36ea4e7b7 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -53,7 +53,7 @@ class CarInterfaceBase(): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate - ret.isPandaBlack = True # TODO: deprecate this field + ret.isPandaBlack = True # TODO: deprecate this field # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque @@ -71,6 +71,7 @@ class CarInterfaceBase(): ret.brakeMaxV = [1.] ret.openpilotLongitudinalControl = False ret.startAccel = 0.0 + ret.minSpeedCan = 0.3 ret.stoppingControl = False ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] @@ -182,9 +183,9 @@ class CarStateBase: @staticmethod def parse_gear_shifter(gear: str) -> car.CarState.GearShifter: d: Dict[str, car.CarState.GearShifter] = { - 'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral, - 'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive, - 'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake + 'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral, + 'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive, + 'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake } return d.get(gear, GearShifter.unknown) diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index fc1219453b..a7ab0ee0cd 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -52,7 +52,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] ret.lateralTuning.pid.kf = 0.00006 - # No steer below disable speed ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 9f2ac20e0a..1a003a8824 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -4,6 +4,7 @@ from selfdrive.car.nissan.values import CAR from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase + class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 8012624a3c..0cfb6fb494 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -62,7 +62,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] if candidate in [CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]: - ret.safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal + ret.safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal ret.mass = 1568 + STD_CARGO_KG ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 1fa84f9155..6e244a1a84 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -8,6 +8,7 @@ from selfdrive.car.interfaces import CarInterfaceBase EventName = car.CarEvent.EventName + class CarInterface(CarInterfaceBase): @staticmethod def compute_gb(accel, speed): @@ -195,6 +196,7 @@ class CarInterface(CarInterfaceBase): elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]: stop_and_go = True + ret.minSpeedCan = 0.375 ret.safetyParam = 73 ret.wheelbase = 2.63906 ret.steerRatio = 13.9 @@ -256,7 +258,7 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.PRIUS_TSS2: stop_and_go = True ret.safetyParam = 73 - ret.wheelbase = 2.70002 #from toyota online sepc. + ret.wheelbase = 2.70002 # from toyota online sepc. ret.steerRatio = 13.4 # True steerRation from older prius tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 252306322b..63da6672ba 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -6,6 +6,7 @@ from selfdrive.car.interfaces import CarInterfaceBase GEAR = car.CarState.GearShifter EventName = car.CarEvent.EventName + class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f31dcb05bf..bafa13f1ea 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -5,8 +5,7 @@ from selfdrive.controls.lib.pid import PIController LongCtrlState = log.ControlsState.LongControlState STOPPING_EGO_SPEED = 0.5 -MIN_CAN_SPEED = 0.3 # TODO: parametrize this in car interface -STOPPING_TARGET_SPEED = MIN_CAN_SPEED + 0.01 +STOPPING_TARGET_SPEED_OFFSET = 0.01 STARTING_TARGET_SPEED = 0.5 BRAKE_THRESHOLD_TO_PID = 0.2 @@ -18,12 +17,13 @@ RATE = 100.0 def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, - output_gb, brake_pressed, cruise_standstill): + output_gb, 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 \ (v_ego < STOPPING_EGO_SPEED and - ((v_pid < STOPPING_TARGET_SPEED and v_target < STOPPING_TARGET_SPEED) or - brake_pressed)) + ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or + brake_pressed)) starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill @@ -78,9 +78,9 @@ class LongControl(): output_gb = self.last_output_gb self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo, v_target_future, self.v_pid, output_gb, - CS.brakePressed, CS.cruiseState.standstill) + CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan) - v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 + 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) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 0412df36d3..2567b46067 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -10,7 +10,7 @@ from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother -from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED +from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX @@ -21,7 +21,7 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] -_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] +_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits @@ -144,7 +144,7 @@ class Planner(): else: starting = long_control_state == LongCtrlState.starting a_ego = min(sm['carState'].aEgo, 0.0) - reset_speed = MIN_CAN_SPEED if starting else v_ego + reset_speed = self.CP.minSpeedCan if starting else v_ego reset_accel = self.CP.startAccel if starting else a_ego self.v_acc = reset_speed self.a_acc = reset_accel diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 471f237023..e371e2abef 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8cc45567f52bb3bff7d354b8c387b2dc51c90731 \ No newline at end of file +f60a1c8e24558c1470f9a240a12037579d62c2c1