mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-29 08:33:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

decouple even more

Browse Source
This commit is contained in:
Jason Wen
2025-03-16 13:29:47 -04:00
parent e80a415daf
commit 54d52f7024
2 changed files with 49 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+8 -10
View File
@@ -67,23 +67,21 @@ class LatControlTorque(LatControl):
low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
self.nnlc.update_calculations(CS, VM, desired_lateral_accel)
ff, pid_log = self.nnlc.update_feed_forward(CS, params, pid_log, setpoint, measurement, calibrated_pose,
desired_lateral_accel, lateral_accel_deadzone)
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
self.nnlc.lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=self.nnlc.use_lateral_jerk, gravity_adjusted=False)
setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
self.nnlc.lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=self.nnlc.use_lateral_jerk, gravity_adjusted=False)
measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False)
pid_log.error = float(torque_from_setpoint - torque_from_measurement)
error = desired_lateral_accel - actual_lateral_accel
friction_input = self.nnlc.update_stock_lateral_jerk(error) if self.nnlc.use_lateral_jerk else error
ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
friction_input, lateral_accel_deadzone, friction_compensation=True,
desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, friction_compensation=True,
gravity_adjusted=True)
# Neural Network Lateral Control updates
# Override stock ff and pid_log.error
ff, pid_log = self.nnlc.update(CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel)
freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(pid_log.error,
feedforward=ff,
sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py
+41 -10
View File
@@ -77,6 +77,7 @@ def roll_pitch_adjust(roll, pitch):
class NeuralNetworkLateralControl:
def __init__(self, lac_torque):
self.lac_torque = lac_torque
self.CI = lac_torque.CI
self.CP = lac_torque.CP
self.CP_SP = lac_torque.CI.CP_SP
@@ -100,6 +101,9 @@ class NeuralNetworkLateralControl:
self.lateral_jerk_measurement: float = 0.0
self.lookahead_lateral_jerk: float = 0.0
self._ff = 0.0
self._pid_log = None
# twilsonco's Lateral Neural Network Feedforward
self.enabled = self.CI.CP_SP.neuralNetworkLateralControl.enabled
@@ -170,11 +174,11 @@ class NeuralNetworkLateralControl:
self.lateral_jerk_setpoint = self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
self.lateral_jerk_measurement = self.lat_jerk_friction_factor * self.actual_lateral_jerk
def update_feed_forward(self, CS, params, pid_log, setpoint, measurement, calibrated_pose,
def update_feed_forward(self, CS, params, setpoint, measurement, calibrated_pose,
desired_lateral_accel, lateral_accel_deadzone):
if not self.enabled or not self.model_valid:
return 0.0, pid_log
return
# update past data
roll = params.roll
@@ -207,7 +211,7 @@ class NeuralNetworkLateralControl:
+ past_rolls + future_rolls
torque_from_setpoint = self.flux_model.evaluate(nnff_setpoint_input)
torque_from_measurement = self.flux_model.evaluate(nnff_measurement_input)
pid_log.error = torque_from_setpoint - torque_from_measurement
self._pid_log.error = torque_from_setpoint - torque_from_measurement
# compute feedforward (same as nn setpoint output)
error = setpoint - measurement
@@ -215,19 +219,46 @@ class NeuralNetworkLateralControl:
nn_input = [CS.vEgo, desired_lateral_accel, friction_input, roll] \
+ past_lateral_accels_desired + future_planned_lateral_accels \
+ past_rolls + future_rolls
ff = self.flux_model.evaluate(nn_input)
self._ff = self.flux_model.evaluate(nn_input)
# apply friction override for cars with low NN friction response
if self.flux_model.friction_override:
pid_log.error += self.torque_from_lateral_accel(LatControlInputs(0.0, 0.0, CS.vEgo, CS.aEgo), self.torque_params,
self._pid_log.error += self.torque_from_lateral_accel(LatControlInputs(0.0, 0.0, CS.vEgo, CS.aEgo), self.torque_params,
friction_input,
lateral_accel_deadzone, friction_compensation=True,
gravity_adjusted=False)
return ff, pid_log
def update_stock_lateral_jerk(self, CS, setpoint, measurement, roll_compensation, desired_lateral_accel, actual_lateral_accel,
lateral_accel_deadzone, gravity_adjusted_lateral_accel):
if not self.use_lateral_jerk:
return
def update_stock_lateral_jerk(self, error):
accel_error = self.lat_accel_friction_factor * error
jerk_error = self.lat_jerk_friction_factor * self.actual_lateral_jerk
_error = desired_lateral_accel - actual_lateral_accel
friction_input = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.actual_lateral_jerk
return accel_error + jerk_error
torque_from_setpoint = self.torque_from_lateral_accel(
LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
self.lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=self.use_lateral_jerk, gravity_adjusted=False
)
torque_from_measurement = self.torque_from_lateral_accel(
LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
self.lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=self.use_lateral_jerk, gravity_adjusted=False
)
self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
self._ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
friction_input, lateral_accel_deadzone, friction_compensation=True,
gravity_adjusted=True)
def update(self, CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel):
self._ff = ff
self._pid_log = pid_log
self.update_calculations(CS, VM, desired_lateral_accel)
self.update_feed_forward(CS, params, setpoint, measurement, calibrated_pose, desired_lateral_accel, lateral_accel_deadzone)
self.update_stock_lateral_jerk(CS, setpoint, measurement, roll_compensation, desired_lateral_accel, actual_lateral_accel,
lateral_accel_deadzone, gravity_adjusted_lateral_accel)
return self._ff, self._pid_log