sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
@@ -0,0 +1,148 @@
|
||||
"""
|
||||
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 json
|
||||
import numpy as np
|
||||
from typing import NamedTuple
|
||||
from collections.abc import Callable
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.car.subaru.values import SubaruFlags
|
||||
from opendbc.car.toyota.values import ToyotaSafetyFlags
|
||||
from opendbc.sunnypilot.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import LongitudinalTuningType
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from opendbc.sunnypilot.car.subaru.values_ext import SubaruFlagsSP, SubaruSafetyFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
|
||||
class LatControlInputs(NamedTuple):
|
||||
lateral_acceleration: float
|
||||
roll_compensation: float
|
||||
vego: float
|
||||
aego: float
|
||||
|
||||
|
||||
TorqueFromLateralAccelCallbackTypeTorqueSpace = Callable[[LatControlInputs, structs.CarParams.LateralTorqueTuning, bool], float]
|
||||
|
||||
|
||||
class CarInterfaceBaseSP:
|
||||
@staticmethod
|
||||
def torque_from_lateral_accel_linear_in_torque_space(latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning,
|
||||
gravity_adjusted: bool) -> float:
|
||||
# The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
|
||||
return latcontrol_inputs.lateral_acceleration / float(torque_params.latAccelFactor)
|
||||
|
||||
def torque_from_lateral_accel_in_torque_space(self) -> TorqueFromLateralAccelCallbackTypeTorqueSpace:
|
||||
return self.torque_from_lateral_accel_linear_in_torque_space
|
||||
|
||||
|
||||
class NanoFFModel:
|
||||
def __init__(self, weights_loc: str, platform: str):
|
||||
self.weights_loc = weights_loc
|
||||
self.platform = platform
|
||||
self.load_weights(platform)
|
||||
|
||||
def load_weights(self, platform: str):
|
||||
with open(self.weights_loc) as fob:
|
||||
self.weights = {k: np.array(v) for k, v in json.load(fob)[platform].items()}
|
||||
|
||||
def relu(self, x: np.ndarray):
|
||||
return np.maximum(0.0, x)
|
||||
|
||||
def forward(self, x: np.ndarray):
|
||||
assert x.ndim == 1
|
||||
x = (x - self.weights['input_norm_mat'][:, 0]) / (self.weights['input_norm_mat'][:, 1] - self.weights['input_norm_mat'][:, 0])
|
||||
x = self.relu(np.dot(x, self.weights['w_1']) + self.weights['b_1'])
|
||||
x = self.relu(np.dot(x, self.weights['w_2']) + self.weights['b_2'])
|
||||
x = self.relu(np.dot(x, self.weights['w_3']) + self.weights['b_3'])
|
||||
x = np.dot(x, self.weights['w_4']) + self.weights['b_4']
|
||||
return x
|
||||
|
||||
def predict(self, x: list[float], do_sample: bool = False):
|
||||
x = self.forward(np.array(x))
|
||||
if do_sample:
|
||||
pred = np.random.laplace(x[0], np.exp(x[1]) / self.weights['temperature'])
|
||||
else:
|
||||
pred = x[0]
|
||||
pred = pred * (self.weights['output_norm_mat'][1] - self.weights['output_norm_mat'][0]) + self.weights['output_norm_mat'][0]
|
||||
return pred
|
||||
|
||||
|
||||
def setup_interfaces(CI, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
params_list: list[dict[str, str]] | None = None,
|
||||
can_recv: CanRecvCallable | None = None, can_send: CanSendCallable | None = None) -> None:
|
||||
if params_list is None:
|
||||
params_list = []
|
||||
|
||||
params_dict = {k: v for param in params_list for k, v in param.items()}
|
||||
|
||||
_initialize_custom_longitudinal_tuning(CI, CP, CP_SP, params_dict)
|
||||
_initialize_coop_steering(CP, CP_SP, params_dict)
|
||||
_initialize_radar_tracks(CP, CP_SP, can_recv, can_send)
|
||||
_initialize_stop_and_go(CP, CP_SP, params_dict)
|
||||
_initialize_toyota(CP, CP_SP, params_dict)
|
||||
|
||||
|
||||
def _initialize_custom_longitudinal_tuning(CI, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
params_dict: dict[str, str]) -> None:
|
||||
|
||||
# Hyundai Custom Longitudinal Tuning
|
||||
if CP.brand == 'hyundai':
|
||||
hyundai_longitudinal_tuning = int(params_dict.get("HyundaiLongitudinalTuning", 0))
|
||||
if hyundai_longitudinal_tuning == LongitudinalTuningType.DYNAMIC:
|
||||
CP_SP.flags |= HyundaiFlagsSP.LONG_TUNING_DYNAMIC.value
|
||||
if hyundai_longitudinal_tuning == LongitudinalTuningType.PREDICTIVE:
|
||||
CP_SP.flags |= HyundaiFlagsSP.LONG_TUNING_PREDICTIVE.value
|
||||
|
||||
_ = CI.get_longitudinal_tuning_sp(CP, CP_SP)
|
||||
|
||||
|
||||
def _initialize_coop_steering(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
params_dict: dict[str, str]) -> None:
|
||||
if CP.brand == 'tesla':
|
||||
coop_steering = int(params_dict.get("TeslaCoopSteering", 0)) == 1
|
||||
if coop_steering:
|
||||
CP_SP.flags |= TeslaFlagsSP.COOP_STEERING.value
|
||||
|
||||
|
||||
def _initialize_radar_tracks(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
can_recv: CanRecvCallable | None = None, can_send: CanSendCallable | None = None) -> None:
|
||||
if CP.brand == 'hyundai':
|
||||
if CP.flags & HyundaiFlags.MANDO_RADAR and (CP.radarUnavailable or CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC):
|
||||
tracks_enabled = hyundai_enable_radar_tracks(can_recv, can_send, bus=0, addr=0x7d0)
|
||||
CP.radarUnavailable = not tracks_enabled
|
||||
|
||||
|
||||
def _initialize_stop_and_go(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_dict: dict[str, str]) -> None:
|
||||
if CP.brand == 'subaru' and not CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID):
|
||||
stop_and_go = int(params_dict.get("SubaruStopAndGo", 0)) == 1
|
||||
stop_and_go_manual_parking_brake = int(params_dict.get("SubaruStopAndGoManualParkingBrake", 0)) == 1
|
||||
|
||||
if stop_and_go:
|
||||
CP_SP.flags |= SubaruFlagsSP.STOP_AND_GO.value
|
||||
if stop_and_go_manual_parking_brake:
|
||||
CP_SP.flags |= SubaruFlagsSP.STOP_AND_GO_MANUAL_PARKING_BRAKE.value
|
||||
if stop_and_go or stop_and_go_manual_parking_brake:
|
||||
CP_SP.safetyParam |= SubaruSafetyFlagsSP.STOP_AND_GO
|
||||
|
||||
|
||||
def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_dict: dict[str, str]) -> None:
|
||||
if CP.brand == 'toyota':
|
||||
toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1
|
||||
toyota_stop_and_go_hack = int(params_dict.get("ToyotaStopAndGoHack", 0)) == 1
|
||||
|
||||
if toyota_stock_long:
|
||||
CP_SP.flags |= ToyotaFlagsSP.STOCK_LONGITUDINAL.value
|
||||
CP.alphaLongitudinalAvailable = False
|
||||
CP.openpilotLongitudinalControl = False
|
||||
CP.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
|
||||
|
||||
if toyota_stop_and_go_hack and CP.openpilotLongitudinalControl:
|
||||
CP_SP.flags |= ToyotaFlagsSP.STOP_AND_GO_HACK.value
|
||||
Reference in new issue
Block a user