mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-09-04 15:23:42 +08:00
Merge pull request #4 from infiniteCable2/curv_corr
Curvature Correction Fix
This commit is contained in:
infiniteCable2
+1
-1
Submodule opendbc_repo updated: 7115553b65...adecffed2b
@@ -18,7 +18,6 @@ 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_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature, CURVATURE_SATURATION_THRESHOLD
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW
|
||||
@@ -58,9 +57,8 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
|
||||
self.steer_limited_by_safety = False
|
||||
self.curvature = 0.0
|
||||
self.curvature_no_roll = 0.0
|
||||
self.roll_compensation = 0.0
|
||||
self.desired_curvature = 0.0
|
||||
self.roll = 0.0
|
||||
|
||||
self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController")
|
||||
self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl")
|
||||
@@ -74,10 +72,9 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
self.LoC = LongControl(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 or
|
||||
self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED):
|
||||
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
|
||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
|
||||
self.LaC = LatControlCurvature(self.CP, self.CP_SP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'torque':
|
||||
@@ -110,8 +107,7 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
|
||||
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
|
||||
self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
|
||||
self.curvature_no_roll = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, 0.0)
|
||||
self.roll = lp.roll
|
||||
self.roll_compensation = -self.VM.roll_compensation(lp.roll, CS.vEgo)
|
||||
|
||||
# Update Torque Params
|
||||
if self.CP.lateralTuning.which() == 'torque':
|
||||
@@ -169,10 +165,10 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
new_desired_curvature = self.smooth_steer.update(new_desired_curvature)
|
||||
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
|
||||
|
||||
steer, steeringAngleDeg, curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
self.steer_limited_by_safety, self.desired_curvature,
|
||||
self.calibrated_pose, curvature_limited) # TODO what if not available
|
||||
actuators.curvature = float(curvature)
|
||||
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,
|
||||
self.calibrated_pose, curvature_limited) # TODO what if not available
|
||||
actuators.torque = float(steer)
|
||||
actuators.steeringAngleDeg = float(steeringAngleDeg)
|
||||
# Ensure no NaNs/Infs
|
||||
@@ -191,13 +187,13 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
CS = self.sm['carState']
|
||||
|
||||
CC.curvatureControllerActive = self.enable_curvature_controller # for car controller curvature correction activation
|
||||
CC.currentCurvatureNoRoll = self.curvature_no_roll
|
||||
CC.rollDEPRECATED = self.roll # for lateral iso limit calculation
|
||||
CC.steerLimited = self.steer_limited_by_safety
|
||||
|
||||
# Orientation and angle rates can be useful for carcontroller
|
||||
# Only calibrated (car) frame is relevant for the carcontroller
|
||||
CC.currentCurvature = self.curvature
|
||||
CC.rollCompensation = self.roll_compensation
|
||||
|
||||
if self.calibrated_pose is not None:
|
||||
CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
|
||||
CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
|
||||
@@ -226,11 +222,10 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
|
||||
if self.sm['selfdriveState'].active:
|
||||
CO = self.sm['carOutput']
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or
|
||||
self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED):
|
||||
self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
|
||||
STEER_ANGLE_SATURATION_THRESHOLD
|
||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
|
||||
self.steer_limited_by_safety = abs(CC.actuators.curvature - CO.actuatorsOutput.curvature) > CURVATURE_SATURATION_THRESHOLD
|
||||
else:
|
||||
self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
|
||||
|
||||
@@ -254,10 +249,9 @@ class Controls(ControlsExt, ModelStateBase):
|
||||
(self.sm['selfdriveState'].state == State.softDisabling))
|
||||
|
||||
lat_tuning = self.CP.lateralTuning.which()
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or
|
||||
self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED):
|
||||
cs.lateralControlState.angleState = lac_log
|
||||
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
|
||||
cs.lateralControlState.curvatureStateDEPRECATED = lac_log
|
||||
elif lat_tuning == 'pid':
|
||||
cs.lateralControlState.pidState = lac_log
|
||||
elif lat_tuning == 'torque':
|
||||
|
||||
@@ -34,4 +34,4 @@ class LatControlAngle(LatControl):
|
||||
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
|
||||
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
angle_log.steeringAngleDesiredDeg = angle_steers_des
|
||||
return 0, float(angle_steers_des), desired_curvature, angle_log
|
||||
return 0, float(angle_steers_des), angle_log
|
||||
|
||||
@@ -9,6 +9,7 @@ CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m
|
||||
class LatControlCurvature(LatControl):
|
||||
def __init__(self, CP, CP_SP, CI):
|
||||
super().__init__(CP, CP_SP, CI)
|
||||
assert False
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
|
||||
@@ -45,4 +45,4 @@ class LatControlPID(LatControl):
|
||||
pid_log.output = float(output_torque)
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
|
||||
|
||||
return output_torque, angle_steers_des, desired_curvature, pid_log
|
||||
return output_torque, angle_steers_des, pid_log
|
||||
|
||||
@@ -97,4 +97,4 @@ class LatControlTorque(LatControl):
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
|
||||
|
||||
# TODO left is positive in this convention
|
||||
return -output_torque, 0.0, desired_curvature, pid_log
|
||||
return -output_torque, 0.0, pid_log
|
||||
|
||||
Reference in New Issue
Block a user