diff --git a/frogpilot/assets/other_images/frogpilot_boot_logo.png b/frogpilot/assets/other_images/frogpilot_boot_logo.png index 2505b8cb5..f47b42158 100644 Binary files a/frogpilot/assets/other_images/frogpilot_boot_logo.png and b/frogpilot/assets/other_images/frogpilot_boot_logo.png differ diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index 98e59a582..a4f90ffad 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -374,6 +374,10 @@ misc_tuning_levels: list[tuple[str, str | bytes, int]] = [ ("WheelControls", "", 2) ] + +def scale_threshold(v_ego): + return 0.0 if v_ego > 31.3 else np.interp(v_ego, [0, 17.9, 26.8, 35.8, 44.7], [0.63, 0.63, 0.65, 0.95, 0.95]) + class FrogPilotVariables: def __init__(self): self.frogpilot_toggles = get_frogpilot_toggles(block=False) diff --git a/frogpilot/controls/lib/conditional_experimental_mode.py b/frogpilot/controls/lib/conditional_experimental_mode.py index 626b263a0..f221510e6 100644 --- a/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/frogpilot/controls/lib/conditional_experimental_mode.py @@ -2,7 +2,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL -from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory +from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, THRESHOLD, params_memory, scale_threshold class ConditionalExperimentalMode: def __init__(self, FrogPilotPlanner): @@ -53,7 +53,7 @@ class ConditionalExperimentalMode: self.status_value = 8 return True - if frogpilot_toggles.conditional_lead and self.slow_lead_detected: + if frogpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 29.1: self.status_value = 9 if self.frogpilot_planner.lead_one.vLead < 1 else 10 return True @@ -69,7 +69,7 @@ class ConditionalExperimentalMode: def update_conditions(self, frogpilotCarState, v_ego, frogpilot_toggles): self.curve_detection(v_ego, frogpilot_toggles) - self.slow_lead(frogpilot_toggles) + self.slow_lead(frogpilot_toggles, v_ego) self.stop_sign_and_light(frogpilotCarState, v_ego, frogpilot_toggles) def curve_detection(self, v_ego, frogpilot_toggles): @@ -78,13 +78,14 @@ class ConditionalExperimentalMode: self.curvature_filter.update(self.frogpilot_planner.road_curvature_detected or curve_active) self.curve_detected = self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED - def slow_lead(self, frogpilot_toggles): + def slow_lead(self, frogpilot_toggles, v_ego): + v_lead = self.frogpilot_planner.lead_one.vLead if self.frogpilot_planner.tracking_lead: slower_lead = frogpilot_toggles.conditional_slower_lead and self.frogpilot_planner.frogpilot_following.slower_lead - stopped_lead = frogpilot_toggles.conditional_stopped_lead and self.frogpilot_planner.lead_one.vLead < 1 - + stopped_lead = frogpilot_toggles.conditional_stopped_lead and v_lead < 1 + lead_threshold = scale_threshold(v_ego) self.slow_lead_filter.update(slower_lead or stopped_lead) - self.slow_lead_detected = self.slow_lead_filter.x >= THRESHOLD + self.slow_lead_detected = self.slow_lead_filter.x >= lead_threshold else: self.slow_lead_filter.x = 0 self.slow_lead_detected = False diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 0726242d3..2170605e7 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -1,6 +1,43 @@ #!/usr/bin/env python3 import numpy as np +def cubic_interp(x, xp, fp): + """Cubic interpolation using NumPy's native operations for speed.""" + # Boundary conditions + if x <= xp[0]: + return fp[0] + elif x >= xp[-1]: + return fp[-1] + + # Find interval + i = np.searchsorted(xp, x) - 1 + i = max(0, min(i, len(xp)-2)) # clamp the index + + # Normalized position + t = (x - xp[i]) / float(xp[i+1] - xp[i]) + + # Hermite cubic formula + return fp[i]*(1 - 3*t**2 + 2*t**3) + fp[i+1]*(3*t**2 - 2*t**3) + +def akima_interp(x, xp, fp): + """Akima-inspired interpolation with reduced overshoot characteristics.""" + if x <= xp[0]: + return fp[0] + elif x >= xp[-1]: + return fp[-1] + + i = np.searchsorted(xp, x) - 1 + i = max(0, min(i, len(xp)-2)) # clamp the index + + t = (x - xp[i]) / float(xp[i+1] - xp[i]) + + # Quintic polynomial to reduce overshoot + t2 = t*t + t4 = t2*t2 + t3 = t2*t + return (fp[i]*(1 - 10*t3 + 15*t4 - 6*t3*t2) + + fp[i+1]*(10*t3 - 15*t4 + 6*t3*t2)) + from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT @@ -11,26 +48,26 @@ A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2 # MPH = [0.0, 11, 22, 34, 45, 56, 89] A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] A_CRUISE_MAX_VALS_ECO = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] -A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] -A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] +A_CRUISE_MAX_VALS_SPORT = [1.5, 1.5, 1.25, 1.5, 1.5, 1.5, 2.0] +A_CRUISE_MAX_VALS_SPORT_PLUS = [2.5, 2.5, 3.0, 2.5, 2.5, 2.5, 2.5] def get_max_accel_eco(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)) def get_max_accel_sport(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)) def get_max_accel_sport_plus(v_ego): - return float(np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)) + return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS)) def get_max_accel_low_speeds(max_accel, v_cruise): - return float(np.interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])) + return float(akima_interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel])) def get_max_accel_ramp_off(max_accel, v_cruise, v_ego): - return float(np.interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel])) + return float(akima_interp(v_cruise - v_ego, [0., 1., 5., 10.], [0., 0.5, 1.0, max_accel])) def get_max_allowed_accel(v_ego): - return float(np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])) # ISO 15622:2018 + return float(akima_interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0])) # ISO 15622:2018 class FrogPilotAcceleration: def __init__(self, FrogPilotPlanner): diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 8de42d598..3528e98d1 100644 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc/gm_global_a_powertrain_generated.dbc @@ -221,7 +221,6 @@ BO_ 715 ASCMGasRegenCmd: 8 K124_ASCM SG_ GasRegenFullStopActive : 13|1@0+ (1,0) [0|0] "" NEO SG_ GasRegenCmdActive : 0|1@0+ (1,0) [0|0] "" NEO SG_ RollingCounter : 7|2@0+ (1,0) [0|0] "" NEO - SG_ GasRegenAlwaysOne3 : 23|1@0+ (1,0) [0|1] "" NEO SG_ GasRegenCmd : 8|14@0+ (1,0) [0|0] "" NEO BO_ 717 ASCM_2CD: 5 K124_ASCM diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index 0e265c3a8..6e6287da1 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index b2910c3c3..6e04f1a68 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 00f7ffb4f..e9a3e24bd 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 145162d80..b699983d0 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index b77a8e8f1..2080db408 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,3 +1,4 @@ +import math from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.common.filter_simple import FirstOrderFilter @@ -48,6 +49,12 @@ class CarController(CarControllerBase): self.params = CarControllerParams(self.CP) self.params_ = Params() + self.mass = CP.mass + self.tireRadius = 0.075 * CP.wheelbase + 0.1453 + self.frontalArea = 1.05 * CP.wheelbase + 0.0679 + self.coeffDrag = 0.30 + self.airDensity = 1.225 + self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt']) self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[self.CP.carFingerprint]['chassis']) @@ -57,16 +64,15 @@ class CarController(CarControllerBase): self.accel_g = 0.0 @staticmethod - def calc_pedal_command(accel: float, long_active: bool) -> float: + def calc_pedal_command(accel: float, long_active: bool, car_velocity) -> float: if not long_active: return 0. zero = 0.15625 # 40/256 - if accel > 0.: - # Scales the accel from 0-1 to 0.156-1 - pedal_gas = clip(((1 - zero) * accel + zero), 0., 1.) + if accel < -0.5: + pedal_gas = 0 else: - # if accel is negative, -0.1 -> 0.015625 - pedal_gas = clip(zero + accel, 0., zero) # Make brake the same size as gas, but clip to regen + pedaloffset = interp(car_velocity, [0., 3, 6, 30], [0.10, 0.175, 0.240, 0.240]) + pedal_gas = clip((pedaloffset + accel * 0.6), 0.0, 1.0) return pedal_gas @@ -121,14 +127,6 @@ class CarController(CarControllerBase): if self.frame % 4 == 0: stopping = actuators.longControlState == LongCtrlState.stopping - # Pitch compensated acceleration; - # TODO: include future pitch (sm['modelDataV2'].orientation.y) to account for long actuator delay - if frogpilot_toggles.long_pitch and len(CC.orientationNED) > 1: - self.pitch.update(CC.orientationNED[1]) - self.accel_g = ACCELERATION_DUE_TO_GRAVITY * apply_deadzone(self.pitch.x, PITCH_DEADZONE) # driving uphill is positive pitch - accel += self.accel_g - brake_accel = actuators.accel + self.accel_g * interp(CS.out.vEgo, BRAKE_PITCH_FACTOR_BP, BRAKE_PITCH_FACTOR_V) - at_full_stop = CC.longActive and CS.out.standstill near_stop = CC.longActive and (CS.out.vEgo < self.params.NEAR_STOP_BRAKE_PHASE) interceptor_gas_cmd = 0 @@ -140,27 +138,38 @@ class CarController(CarControllerBase): self.apply_gas = self.params.INACTIVE_REGEN self.apply_brake = int(min(-100 * self.CP.stopAccel, self.params.MAX_BRAKE)) else: - # Normal operation - if self.CP.carFingerprint in EV_CAR: - self.params.update_ev_gas_brake_threshold(CS.out.vEgo) - if frogpilot_toggles.sport_plus: - self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) - else: - self.apply_gas = int(round(interp(accel, self.params.EV_GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(brake_accel, self.params.EV_BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + + if len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping: + accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY else: - if frogpilot_toggles.sport_plus: - self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP_PLUS, self.params.GAS_LOOKUP_V_PLUS))) - else: - self.apply_gas = int(round(interp(accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))) - self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + accel_due_to_pitch = 0.0 + + if frogpilot_toggles.sport_plus: + gas_max = self.params.MAX_GAS_PLUS + accel_max = self.params.ACCEL_MAX_PLUS + else: + gas_max = self.params.MAX_GAS + accel_max = self.params.ACCEL_MAX + + accel = clip(actuators.accel + accel_due_to_pitch, self.params.ACCEL_MIN, accel_max) + torque = self.tireRadius * ((self.mass*accel) + (0.5*self.coeffDrag*self.frontalArea*self.airDensity*CS.out.vEgo**2)) + + scaled_torque = torque + self.params.ZERO_GAS + apply_gas_torque = clip(scaled_torque, self.params.MAX_ACC_REGEN, gas_max) + BRAKE_SWITCH = int(round(interp(CS.out.vEgo, self.params.BRAKE_SWITCH_LOOKUP_BP, self.params.BRAKE_SWITCH_LOOKUP_V))) + brake_accel = min((scaled_torque - BRAKE_SWITCH)/(self.tireRadius*self.mass), 0) + self.apply_gas = int(round(apply_gas_torque)) + self.apply_brake = int(round(interp(brake_accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V))) + if self.apply_brake > 0: + self.apply_gas = self.params.INACTIVE_REGEN + # Don't allow any gas above inactive regen while stopping # FIXME: brakes aren't applied immediately when enabling at a stop if stopping: self.apply_gas = self.params.INACTIVE_REGEN if self.CP.carFingerprint in CC_ONLY_CAR: # gas interceptor only used for full long control on cars without ACC - interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive) + interceptor_gas_cmd = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo) if self.CP.enableGasInterceptor and self.apply_gas > self.params.INACTIVE_REGEN and CS.out.cruiseState.standstill: # "Tap" the accelerator pedal to re-engage ACC diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f405d2092..72a4d80c8 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -96,7 +96,7 @@ class CarState(CarStateBase): if self.CP.enableGasInterceptor: ret.gas = (pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + pt_cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. - threshold = 10 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting. + threshold = 12 if self.CP.carFingerprint in CAMERA_ACC_CAR else 4 # Panda 515 threshold = 10.88. Set lower to avoid panda blocking messages and GasInterceptor faulting. ret.gasPressed = ret.gas > threshold else: ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 32a0ca761..d8d50cfd4 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -66,7 +66,6 @@ def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop): "GasRegenFullStopActive": at_full_stop, "GasRegenAlwaysOne": 1, "GasRegenAlwaysOne2": 1, - "GasRegenAlwaysOne3": 1, } dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 8e5307c3a..c273d963b 100644 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -110,7 +110,7 @@ class CarInterface(CarInterfaceBase): else: ret.transmissionType = TransmissionType.automatic - ret.longitudinalTuning.kiBP = [5., 35.] + ret.longitudinalTuning.kiBP = [5., 35., 60.] if candidate in CAMERA_ACC_CAR: ret.experimentalLongitudinalAvailable = candidate not in CC_ONLY_CAR @@ -122,13 +122,14 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 10 * CV.KPH_TO_MS # Tuning for experimental long - ret.longitudinalTuning.kiV = [2.0, 1.5] + ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5] ret.vEgoStopping = 0.1 ret.vEgoStarting = 0.1 - ret.stoppingDecelRate = 2.0 # reach brake quickly after enabling + ret.stoppingDecelRate = 1.0 # reach brake quickly after enabling ret.vEgoStopping = 0.25 ret.vEgoStarting = 0.25 + ret.stopAccel = -0.25 if experimental_long: ret.pcmCruise = False @@ -136,7 +137,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG elif candidate in SDGM_CAR: - ret.longitudinalTuning.kiV = [0., 0.] # TODO: tuning + ret.longitudinalTuning.kiV = [0., 0., 0.] # TODO: tuning ret.experimentalLongitudinalAvailable = False ret.networkLocation = NetworkLocation.fwdCamera ret.pcmCruise = True @@ -155,7 +156,7 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = 7 * CV.MPH_TO_MS # Tuning - ret.longitudinalTuning.kiV = [2.4, 1.5] + ret.longitudinalTuning.kiV = [0.5, 0.5, 0.5] if ret.enableGasInterceptor: # Need to set ASCM long limits when using pedal interceptor, instead of camera ACC long limits @@ -206,6 +207,7 @@ class CarInterface(CarInterfaceBase): elif candidate in (CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_BOLT_CC): ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + ret.lateralTuning.torque.kp = 0.6 if ret.enableGasInterceptor: # ACC Bolts use pedal for full longitudinal control, not just sng @@ -271,13 +273,13 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.autoResumeSng = True - if candidate in CC_ONLY_CAR: + if candidate in CC_ONLY_CAR: #pedal interceptor tuning 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] - ret.longitudinalTuning.kf = 0.15 + ret.longitudinalTuning.kiBP = [0., 3., 6., 35.] + ret.longitudinalTuning.kiV = [0.125, 0.175, 0.225, 0.33] + ret.longitudinalTuning.kf = 0.25 ret.stoppingDecelRate = 0.8 else: # Pedal used for SNG, ACC for longitudinal control otherwise ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG @@ -294,16 +296,15 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = not frogpilot_toggles.disable_openpilot_long ret.pcmCruise = False - ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate) - - 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] + if not ret.enableGasInterceptor and candidate in CC_ONLY_CAR: #redneck tuning + 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.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.56] # == 2 km/h/s, 1.25 mph/s + ret.longitudinalActuatorDelay = 1. # TODO: measure this + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [0.1] + ret.stoppingDecelRate = 11.18 # == 25 mph/s (.04 rate) if candidate in CC_ONLY_CAR: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_NO_ACC diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 4ec137f88..4140b8815 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -38,7 +38,7 @@ class CarControllerParams: def __init__(self, CP): # Gas/brake lookups - self.ZERO_GAS = 6144 # Coasting + self.ZERO_GAS = 6150 # Coasting self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen if CP.carFingerprint in CAMERA_ACC_CAR and CP.carFingerprint not in CC_ONLY_CAR: @@ -49,6 +49,7 @@ class CarControllerParams: # Camera ACC vehicles have no regen while enabled. # Camera transitions to MAX_ACC_REGEN from ZERO_GAS and uses friction brakes instantly self.max_regen_acceleration = 0. + self.BRAKE_SWITCH_MAX = self.MAX_ACC_REGEN if CP.carFingerprint in EV_CAR else self.ZERO_GAS elif CP.carFingerprint in SDGM_CAR: self.MAX_GAS = 7496 @@ -56,6 +57,7 @@ class CarControllerParams: self.MAX_ACC_REGEN = 5610 self.INACTIVE_REGEN = 5650 self.max_regen_acceleration = 0. + self.BRAKE_SWITCH = self.ZERO_GAS else: self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill. @@ -65,15 +67,19 @@ class CarControllerParams: # ICE has much less engine braking force compared to regen in EVs, # lower threshold removes some braking deadzone self.max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1 + self.BRAKE_SWITCH_MAX = self.MAX_ACC_REGEN if CP.carFingerprint in EV_CAR else self.ZERO_GAS self.GAS_LOOKUP_BP = [self.max_regen_acceleration, 0., self.ACCEL_MAX] self.GAS_LOOKUP_BP_PLUS = [self.max_regen_acceleration, 0., self.ACCEL_MAX_PLUS] self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS] self.GAS_LOOKUP_V_PLUS = [self.MAX_ACC_REGEN, self.ZERO_GAS, self.MAX_GAS_PLUS] - self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, self.max_regen_acceleration] + self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, 0.] self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] + self.BRAKE_SWITCH_LOOKUP_BP = [0.5, 10] + self.BRAKE_SWITCH_LOOKUP_V = [self.ZERO_GAS, self.BRAKE_SWITCH_MAX] + # determined by letting Volt regen to a stop in L gear from 89mph, # and by letting off gas and allowing car to creep, for determining # the positive threshold values at very low speed @@ -169,7 +175,7 @@ class CAR(Platforms): GMCarDocs("Chevrolet Silverado 1500 2020-21", "Safety Package II"), GMCarDocs("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"), ], - GMCarSpecs(mass=2450, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0), + GMCarSpecs(mass=2994, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0), ) CHEVROLET_EQUINOX = GMPlatformConfig( [GMCarDocs("Chevrolet Equinox 2019-22")], diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f18184e58..6580bffbb 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,6 +4,8 @@ from openpilot.common.realtime import DT_CTRL 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 +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.selfdrive.car.gm.values import CarControllerParams CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -85,17 +87,49 @@ def long_control_state_trans_old_long(CP, active, long_control_state, v_ego, v_t return long_control_state - class LongControl: def __init__(self, CP): self.CP = CP self.long_control_state = LongCtrlState.off + self.experimental_mode = False + pos_p_limit = 0.0 self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) + k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL, + pos_p_limit=pos_p_limit) self.v_pid = 0.0 + self._mode_setup() self.last_output_accel = 0.0 + + + def update_mpc_mode(self, experimental_mode): + new_mode = 'blended' if experimental_mode else 'acc' + + if self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc': + self.mode_transition_timer = 0.0 + + if new_mode != self.current_mode: + self.prev_mode = self.current_mode + self.transitioning = True + self.mode_transition_timer = 0.0 + self.mode_transition_filter.x = self.last_output_accel + + self.current_mode = new_mode + + if self.transitioning: + self.mode_transition_timer += DT_CTRL + if self.mode_transition_timer >= self.mode_transition_duration: + self.transitioning = False + + def _mode_setup(self): + self.prev_mode = 'acc' + self.current_mode = 'acc' + self.mode_transition_filter = FirstOrderFilter(0.0, 0.5, DT_CTRL) + self.mode_transition_timer = 0.0 + self.mode_transition_duration = 1.0 + self.transitioning = False + def reset(self): self.pid.reset() @@ -124,8 +158,19 @@ class LongControl: else: # LongCtrlState.pid error = a_target - CS.aEgo - output_accel = self.pid.update(error, speed=CS.vEgo, - feedforward=a_target) + self.update_mpc_mode(self.experimental_mode) + raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target) + + + if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': + if raw_output_accel < 0 and raw_output_accel < self.last_output_accel: + progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration) + blend_factor = 1.0 - (1.0 - progress) * (1.0 - abs(raw_output_accel / CarControllerParams.ACCEL_MIN)) + output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * blend_factor + else: + output_accel = raw_output_accel + else: + output_accel = raw_output_accel self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 36cbf9c4e..14f203153 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -5,7 +5,9 @@ from openpilot.common.numpy_fast import clip, interp class PIDController: - def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): + def __init__(self, k_p, k_i, k_f=0., k_d=0., + pos_limit=1e308, neg_limit=-1e308, rate=100, + pos_p_limit=None, neg_p_limit=None): self._k_p = k_p self._k_i = k_i self._k_d = k_d @@ -20,6 +22,9 @@ class PIDController: self.pos_limit = pos_limit self.neg_limit = neg_limit + self.pos_p_limit = pos_p_limit + self.neg_p_limit = neg_p_limit + self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate self.speed = 0.0 @@ -53,6 +58,10 @@ class PIDController: self.speed = speed self.p = float(error) * self.k_p + if self.pos_p_limit is not None and self.p > self.pos_p_limit: + self.p = self.pos_p_limit + elif self.neg_p_limit is not None and self.p < self.neg_p_limit: + self.p = self.neg_p_limit self.f = feedforward * self.k_f self.d = error_rate * self.k_d diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 177600958..e4d023912 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -18,7 +18,7 @@ from openpilot.common.simple_kalman import KF1D from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles # Default lead acceleration decay set to 50% at 1s -_LEAD_ACCEL_TAU = 1.5 +_LEAD_ACCEL_TAU = 0.6 # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -79,7 +79,7 @@ class Track: # Learn if constant acceleration if abs(self.aLeadK) < 0.5: - self.aLeadTau.x = _LEAD_ACCEL_TAU + self.aLeadTau.x = min(max(self.aLeadTau, 1e-2) * 1.1, _LEAD_ACCEL_TAU) else: self.aLeadTau.update(0.0) @@ -163,14 +163,16 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): - lead_v_rel_pred = lead_msg.v[0] - model_v_ego + prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0) + blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK + get_RadarState_from_vision.prev_aLeadK = blended_aLeadK return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_v_rel_pred), - "vLead": float(v_ego + lead_v_rel_pred), - "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": float(lead_msg.a[0]), + "vRel": float(lead_msg.v[0] - model_v_ego), + "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "aLeadK": blended_aLeadK, "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob),