From e9bafa6ea34d2f53a5264bf85ea3b99830837828 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 10 May 2024 12:06:28 -0700 Subject: [PATCH] Controls - Lateral Tuning - NNFF-Lite 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> --- selfdrive/car/interfaces.py | 1 + selfdrive/controls/lib/latcontrol_torque.py | 16 ++++++++++------ 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 0d87823..a4b9445 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -214,6 +214,7 @@ class CarInterfaceBase(ABC): nnff_supported = self.initialize_lat_torque_nn(CP.carFingerprint, eps_firmware) 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_lite = not use_comma_nnff and not self.use_nnff and lateral_tune and self.params.get_bool("NNFFLite") self.belowSteerSpeed_shown = False self.disable_belowSteerSpeed = False diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 117d5a1..5402698 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -74,8 +74,9 @@ class LatControlTorque(LatControl): # Twilsonco's Lateral Neural Network Feedforward 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, # 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. @@ -136,7 +137,7 @@ class LatControlTorque(LatControl): if self.use_steering_angle: actual_curvature = actual_curvature_vm 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_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2 else: @@ -158,7 +159,7 @@ class LatControlTorque(LatControl): lateral_jerk_setpoint = 0 lateral_jerk_measurement = 0 - if self.use_nnff: + if self.use_nnff or self.use_nnff_lite: # prepare "look-ahead" desired lateral jerk lat_accel_friction_factor = self.lat_accel_friction_factor if len(model_data.acceleration.y) == ModelConstants.IDX_N: @@ -223,12 +224,15 @@ class LatControlTorque(LatControl): else: 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, - 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, - 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 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, friction_input, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=True)