|
|
@ -19,13 +19,13 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque |
|
|
|
from openpilot.selfdrive.controls.lib.longcontrol import LongControl |
|
|
|
from openpilot.selfdrive.controls.lib.longcontrol import LongControl |
|
|
|
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose |
|
|
|
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
State = log.SelfdriveState.OpenpilotState |
|
|
|
State = log.SelfdriveState.OpenpilotState |
|
|
|
LaneChangeState = log.LaneChangeState |
|
|
|
LaneChangeState = log.LaneChangeState |
|
|
|
LaneChangeDirection = log.LaneChangeDirection |
|
|
|
LaneChangeDirection = log.LaneChangeDirection |
|
|
|
|
|
|
|
|
|
|
|
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) |
|
|
|
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class Controls: |
|
|
|
class Controls: |
|
|
|
def __init__(self) -> None: |
|
|
|
def __init__(self) -> None: |
|
|
|
self.params = Params() |
|
|
|
self.params = Params() |
|
|
@ -114,7 +114,7 @@ class Controls: |
|
|
|
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, |
|
|
|
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, |
|
|
|
self.steer_limited, self.desired_curvature, |
|
|
|
self.steer_limited, self.desired_curvature, |
|
|
|
self.calibrated_pose) # TODO what if not available |
|
|
|
self.calibrated_pose) # TODO what if not available |
|
|
|
actuators.steer = float(steer) |
|
|
|
actuators.torque = float(steer) |
|
|
|
actuators.steeringAngleDeg = float(steeringAngleDeg) |
|
|
|
actuators.steeringAngleDeg = float(steeringAngleDeg) |
|
|
|
# Ensure no NaNs/Infs |
|
|
|
# Ensure no NaNs/Infs |
|
|
|
for p in ACTUATOR_FIELDS: |
|
|
|
for p in ACTUATOR_FIELDS: |
|
|
@ -164,7 +164,7 @@ class Controls: |
|
|
|
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ |
|
|
|
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ |
|
|
|
STEER_ANGLE_SATURATION_THRESHOLD |
|
|
|
STEER_ANGLE_SATURATION_THRESHOLD |
|
|
|
else: |
|
|
|
else: |
|
|
|
self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2 |
|
|
|
self.steer_limited = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 |
|
|
|
|
|
|
|
|
|
|
|
# TODO: both controlsState and carControl valids should be set by |
|
|
|
# TODO: both controlsState and carControl valids should be set by |
|
|
|
# sm.all_checks(), but this creates a circular dependency |
|
|
|
# sm.all_checks(), but this creates a circular dependency |
|
|
@ -212,6 +212,7 @@ class Controls: |
|
|
|
self.publish(CC, lac_log) |
|
|
|
self.publish(CC, lac_log) |
|
|
|
rk.monitor_time() |
|
|
|
rk.monitor_time() |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def main(): |
|
|
|
def main(): |
|
|
|
config_realtime_process(4, Priority.CTRL_HIGH) |
|
|
|
config_realtime_process(4, Priority.CTRL_HIGH) |
|
|
|
controls = Controls() |
|
|
|
controls = Controls() |
|
|
|