diff --git a/cereal/deprecated.capnp b/cereal/deprecated.capnp index 45ce25c68..5e4b0b4de 100644 --- a/cereal/deprecated.capnp +++ b/cereal/deprecated.capnp @@ -760,18 +760,6 @@ struct LateralLQRState @0x9024e2d790c82ade { steeringAngleDesiredDeg @6 :Float32; } -struct LateralCurvatureState @0xad9d8095c06f7c61 { - active @0 :Bool; - actualCurvature @1 :Float32; - desiredCurvature @2 :Float32; - error @3 :Float32; - p @4 :Float32; - i @5 :Float32; - f @6 :Float32; - output @7 :Float32; - saturated @8 :Bool; -} - struct LateralPlannerSolution @0x84caeca5a6b4acfe { x @0 :List(Float32); y @1 :List(Float32); diff --git a/cereal/log.capnp b/cereal/log.capnp index d12cd6cdc..b7202dfcf 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -818,7 +818,7 @@ struct ControlsState @0x97ff69c53601abf1 { debugState @59 :LateralDebugState; torqueState @60 :LateralTorqueState; - curvatureStateDEPRECATED @65 :Deprecated.LateralCurvatureState; + curvatureState @65 :LateralCurvatureState; lqrStateDEPRECATED @55 :Deprecated.LateralLQRState; indiStateDEPRECATED @52 :Deprecated.LateralINDIState; } @@ -867,6 +867,18 @@ struct ControlsState @0x97ff69c53601abf1 { saturated @3 :Bool; } + struct LateralCurvatureState @0xad9d8095c06f7c61 { + active @0 :Bool; + actualCurvature @1 :Float32; + desiredCurvature @2 :Float32; + error @3 :Float32; + p @4 :Float32; + i @5 :Float32; + f @6 :Float32; + output @7 :Float32; + saturated @8 :Bool; + } + deprecated :group { vEgo @0 :Float32; vEgoRaw @32 :Float32; diff --git a/opendbc_repo b/opendbc_repo index 560b75d6b..7343ffe79 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 560b75d6bb5ba14dc594727e5cc7c7bbf0ed0f3c +Subproject commit 7343ffe79d4ce3244cb0faead4cfbebc3ffc9d87 diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 62ddadef8..dd0826e51 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -186,7 +186,7 @@ class TestCarModelBase(unittest.TestCase): # make sure car params are within a valid range self.assertGreater(self.CP.mass, 1) - if self.CP.steerControlType != SteerControlType.angle: + if self.CP.steerControlType not in (SteerControlType.angle, SteerControlType.curvature): tuning = self.CP.lateralTuning.which() if tuning == 'pid': self.assertTrue(len(self.CP.lateralTuning.pid.kpV)) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e1d9650d7..cdd00f723 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -15,6 +15,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD +from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS @@ -53,6 +54,8 @@ class Controls: self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL) + elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature: + self.LaC = LatControlCurvature(self.CP, self.CI, DT_CTRL) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL) elif self.CP.lateralTuning.which() == 'torque': @@ -124,11 +127,14 @@ class Controls: lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS actuators.curvature = self.desired_curvature - steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, - curvature_limited, lat_delay) + steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, + self.steer_limited_by_safety, self.desired_curvature, + curvature_limited, lat_delay) actuators.torque = float(steer) - actuators.steeringAngleDeg = float(steeringAngleDeg) + if self.CP.steerControlType == car.CarParams.SteerControlType.curvature: + actuators.curvature = float(lateral_output) + else: + actuators.steeringAngleDeg = float(lateral_output) # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: attr = getattr(actuators, p) @@ -199,6 +205,8 @@ class Controls: lat_tuning = self.CP.lateralTuning.which() if self.CP.steerControlType == car.CarParams.SteerControlType.angle: cs.lateralControlState.angleState = lac_log + elif self.CP.steerControlType == car.CarParams.SteerControlType.curvature: + cs.lateralControlState.curvatureState = lac_log elif lat_tuning == 'pid': cs.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py new file mode 100644 index 000000000..cd2d8e328 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -0,0 +1,58 @@ +import math + +from cereal import log +from openpilot.common.pid import PIDController +from openpilot.selfdrive.controls.lib.latcontrol import LatControl +from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE + +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': + ct = CP.lateralTuning.pid + self.pid = PIDController((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV), + pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt) + self.kf = ct.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, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay): + 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: + # no PID or override: feedforward only + 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 diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/mici/onroad/torque_bar.py index 406e6ff71..6a1d12b6c 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/mici/onroad/torque_bar.py @@ -163,7 +163,7 @@ class TorqueBar(Widget): return # torque line - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': + if ui_state.sm['controlsState'].lateralControlState.which() in ('angleState', 'curvatureState'): controls_state = ui_state.sm['controlsState'] car_state = ui_state.sm['carState'] live_parameters = ui_state.sm['liveParameters']