diff --git a/common/params.cc b/common/params.cc index e06508a283..de7345764e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -276,6 +276,7 @@ std::unordered_map keys = { {"NavModelUrl", PERSISTENT | BACKUP}, {"NNFF", PERSISTENT | BACKUP}, {"NNFFCarModel", PERSISTENT | BACKUP}, + {"NNFFNoLateralJerk", PERSISTENT | BACKUP}, {"OnroadScreenOff", PERSISTENT | BACKUP}, {"OnroadScreenOffBrightness", PERSISTENT | BACKUP}, {"OnroadScreenOffEvent", PERSISTENT | BACKUP}, @@ -308,6 +309,7 @@ std::unordered_map keys = { {"SunnylinkDongleId", PERSISTENT}, {"TorqueDeadzoneDeg", PERSISTENT | BACKUP}, {"TorqueFriction", PERSISTENT | BACKUP}, + {"TorqueLateralJerk", PERSISTENT | BACKUP}, {"TorqueMaxLatAccel", PERSISTENT | BACKUP}, {"TorquedOverride", PERSISTENT | BACKUP}, {"ToyotaSnG", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c5d0e7ef8f..53c6cabc21 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -83,7 +83,8 @@ class LatControlTorque(LatControl): self.torqued_override = self.param_s.get_bool("TorquedOverride") self._frame = 0 - self.use_lateral_jerk = False # TODO: make this a parameter in the UI + self.use_lateral_jerk = self.param_s.get_bool("TorqueLateralJerk") # TODO: make this a parameter in the UI + self.nnff_no_lateral_jerk = self.param_s.get_bool("NNFFNoLateralJerk") # TODO: make this a parameter in the UI # Twilsonco's Lateral Neural Network Feedforward self.use_nn = CI.has_lateral_torque_nn @@ -140,6 +141,8 @@ class LatControlTorque(LatControl): if self._frame % 250 == 0: self._frame = 0 self.torqued_override = self.param_s.get_bool("TorquedOverride") + self.use_lateral_jerk = self.param_s.get_bool("TorqueLateralJerk") + self.nnff_no_lateral_jerk = self.param_s.get_bool("NNFFNoLateralJerk") if not self.torqued_override: return @@ -197,7 +200,7 @@ class LatControlTorque(LatControl): predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs) desired_lateral_jerk = (interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, model_data.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) - if self.use_steering_angle or lookahead_lateral_jerk == 0.0: + if self.nnff_no_lateral_jerk or self.use_steering_angle or lookahead_lateral_jerk == 0.0: lookahead_lateral_jerk = 0.0 actual_lateral_jerk = 0.0 self.lat_accel_friction_factor = 1.0 @@ -231,7 +234,14 @@ class LatControlTorque(LatControl): + past_rolls + future_rolls torque_from_setpoint = self.torque_from_nn(nnff_setpoint_input) torque_from_measurement = self.torque_from_nn(nnff_measurement_input) + pid_log.error = torque_from_setpoint - torque_from_measurement + error_blend_factor = interp(abs(desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]) + if error_blend_factor > 0.0: # blend in stronger error response when in high lat accel + nnff_error_input = [CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0] + torque_from_error = self.torque_from_nn(nnff_error_input) + if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error): + pid_log.error = pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor # compute feedforward (same as nn setpoint output) error = setpoint - measurement diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 944fb12789..c4fd9e1309 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -72,6 +72,7 @@ def manager_init() -> None: ("MadsIconToggle", "1"), ("MaxTimeOffroad", "9"), ("NNFF", "0"), + ("NNFFNoLateralJerk", "0"), ("OnroadScreenOff", "-2"), ("OnroadScreenOffBrightness", "50"), ("OnroadScreenOffEvent", "1"), @@ -91,6 +92,7 @@ def manager_init() -> None: ("StockLongToyota", "0"), ("TorqueDeadzoneDeg", "0"), ("TorqueFriction", "1"), + ("TorqueLateralJerk", "0"), ("TorqueMaxLatAccel", "250"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"),