mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-09 09:43:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

TorqueTune

Browse Source
This commit is contained in:
firestar5683
2025-02-10 14:16:13 -06:00
parent f912421bd7
commit 194084f415
17 changed files with 192 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/assets/other_images/frogpilot_boot_logo.png
BIN
View File
Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 778 KiB

After

Width:  |  Height:  |  Size: 568 KiB

frogpilot/common/frogpilot_variables.py
+4
View File
@@ -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)
frogpilot/controls/lib/conditional_experimental_mode.py
+8 -7
View File
@@ -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
frogpilot/controls/lib/frogpilot_acceleration.py
+45 -8
View File
@@ -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):
opendbc/gm_global_a_powertrain_generated.dbc
-1
View File
@@ -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
panda/board/obj/bootstub.panda.bin
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda_h7.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda_h7.bin.signed
BIN
View File
Binary file not shown.
selfdrive/car/gm/carcontroller.py
+37 -28
View File
@@ -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
selfdrive/car/gm/carstate.py
+1 -1
View File
@@ -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.
selfdrive/car/gm/gmcan.py
-1
View File
@@ -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]
selfdrive/car/gm/interface.py
+20 -19
View File
@@ -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
selfdrive/car/gm/values.py
+9 -3
View File
@@ -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")],
selfdrive/controls/lib/longcontrol.py
+49 -4
View File
@@ -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
selfdrive/controls/lib/pid.py
+10 -1
View File
@@ -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
selfdrive/controls/radard.py
+9 -7
View File
@@ -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),