|
|
@ -1,7 +1,7 @@ |
|
|
|
#!/usr/bin/env python3 |
|
|
|
#!/usr/bin/env python3 |
|
|
|
import os |
|
|
|
import os |
|
|
|
import math |
|
|
|
import math |
|
|
|
from numbers import Number |
|
|
|
from typing import SupportsFloat |
|
|
|
|
|
|
|
|
|
|
|
from cereal import car, log |
|
|
|
from cereal import car, log |
|
|
|
from common.numpy_fast import clip |
|
|
|
from common.numpy_fast import clip |
|
|
@ -17,6 +17,7 @@ from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can |
|
|
|
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET |
|
|
|
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET |
|
|
|
from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise |
|
|
|
from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise |
|
|
|
from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature |
|
|
|
from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature |
|
|
|
|
|
|
|
from selfdrive.controls.lib.latcontrol import LatControl |
|
|
|
from selfdrive.controls.lib.longcontrol import LongControl |
|
|
|
from selfdrive.controls.lib.longcontrol import LongControl |
|
|
|
from selfdrive.controls.lib.latcontrol_pid import LatControlPID |
|
|
|
from selfdrive.controls.lib.latcontrol_pid import LatControlPID |
|
|
|
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI |
|
|
|
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI |
|
|
@ -137,6 +138,7 @@ class Controls: |
|
|
|
self.LoC = LongControl(self.CP) |
|
|
|
self.LoC = LongControl(self.CP) |
|
|
|
self.VM = VehicleModel(self.CP) |
|
|
|
self.VM = VehicleModel(self.CP) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
self.LaC: LatControl |
|
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle: |
|
|
|
if self.CP.steerControlType == car.CarParams.SteerControlType.angle: |
|
|
|
self.LaC = LatControlAngle(self.CP, self.CI) |
|
|
|
self.LaC = LatControlAngle(self.CP, self.CI) |
|
|
|
elif self.CP.lateralTuning.which() == 'pid': |
|
|
|
elif self.CP.lateralTuning.which() == 'pid': |
|
|
@ -607,7 +609,7 @@ class Controls: |
|
|
|
# Ensure no NaNs/Infs |
|
|
|
# Ensure no NaNs/Infs |
|
|
|
for p in ACTUATOR_FIELDS: |
|
|
|
for p in ACTUATOR_FIELDS: |
|
|
|
attr = getattr(actuators, p) |
|
|
|
attr = getattr(actuators, p) |
|
|
|
if not isinstance(attr, Number): |
|
|
|
if not isinstance(attr, SupportsFloat): |
|
|
|
continue |
|
|
|
continue |
|
|
|
|
|
|
|
|
|
|
|
if not math.isfinite(attr): |
|
|
|
if not math.isfinite(attr): |
|
|
|