diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5ecff1ccdf..ebfd0c734e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -121,7 +121,7 @@ class Controls: actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_controls, self.desired_curvature, - self.calibrated_pose, curvature_limited) # TODO what if not available + curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index f3f1bc5acf..807161735c 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -12,7 +12,7 @@ class LatControlAngle(LatControl): self.sat_check_min_speed = 5. self.use_steer_limited_by_controls = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 1f1199565e..4e4e0772fe 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3aef57baca..f8455b9bda 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -29,7 +29,6 @@ class LatControlTorque(LatControl): self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.torque_from_lateral_accel = CI.torque_from_lateral_accel() - self.use_steering_angle = self.torque_params.useSteeringAngle self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): @@ -37,22 +36,15 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 pid_log.active = False else: - actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY - if self.use_steering_angle: - actual_curvature = actual_curvature_vm - curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) - else: - assert calibrated_pose is not None - actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo - actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) - curvature_deadzone = 0.0 + curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0)) desired_lateral_accel = desired_curvature * CS.vEgo ** 2 # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index a01f013ddd..f32e20b1e5 100644 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -9,8 +9,6 @@ from opendbc.car.vehicle_model import VehicleModel from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle -from openpilot.selfdrive.locationd.helpers import Pose -from openpilot.common.mock.generators import generate_livePose class TestLatControl: @@ -30,18 +28,15 @@ class TestLatControl: params = log.LiveParametersData.new_message() - lp = generate_livePose() - pose = Pose.from_live_pose(lp.livePose) - # Saturate for curvature limited and controller limited for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True) assert lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False) assert not lac_log.saturated for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False) assert lac_log.saturated