|
|
|
@ -87,7 +87,7 @@ def get_torque_params(): |
|
|
|
|
# generic car and radar interfaces |
|
|
|
|
|
|
|
|
|
class CarInterfaceBase(ABC): |
|
|
|
|
def __init__(self, CP, CarController, CarState): |
|
|
|
|
def __init__(self, CP: car.CarParams, CarController, CarState): |
|
|
|
|
self.CP = CP |
|
|
|
|
|
|
|
|
|
self.frame = 0 |
|
|
|
@ -97,7 +97,7 @@ class CarInterfaceBase(ABC): |
|
|
|
|
self.silent_steer_warning = True |
|
|
|
|
self.v_ego_cluster_seen = False |
|
|
|
|
|
|
|
|
|
self.CS = CarState(CP) |
|
|
|
|
self.CS: CarStateBase = CarState(CP) |
|
|
|
|
self.cp = self.CS.get_can_parser(CP) |
|
|
|
|
self.cp_cam = self.CS.get_cam_can_parser(CP) |
|
|
|
|
self.cp_adas = self.CS.get_adas_can_parser(CP) |
|
|
|
@ -116,7 +116,7 @@ class CarInterfaceBase(ABC): |
|
|
|
|
return ACCEL_MIN, ACCEL_MAX |
|
|
|
|
|
|
|
|
|
@classmethod |
|
|
|
|
def get_non_essential_params(cls, candidate: str): |
|
|
|
|
def get_non_essential_params(cls, candidate: str) -> car.CarParams: |
|
|
|
|
""" |
|
|
|
|
Parameters essential to controlling the car may be incomplete or wrong without FW versions or fingerprints. |
|
|
|
|
""" |
|
|
|
@ -151,7 +151,7 @@ class CarInterfaceBase(ABC): |
|
|
|
|
@staticmethod |
|
|
|
|
@abstractmethod |
|
|
|
|
def _get_params(ret: car.CarParams, candidate, fingerprint: dict[int, dict[int, int]], |
|
|
|
|
car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool): |
|
|
|
|
car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool) -> car.CarParams: |
|
|
|
|
raise NotImplementedError |
|
|
|
|
|
|
|
|
|
@staticmethod |
|
|
|
@ -211,7 +211,7 @@ class CarInterfaceBase(ABC): |
|
|
|
|
return ret |
|
|
|
|
|
|
|
|
|
@staticmethod |
|
|
|
|
def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True): |
|
|
|
|
def configure_torque_tune(candidate: str, tune: car.CarParams.LateralTuning, steering_angle_deadzone_deg: float = 0.0, use_steering_angle: bool = True): |
|
|
|
|
params = get_torque_params()[candidate] |
|
|
|
|
|
|
|
|
|
tune.init('torque') |
|
|
|
@ -342,10 +342,10 @@ class CarInterfaceBase(ABC): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class RadarInterfaceBase(ABC): |
|
|
|
|
def __init__(self, CP): |
|
|
|
|
def __init__(self, CP: car.CarParams): |
|
|
|
|
self.CP = CP |
|
|
|
|
self.rcp = None |
|
|
|
|
self.pts = {} |
|
|
|
|
self.pts: dict[int, car.RadarData.RadarPoint] = {} |
|
|
|
|
self.delay = 0 |
|
|
|
|
self.radar_ts = CP.radarTimeStep |
|
|
|
|
self.frame = 0 |
|
|
|
@ -358,7 +358,7 @@ class RadarInterfaceBase(ABC): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class CarStateBase(ABC): |
|
|
|
|
def __init__(self, CP): |
|
|
|
|
def __init__(self, CP: car.CarParams): |
|
|
|
|
self.CP = CP |
|
|
|
|
self.car_fingerprint = CP.carFingerprint |
|
|
|
|
self.out = car.CarState.new_message() |
|
|
|
@ -380,6 +380,10 @@ class CarStateBase(ABC): |
|
|
|
|
K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) |
|
|
|
|
self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) |
|
|
|
|
|
|
|
|
|
@abstractmethod |
|
|
|
|
def update(self, *args) -> car.CarState: |
|
|
|
|
pass |
|
|
|
|
|
|
|
|
|
def update_speed_kf(self, v_ego_raw): |
|
|
|
|
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed |
|
|
|
|
self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) |
|
|
|
@ -462,12 +466,12 @@ class CarStateBase(ABC): |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class CarControllerBase(ABC): |
|
|
|
|
def __init__(self, dbc_name: str, CP): |
|
|
|
|
def __init__(self, dbc_name: str, CP: car.CarParams): |
|
|
|
|
self.CP = CP |
|
|
|
|
self.frame = 0 |
|
|
|
|
|
|
|
|
|
@abstractmethod |
|
|
|
|
def update(self, CC: car.CarControl.Actuators, CS: car.CarState, now_nanos: int) -> tuple[car.CarControl.Actuators, list[CanData]]: |
|
|
|
|
def update(self, CC: car.CarControl, CS: CarStateBase, now_nanos: int) -> tuple[car.CarControl.Actuators, list[CanData]]: |
|
|
|
|
pass |
|
|
|
|
|
|
|
|
|
|
|
|
|
|