mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-23 02:23:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

NNLC: "Lazy" tight curve fixes and more toggles

Browse Source
This commit is contained in:
Jason Wen
2024-03-26 17:49:01 -04:00
parent 746d2b750c
commit 7153a7bb84
3 changed files with 16 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+2
View File
@@ -276,6 +276,7 @@ std::unordered_map<std::string, uint32_t> 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<std::string, uint32_t> keys = {
{"SunnylinkDongleId", PERSISTENT},
{"TorqueDeadzoneDeg", PERSISTENT | BACKUP},
{"TorqueFriction", PERSISTENT | BACKUP},
{"TorqueLateralJerk", PERSISTENT | BACKUP},
{"TorqueMaxLatAccel", PERSISTENT | BACKUP},
{"TorquedOverride", PERSISTENT | BACKUP},
{"ToyotaSnG", PERSISTENT | BACKUP},
selfdrive/controls/lib/latcontrol_torque.py
+12 -2
View File
@@ -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
selfdrive/manager/manager.py
+2
View File
@@ -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"),