diff --git a/cereal b/cereal index d81d86e7cd..20b65eeb1f 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit d81d86e7cd83d1eb40314964a4d194231381d557 +Subproject commit 20b65eeb1f6c580cdd7d63e53639f4fc48bc2f56 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 698c149515..78bcf8a9b0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -172,7 +172,6 @@ class Controls: self.last_actuators = car.CarControl.Actuators.new_message() self.steer_limited = False self.desired_curvature = 0.0 - self.desired_curvature_rate = 0.0 self.experimental_mode = False self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False @@ -611,13 +610,10 @@ class Controls: actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) # Steering PID loop and lateral MPC - self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo, - lat_plan.psis, - lat_plan.curvatures, - lat_plan.curvatureRates) + self.desired_curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures) actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited, self.desired_curvature, - self.desired_curvature_rate, self.sm['liveLocationKalman']) + self.sm['liveLocationKalman']) actuators.curvature = self.desired_curvature else: lac_log = log.ControlsState.LateralDebugState.new_message() @@ -787,7 +783,6 @@ class Controls: controlsState.active = self.active controlsState.curvature = curvature controlsState.desiredCurvature = self.desired_curvature - controlsState.desiredCurvatureRate = self.desired_curvature_rate controlsState.state = self.state controlsState.engageable = not self.events.contains(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index b728f1114c..9a5d1779fc 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -163,11 +163,10 @@ def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): +def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures): if len(psis) != CONTROL_N: psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N - curvature_rates = [0.0]*CONTROL_N v_ego = max(MIN_SPEED, v_ego) # TODO this needs more thought, use .2s extra for now to estimate other delays @@ -182,16 +181,12 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): desired_curvature = 2 * average_curvature_desired - current_curvature_desired # This is the "desired rate of the setpoint" not an actual desired rate - desired_curvature_rate = curvature_rates[0] max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 - safe_desired_curvature_rate = clip(desired_curvature_rate, - -max_curvature_rate, - max_curvature_rate) safe_desired_curvature = clip(desired_curvature, current_curvature_desired - max_curvature_rate * DT_MDL, current_curvature_desired + max_curvature_rate * DT_MDL) - return safe_desired_curvature, safe_desired_curvature_rate + return safe_desired_curvature def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 723af7f806..fddb331ccb 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -17,7 +17,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index d363295f0c..329c486eb9 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -11,7 +11,7 @@ class LatControlAngle(LatControl): super().__init__(CP, CI) self.sat_check_min_speed = 5. - def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): 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 c673159ebb..9e6160838b 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, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): 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 f46ab9eb6c..65fd1b51c5 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -36,7 +36,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 5faad914cf..248abf1d7b 100755 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -34,7 +34,7 @@ class TestLatControl(unittest.TestCase): llk = gen_llk() for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, False, 1, 0, llk) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, llk) self.assertTrue(lac_log.saturated)