144 lines
6.2 KiB
Python
144 lines
6.2 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
"""
|
|
import json
|
|
import numpy as np
|
|
from typing import NamedTuple
|
|
from collections.abc import Callable
|
|
|
|
from iqdbc.car import structs
|
|
from iqdbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
|
from iqdbc.car.hyundai.values import HyundaiFlags
|
|
from iqdbc.car.subaru.values import SubaruFlags
|
|
from iqdbc.lvbs.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks
|
|
from iqdbc.lvbs.car.hyundai.longitudinal.helpers import LongitudinalTuningType
|
|
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
|
from iqdbc.lvbs.car.subaru.values_ext import SubaruFlagsIQ, SubaruSafetyFlagsIQ
|
|
from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
|
|
from iqdbc.lvbs.car.toyota.values import ToyotaFlagsIQ
|
|
|
|
|
|
class LatControlInputs(NamedTuple):
|
|
lateral_acceleration: float
|
|
roll_compensation: float
|
|
vego: float
|
|
aego: float
|
|
|
|
|
|
TorqueFromLateralAccelCallbackTypeTorqueSpace = Callable[[LatControlInputs, structs.CarParams.LateralTorqueTuning, bool], float]
|
|
|
|
|
|
class CarInterfaceBaseIQ:
|
|
@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 apply_iq_car_config(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
|
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()}
|
|
|
|
_apply_long_tuning(CI, CP, CP_IQ, params_dict)
|
|
_apply_torque_blend(CP, CP_IQ, params_dict)
|
|
_initialize_radar_tracks(CP, CP_IQ, can_recv, can_send)
|
|
_apply_creep_assist(CP, CP_IQ, params_dict)
|
|
_apply_toyota_options(CP, CP_IQ, params_dict)
|
|
|
|
|
|
def _apply_long_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
|
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_IQ.flags |= HyundaiFlagsIQ.LONG_TUNING_DYNAMIC.value
|
|
if hyundai_longitudinal_tuning == LongitudinalTuningType.PREDICTIVE:
|
|
CP_IQ.flags |= HyundaiFlagsIQ.LONG_TUNING_PREDICTIVE.value
|
|
|
|
_ = CI.get_longitudinal_tuning_iq(CP, CP_IQ)
|
|
|
|
|
|
def _apply_torque_blend(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
|
params_dict: dict[str, str]) -> None:
|
|
if CP.brand == 'tesla':
|
|
torque_blend = int(params_dict.get("IQTeslaTorqueBlend", 0)) == 1
|
|
if torque_blend:
|
|
CP_IQ.flags |= TeslaFlagsIQ.COOP_STEERING.value
|
|
|
|
|
|
def _initialize_radar_tracks(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
|
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_IQ.flags & HyundaiFlagsIQ.ENHANCED_SCC):
|
|
tracks_enabled = hyundai_enable_radar_tracks(can_recv, can_send, bus=0, addr=0x7d0)
|
|
CP.radarUnavailable = not tracks_enabled
|
|
|
|
|
|
def _apply_creep_assist(CP: structs.CarParams, CP_IQ: structs.IQCarParams, 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("IQSubaruCreepAssist", 0)) == 1
|
|
stop_and_go_manual_parking_brake = int(params_dict.get("IQSubaruCreepAssistManualBrake", 0)) == 1
|
|
|
|
if stop_and_go:
|
|
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value
|
|
if stop_and_go_manual_parking_brake:
|
|
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE.value
|
|
if stop_and_go or stop_and_go_manual_parking_brake:
|
|
CP_IQ.safetyParam |= SubaruSafetyFlagsIQ.STOP_AND_GO
|
|
|
|
|
|
def _apply_toyota_options(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
|
|
if CP.brand == 'toyota':
|
|
toyota_stock_long = int(params_dict.get("IQToyotaFactoryLong", 0)) == 1
|
|
toyota_sng_hack = int(params_dict.get("ToyotaSnGHack", 0)) == 1
|
|
|
|
if toyota_stock_long:
|
|
CP_IQ.flags |= ToyotaFlagsIQ.STOCK_LONGITUDINAL.value
|
|
|
|
if toyota_sng_hack:
|
|
CP_IQ.flags |= ToyotaFlagsIQ.STOP_AND_GO_HACK.value
|
|
CP.minEnableSpeed = -1.
|
|
CP.autoResumeSng = CP.openpilotLongitudinalControl
|