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

Revert "Upstream Sync"

Browse Source 
This reverts commit 1ec8084286.
This commit is contained in:
firestar5683
2025-10-21 23:12:19 -05:00
parent 4bbaf022f3
commit dbabb56568
7 changed files with 30 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_pid.py
+2 -3
View File
@@ -10,8 +10,7 @@ class LatControlPID(LatControl):
super().__init__(CP, CI, dt)
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.ff_factor = CP.lateralTuning.pid.kf
k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles):
@@ -31,7 +30,7 @@ class LatControlPID(LatControl):
else:
# offset does not contribute to resistive torque
ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
ff = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(error,
selfdrive/controls/lib/latcontrol_torque.py
+18 -25
View File
@@ -27,15 +27,7 @@ MAX_LAT_JERK_UP = 2.5 # m/s^3
LOW_SPEED_X = [0, 10, 20, 30]
LOW_SPEED_Y = [15, 13, 10, 5]
KP = 1.0
KI = 0.1
KD = 0.3
# filter jerk and measurement rate with cutoff frequency equal ~0.5 jerk up limit
LP_FILTER_CUTOFF_HZ = 1.2
JERK_LOOKAHEAD_SECONDS = 0.19
JERK_GAIN = 0.3
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
class LatControlTorque(LatControl):
def __init__(self, CP, FPCP, CI, dt):
@@ -43,15 +35,14 @@ class LatControlTorque(LatControl):
self.torque_params = FPCP.lateralTuning.torque
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
self.pid = PIDController(KP, KI, KD, rate=1/self.dt)
self.pid = PIDController(self.torque_params.kp, self.torque_params.ki,
k_f=self.torque_params.kf, rate=1/self.dt)
self.update_limits()
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt=self.dt)
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt=self.dt)
self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt)
self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)
self.previous_measurement = 0.0
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
# FrogPilot variables
self.nnff = NeuralNetworkFeedforward(CP, self)
@@ -79,24 +70,22 @@ class LatControlTorque(LatControl):
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len))
expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES))
expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames]
# TODO factor out lateral jerk from error to later replace it with delay independent alternative
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
self.lat_accel_request_buffer.append(future_desired_lateral_accel)
self.requested_lateral_accel_buffer.append(future_desired_lateral_accel)
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay
measurement = measured_curvature * CS.vEgo ** 2
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
self.previous_measurement = measurement
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
setpoint = expected_lateral_accel
setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
error = setpoint - measurement
error_lsf = error + low_speed_factor / KP * error
error_lsf = error + low_speed_factor / self.torque_params.kp * error
if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite:
pid_log, ff = self.nnff.compute_nnff(
@@ -115,10 +104,15 @@ class LatControlTorque(LatControl):
ff = gravity_adjusted_future_lateral_accel
# latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
ff -= self.torque_params.latAccelOffset
ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
# TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
output_lataccel = self.pid.update(pid_log.error, -measurement_rate, CS.vEgo, ff, freeze_integrator)
output_lataccel = self.pid.update(pid_log.error,
-measurement_rate,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
pid_log.active = True
@@ -129,7 +123,6 @@ class LatControlTorque(LatControl):
pid_log.output = float(-output_torque) # TODO: log lat accel?
pid_log.actualLateralAccel = float(measurement)
pid_log.desiredLateralAccel = float(setpoint)
pid_log.desiredLateralJerk= float(desired_lateral_jerk)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
# TODO left is positive in this convention
selfdrive/controls/lib/longcontrol.py
+1 -1
View File
@@ -95,7 +95,7 @@ class LongControl:
pos_p_limit = 0.0 # if params("NoPositivePResponse") else None # put parameter-based control here
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
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()
selfdrive/controls/lib/pid.py
+3 -2
View File
@@ -5,12 +5,13 @@ from openpilot.common.numpy_fast import clip, interp
class PIDController:
def __init__(self, k_p, k_i, k_d=0.,
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
self.k_f = k_f # feedforward gain
if isinstance(self._k_p, Number):
self._k_p = [[0], [self._k_p]]
if isinstance(self._k_i, Number):
@@ -66,7 +67,7 @@ class PIDController:
elif self.neg_p_limit is not None and self.p < self.neg_p_limit:
self.p = self.neg_p_limit
self.d = self.k_d * error_rate
self.f = feedforward
self.f = self.k_f * feedforward
if override:
self.i -= self.i_unwind_rate * float(np.sign(self.i))