diff --git a/opendbc_repo b/opendbc_repo index 7115553b6..adecffed2 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 7115553b65531feccff128edd892b6842018b9e5 +Subproject commit adecffed2b1bca59d5d9e5b2a1c3a6bb54c16397 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5aa2491b9..bd4735815 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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': diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 206078bdf..787bd2dbe 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index e56e8c3f7..0544085c6 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -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() diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 22eaff789..fd79c29bd 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index af0fe14bc..e4554ae46 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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