mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-03 22:53:44 +08:00
firestar5683
65 lines
2.5 KiB
Python
65 lines
2.5 KiB
Python
import math
|
|
from types import SimpleNamespace
|
|
|
|
import capnp
|
|
|
|
from cereal import log
|
|
from openpilot.common.pid import PIDController
|
|
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
|
|
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
|
from openpilot.selfdrive.locationd.helpers import Pose
|
|
|
|
|
|
CURVATURE_SATURATION_THRESHOLD = 1e-3 # 1/m
|
|
|
|
|
|
class LatControlCurvature(LatControl):
|
|
def __init__(self, CP, CI, dt):
|
|
super().__init__(CP, CI, dt)
|
|
self.sat_check_min_speed = 5.
|
|
if CP.lateralTuning.which() == "pid":
|
|
tuning = CP.lateralTuning.pid
|
|
self.pid = PIDController((tuning.kpBP, tuning.kpV), (tuning.kiBP, tuning.kiV),
|
|
pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt)
|
|
self.kf = tuning.kf
|
|
else:
|
|
self.pid = None
|
|
self.kf = 1.
|
|
|
|
def reset(self):
|
|
super().reset()
|
|
if self.pid is not None:
|
|
self.pid.reset()
|
|
|
|
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float,
|
|
curvature_limited: bool, lat_delay: float, calibrated_pose: Pose,
|
|
model_data: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace):
|
|
curvature_log = log.ControlsState.LateralCurvatureState.new_message()
|
|
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
|
|
error = desired_curvature - actual_curvature
|
|
|
|
if not active:
|
|
output_curvature = 0.0
|
|
curvature_log.active = False
|
|
if self.pid is not None:
|
|
self.pid.reset()
|
|
elif self.pid is None or CS.steeringPressed:
|
|
if self.pid is not None:
|
|
self.pid.reset()
|
|
output_curvature = self.kf * desired_curvature
|
|
curvature_log.active = True
|
|
else:
|
|
output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=self.kf * desired_curvature)
|
|
curvature_log.p = float(self.pid.p)
|
|
curvature_log.i = float(self.pid.i)
|
|
curvature_log.f = float(self.pid.f)
|
|
curvature_log.active = True
|
|
|
|
curvature_log.error = float(error)
|
|
curvature_log.actualCurvature = float(actual_curvature)
|
|
curvature_log.desiredCurvature = float(desired_curvature)
|
|
curvature_log.output = float(output_curvature)
|
|
curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS,
|
|
False, curvature_limited))
|
|
return 0.0, float(output_curvature), curvature_log
|