zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Controls - Lateral Tuning - NNFF-Lite

Browse Source 
Use Twilsonco's Neural Network Feedforward for enhanced precision in lateral control for cars without available NNFF logs.

Co-Authored-By: Tim Wilson <7284371+twilsonco@users.noreply.github.com>
This commit is contained in:
FrogAi 2024-05-10 12:06:28 -07:00
parent 5589bead02
commit e9bafa6ea3
2 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
selfdrive/car/interfaces.py
View File

@ -214,6 +214,7 @@ class CarInterfaceBase(ABC):
nnff_supported = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware) nnff_supported = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware)
use_comma_nnff = self.check_comma_nn_ff_support(CP.carFingerprint) use_comma_nnff = self.check_comma_nn_ff_support(CP.carFingerprint)
self.use_nnff = not use_comma_nnff and nnff_supported and lateral_tune and self.params.get_bool("NNFF") self.use_nnff = not use_comma_nnff and nnff_supported and lateral_tune and self.params.get_bool("NNFF")
self.use_nnff_lite = not use_comma_nnff and not self.use_nnff and lateral_tune and self.params.get_bool("NNFFLite")
self.belowSteerSpeed_shown = False self.belowSteerSpeed_shown = False
self.disable_belowSteerSpeed = False self.disable_belowSteerSpeed = False

16
selfdrive/controls/lib/latcontrol_torque.py
View File

@ -74,8 +74,9 @@ class LatControlTorque(LatControl):
# Twilsonco's Lateral Neural Network Feedforward # Twilsonco's Lateral Neural Network Feedforward
self.use_nnff = CI.use_nnff self.use_nnff = CI.use_nnff
self.use_nnff_lite = CI.use_nnff_lite
if self.use_nnff: if self.use_nnff or self.use_nnff_lite:
# Instantaneous lateral jerk changes very rapidly, making it not useful on its own, # Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
# however, we can "look ahead" to the future planned lateral jerk in order to guage # however, we can "look ahead" to the future planned lateral jerk in order to guage
# whether the current desired lateral jerk will persist into the future, i.e. # whether the current desired lateral jerk will persist into the future, i.e.
@ -136,7 +137,7 @@ class LatControlTorque(LatControl):
if self.use_steering_angle: if self.use_steering_angle:
actual_curvature = actual_curvature_vm actual_curvature = actual_curvature_vm
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
if self.use_nnff: if self.use_nnff or self.use_nnff_lite:
actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0) actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2 actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
else: else:
@ -158,7 +159,7 @@ class LatControlTorque(LatControl):
lateral_jerk_setpoint = 0 lateral_jerk_setpoint = 0
lateral_jerk_measurement = 0 lateral_jerk_measurement = 0
if self.use_nnff: if self.use_nnff or self.use_nnff_lite:
# prepare "look-ahead" desired lateral jerk # prepare "look-ahead" desired lateral jerk
lat_accel_friction_factor = self.lat_accel_friction_factor lat_accel_friction_factor = self.lat_accel_friction_factor
if len(model_data.acceleration.y) == ModelConstants.IDX_N: if len(model_data.acceleration.y) == ModelConstants.IDX_N:
@ -223,12 +224,15 @@ class LatControlTorque(LatControl):
else: else:
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation 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, torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False) lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=self.use_nnff_lite, gravity_adjusted=False)
torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=False, gravity_adjusted=False) lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=self.use_nnff_lite, gravity_adjusted=False)
pid_log.error = torque_from_setpoint - torque_from_measurement pid_log.error = torque_from_setpoint - torque_from_measurement
error = desired_lateral_accel - actual_lateral_accel error = desired_lateral_accel - actual_lateral_accel
friction_input = error if self.use_nnff_lite:
friction_input = lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
else:
friction_input = error
ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, 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, friction_input, lateral_accel_deadzone, friction_compensation=True,
gravity_adjusted=True) gravity_adjusted=True)