sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed 2026-07-21 07:44:02 +00:00
commit de77c89cf6
3995 files changed
+1260688

No files matched your search

Whitespace-only changes.
@@ -0,0 +1,68 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import os
import tomllib
from difflib import SequenceMatcher
from opendbc.car import structs
from openpilot.common.basedir import BASEDIR
TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "neural_network_data", "neural_network_lateral_control")
TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "opendbc_repo", "opendbc", "car", "torque_data/substitute.toml")
MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
def similarity(s1: str, s2: str) -> float:
return SequenceMatcher(None, s1, s2).ratio()
def get_nn_model_path(CP: structs.CarParams) -> tuple[str, str, bool]:
car_fingerprint = CP.carFingerprint
eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
def check_nn_path(_nn_candidate):
_model_path = None
_max_similarity = -1.0
for f in os.listdir(TORQUE_NN_MODEL_PATH):
if f.endswith(".json"):
model = os.path.splitext(f)[0]
similarity_score = similarity(model, _nn_candidate)
if similarity_score > _max_similarity:
_max_similarity = similarity_score
_model_path = os.path.join(TORQUE_NN_MODEL_PATH, f)
return _model_path, _max_similarity
if len(eps_fw) > 3:
eps_fw = eps_fw.replace("\\", "")
nn_candidate = f"{car_fingerprint} {eps_fw}"
else:
nn_candidate = car_fingerprint
model_path, max_similarity = check_nn_path(nn_candidate)
exact_match = max_similarity >= 0.99
if car_fingerprint not in model_path or 0.0 <= max_similarity < 0.9:
nn_candidate = car_fingerprint
model_path, max_similarity = check_nn_path(nn_candidate)
exact_match = max_similarity >= 0.99
if 0.0 <= max_similarity < 0.9:
with open(TORQUE_NN_MODEL_SUBSTITUTE_PATH, 'rb') as f:
sub = tomllib.load(f)
sub_candidate = sub.get(car_fingerprint, car_fingerprint)
for candidate in [car_fingerprint, sub_candidate]:
model_path, max_similarity = check_nn_path(candidate)
exact_match = False
if CP.steerControlType == structs.CarParams.SteerControlType.angle:
model_path = MOCK_MODEL_PATH
model_name = os.path.splitext(os.path.basename(model_path))[0]
return model_path, model_name, exact_match
@@ -0,0 +1,83 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from json import load
import numpy as np
from openpilot.selfdrive.modeld.parse_model_outputs import safe_exp
# dict used to rename activation functions whose names aren't valid python identifiers
ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'}
class NNTorqueModel:
def __init__(self, params_file, zero_bias=False):
with open(params_file) as f:
params = load(f)
self.input_size = params["input_size"]
self.output_size = params["output_size"]
self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
self.input_std = np.array(params["input_std"], dtype=np.float32).T
self.layers = []
self.friction_override = False
for layer_params in params["layers"]:
W = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_W'))], dtype=np.float32).T
b = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_b'))], dtype=np.float32).T
if zero_bias:
b = np.zeros_like(b)
activation = layer_params["activation"]
for k, v in ACTIVATION_FUNCTION_NAMES.items():
activation = activation.replace(k, v)
self.layers.append((W, b, activation))
self.validate_layers()
self.check_for_friction_override()
# Begin activation functions.
# These are called by name using the keys in the model json file
@staticmethod
def sigmoid(x):
return 1 / (1 + safe_exp(-x))
@staticmethod
def identity(x):
return x
# End activation functions
def forward(self, x):
for W, b, activation in self.layers:
x = getattr(self, activation)(x.dot(W) + b)
return x
def evaluate(self, input_array):
in_len = len(input_array)
if in_len != self.input_size:
# If the input is length 2-4, then it's a simplified evaluation.
# In that case, need to add on zeros to fill out the input array to match the correct length.
if 2 <= in_len:
input_array = input_array + [0] * (self.input_size - in_len)
else:
raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
input_array = np.array(input_array, dtype=np.float32)
# Rescale the input array using the input_mean and input_std
input_array = (input_array - self.input_mean) / self.input_std
output_array = self.forward(input_array)
return float(output_array[0, 0])
def validate_layers(self):
for _, _, activation in self.layers:
if not hasattr(self, activation):
raise ValueError(f"Unknown activation: {activation}")
def check_for_friction_override(self):
y = self.evaluate([10.0, 0.0, 0.2])
self.friction_override = (y < 0.1)
@@ -0,0 +1,164 @@
"""
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
from collections import deque
import math
import numpy as np
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
from opendbc.sunnypilot.car.interfaces import LatControlInputs
from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.params import Params
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase, sign
from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import MOCK_MODEL_PATH
from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel
LOW_SPEED_X = [0, 10, 20, 30]
LOW_SPEED_Y = [12, 3, 1, 0]
# At a given roll, if pitch magnitude increases, the
# gravitational acceleration component starts pointing
# in the longitudinal direction, decreasing the lateral
# acceleration component. Here we do the same thing
# to the roll value itself, then passed to nnff.
def roll_pitch_adjust(roll, pitch):
return roll * math.cos(pitch)
class NeuralNetworkLateralControl(LatControlTorqueExtBase):
def __init__(self, lac_torque, CP, CP_SP, CI):
super().__init__(lac_torque, CP, CP_SP, CI)
self.params = Params()
self.enabled = self.params.get_bool("NeuralNetworkLateralControl")
self.has_nn_model = CP_SP.neuralNetworkLateralControl.model.path != MOCK_MODEL_PATH
# NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data
# of lat accel and roll
# Past value is computed using previous desired lat accel and observed roll
self.model = NNTorqueModel(CP_SP.neuralNetworkLateralControl.model.path)
self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
self.pitch_last = 0.0
# setup future time offsets
self.future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
self.nn_future_times = [i + self.desired_lat_jerk_time for i in self.future_times]
# setup past time offsets
self.past_times = [-0.3, -0.2, -0.1]
history_check_frames = [int(abs(i)*100) for i in self.past_times]
self.history_frame_offsets = [history_check_frames[0] - i for i in history_check_frames]
self.lateral_accel_desired_deque = deque(maxlen=history_check_frames[0])
self.roll_deque = deque(maxlen=history_check_frames[0])
self.error_deque = deque(maxlen=history_check_frames[0])
self.past_future_len = len(self.past_times) + len(self.nn_future_times)
@property
def _nnlc_enabled(self):
return self.enabled and self.model_valid and self.has_nn_model
def update_limits(self):
if not self._nnlc_enabled:
return
self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
def update_lateral_lag(self, lag):
super().update_lateral_lag(lag)
self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times]
def update_feedforward_torque_space(self, CS):
torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._roll_compensation, CS.vEgo, CS.aEgo),
self.torque_params, gravity_adjusted=False)
torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._measurement, self._roll_compensation, CS.vEgo, CS.aEgo),
self.torque_params, gravity_adjusted=False)
self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation,
CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True)
self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone,
FRICTION_THRESHOLD, self.torque_params)
def update_output_torque(self, CS):
freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
self._output_torque = self._pid.update(self._pid_log.error,
feedforward=self._ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
if not self._nnlc_enabled:
return
self.update_feedforward_torque_space(CS)
low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature
self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature
# update past data
roll = params.roll
if calibrated_pose is not None:
pitch = self.pitch.update(calibrated_pose.orientation.pitch)
roll = roll_pitch_adjust(roll, pitch)
self.pitch_last = pitch
self.roll_deque.append(roll)
self.lateral_accel_desired_deque.append(self._desired_lateral_accel)
# prepare past and future values
# adjust future times to account for longitudinal acceleration
adjusted_future_times = [t + 0.5 * CS.aEgo * (t / max(CS.vEgo, 1.0)) for t in self.nn_future_times]
past_rolls = [self.roll_deque[min(len(self.roll_deque) - 1, i)] for i in self.history_frame_offsets]
future_rolls = [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.x) + roll,
np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.y) + self.pitch_last) for t in
adjusted_future_times]
past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque) - 1, i)]
for i in self.history_frame_offsets]
future_planned_lateral_accels = [np.interp(t, ModelConstants.T_IDXS, self.model_v2.acceleration.y) for t in
adjusted_future_times]
# compute NNFF error response
nnff_setpoint_input = [CS.vEgo, self._setpoint, self.lateral_jerk_setpoint, roll] \
+ [self._setpoint] * self.past_future_len \
+ past_rolls + future_rolls
# past lateral accel error shouldn't count, so use past desired like the setpoint input
nnff_measurement_input = [CS.vEgo, self._measurement, self.lateral_jerk_measurement, roll] \
+ [self._measurement] * self.past_future_len \
+ past_rolls + future_rolls
torque_from_setpoint = self.model.evaluate(nnff_setpoint_input)
torque_from_measurement = self.model.evaluate(nnff_measurement_input)
self._pid_log.error = torque_from_setpoint - torque_from_measurement
# The "pure" NNLC error response can be too weak for cars whose models were trained
# with a lack of high-magnitude lateral acceleration data, for which the NNLC model
# torque response flattens out at high lateral accelerations.
# This workaround blends in a guaranteed stronger error response only when the
# desired lateral acceleration is high enough to warrant it, by using the lateral acceleration
# error as the input to the NNLC model. This is not ideal, and potentially degrades the NNLC
# accuracy for cars that don't have this issue, but it's necessary until a better NNLC model
# structure is used that doesn't create this issue when high-magnitude data is missing.
error_blend_factor = float(np.interp(abs(self._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 inputs 5+ are optional, and if left out are replaced with 0.0 inside the NNFF class
nnff_error_input = [CS.vEgo, self._setpoint - self._measurement, self.lateral_jerk_setpoint - self.lateral_jerk_measurement, 0.0]
torque_from_error = self.model.evaluate(nnff_error_input)
if sign(self._pid_log.error) == sign(torque_from_error) and abs(self._pid_log.error) < abs(torque_from_error):
self._pid_log.error = self._pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor
# compute feedforward (same as nn setpoint output)
friction_input = self.update_friction_input(self._setpoint, self._measurement)
nn_input = [CS.vEgo, self._desired_lateral_accel, friction_input, roll] \
+ past_lateral_accels_desired + future_planned_lateral_accels \
+ past_rolls + future_rolls
self._ff = self.model.evaluate(nn_input)
# apply friction override for cars with low NN friction response
if self.model.friction_override:
self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
self.update_output_torque(CS)
Whitespace-only changes.
@@ -0,0 +1,43 @@
from opendbc.car.car_helpers import interfaces
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.nissan.values import CAR as NISSAN
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.tesla.values import CAR as TESLA
from openpilot.common.parameterized import parameterized
from openpilot.common.params import Params
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
FINGERPRINT_EXACT_MATCH = [HONDA.HONDA_CIVIC_BOSCH, TOYOTA.TOYOTA_RAV4_TSS2_2022, HYUNDAI.HYUNDAI_IONIQ_5]
FINGERPRINT_FUZZY_MATCH = [HONDA.HONDA_CIVIC_BOSCH_DIESEL, HYUNDAI.GENESIS_G70_2020, HYUNDAI.HYUNDAI_IONIQ_6]
FINGERPRINT_ANGLE_NO_MATCH = [TOYOTA.TOYOTA_RAV4_TSS2_2023, NISSAN.NISSAN_LEAF, TESLA.TESLA_MODEL_3]
class TestNNLCFingerprintBase:
@staticmethod
def _setup_platform(car_name):
CarInterface = interfaces[car_name]
CP = CarInterface.get_non_essential_params(car_name)
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
CI = CarInterface(CP, CP_SP)
sunnypilot_interfaces.setup_interfaces(CI, Params())
return CI
@parameterized.expand(FINGERPRINT_EXACT_MATCH)
def test_exact_fingerprint(self, car_name):
CI = self._setup_platform(car_name)
assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and not CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
@parameterized.expand(FINGERPRINT_FUZZY_MATCH)
def test_fuzzy_fingerprint(self, car_name):
CI = self._setup_platform(car_name)
assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
@parameterized.expand(FINGERPRINT_ANGLE_NO_MATCH)
def test_no_fingerprint(self, car_name):
CI = self._setup_platform(car_name)
assert CI.CP_SP.neuralNetworkLateralControl.model.name == "MOCK"
@@ -0,0 +1,31 @@
from opendbc.car.car_helpers import interfaces
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
from openpilot.common.parameterized import parameterized
from openpilot.common.params import Params
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car.helpers import convert_to_capnp
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
class TestNNTorqueModel:
@parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN])
def test_load_model(self, car_name):
params = Params()
params.put_bool("NeuralNetworkLateralControl", True, block=True)
CarInterface = interfaces[car_name]
CP = CarInterface.get_non_essential_params(car_name)
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
CI = CarInterface(CP, CP_SP)
sunnypilot_interfaces.setup_interfaces(CI, params)
CP_SP = convert_to_capnp(CP_SP)
controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
assert controller.extension.has_nn_model
@@ -0,0 +1,103 @@
import numpy as np
from openpilot.cereal import log, messaging
from opendbc.car.structs import car
from opendbc.car.car_helpers import interfaces
from opendbc.car.gm.values import CAR as GM
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.parameterized import parameterized
from openpilot.common.params import Params
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car.helpers import convert_to_capnp
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.locationd.helpers import Pose
from openpilot.common.mock.generators import generate_livePose
from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
from openpilot.selfdrive.modeld.constants import ModelConstants
def generate_modelV2():
model = messaging.new_message('modelV2')
position = log.XYZTData.new_message()
speed = 30
position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
model.modelV2.position = position
orientation = log.XYZTData.new_message()
curvature = 0.05
orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
model.modelV2.orientation = orientation
velocity = log.XYZTData.new_message()
velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
velocity.x[0] = float(speed) # always start at current speed
model.modelV2.velocity = velocity
acceleration = log.XYZTData.new_message()
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
model.modelV2.acceleration = acceleration
return model
class TestNeuralNetworkLateralControl:
@parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, GM.CHEVROLET_BOLT_EUV])
def test_saturation(self, car_name):
params = Params()
params.put_bool("NeuralNetworkLateralControl", True, block=True)
CarInterface = interfaces[car_name]
CP = CarInterface.get_non_essential_params(car_name)
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
CI = CarInterface(CP, CP_SP)
sunnypilot_interfaces.setup_interfaces(CI, params)
CP_SP = convert_to_capnp(CP_SP)
VM = VehicleModel(CP)
controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
torque_params = CP.lateralTuning.torque
CS = car.CarState.new_message()
CS.vEgo = 30
CS.steeringPressed = False
params = log.LiveParametersData.new_message()
lp = generate_livePose()
pose = Pose.from_live_pose(lp.livePose)
mdl = generate_modelV2()
sm = {'modelV2': mdl.modelV2}
model_v2 = sm['modelV2']
controller.extension.model_v2 = model_v2
# Saturate for curvature limited and controller limited
test_lag = 0.3
for _ in range(1000):
controller.extension.update_model_v2(model_v2)
controller.extension.update_lateral_lag(test_lag)
controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
controller.extension.update_limits()
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
assert lac_log.saturated
for _ in range(1000):
controller.extension.update_model_v2(model_v2)
controller.extension.update_lateral_lag(test_lag)
controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
controller.extension.update_limits()
_, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
assert not lac_log.saturated
for _ in range(1000):
controller.extension.update_model_v2(model_v2)
controller.extension.update_lateral_lag(test_lag)
controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
controller.extension.update_limits()
_, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
assert lac_log.saturated