diff --git a/opendbc_repo b/opendbc_repo index 938043e24a..d4c6f68c39 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 938043e24a0ff1a3a6f81a4c793822d4733f09ef +Subproject commit d4c6f68c39dbab1c2663bd3c72a0087f93858213 diff --git a/openpilot/selfdrive/controls/lib/drive_helpers.py b/openpilot/selfdrive/controls/lib/drive_helpers.py index 5392ff8875..497f8d1bdf 100644 --- a/openpilot/selfdrive/controls/lib/drive_helpers.py +++ b/openpilot/selfdrive/controls/lib/drive_helpers.py @@ -15,6 +15,9 @@ MAX_LATERAL_JERK = 5.0 # m/s^3 MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2 +def should_stop(v_ego: float, a_target: float) -> bool: + return bool(v_ego < 0.25 and a_target < 0.1) + def clamp(val, min_val, max_val): clamped_val = float(np.clip(val, min_val, max_val)) return clamped_val, clamped_val != val @@ -40,7 +43,7 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, b return float(new_curvature), limited_accel or limited_max_curv -def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.3): +def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL): if len(speeds) == len(t_idxs): v_now = speeds[0] a_now = accels[0] @@ -53,8 +56,7 @@ def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0. v_now = 0.0 v_target = 0.0 a_target = 0.0 - should_stop = (v_now < vEgoStopping and a_target < 0.1) - return a_target, should_stop + return a_target, should_stop(v_now, a_target) def curv_from_psis(psi_target, psi_rate, vego, action_t): vego = np.clip(vego, MIN_SPEED, np.inf) diff --git a/openpilot/selfdrive/controls/lib/longcontrol.py b/openpilot/selfdrive/controls/lib/longcontrol.py index 07abd0d5d3..437bcef777 100644 --- a/openpilot/selfdrive/controls/lib/longcontrol.py +++ b/openpilot/selfdrive/controls/lib/longcontrol.py @@ -62,7 +62,8 @@ class LongControl: output_accel = self.last_output_accel if output_accel > self.CP.stopAccel: output_accel = min(output_accel, 0.0) - output_accel -= self.CP.stoppingDecelRate * DT_CTRL + # TODO: can we just go straight to stopAccel? + output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop self.reset() else: # LongCtrlState.pid diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 884b4d00bc..116de14e0e 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -129,7 +129,7 @@ class LongitudinalPlanner: action_t = self.CP.longitudinalActuatorDelay + DT_MDL output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + action_t=action_t) output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop diff --git a/openpilot/tools/joystick/joystickd.py b/openpilot/tools/joystick/joystickd.py index a1c00746b6..ac2f99f67e 100755 --- a/openpilot/tools/joystick/joystickd.py +++ b/openpilot/tools/joystick/joystickd.py @@ -48,7 +48,7 @@ def joystickd_thread(): if CC.longActive: actuators.accel = 4.0 * float(np.clip(joystick_axes[0], -1, 1)) - actuators.longControlState = LongCtrlState.pid if sm['carState'].vEgo > CP.vEgoStopping else LongCtrlState.stopping + actuators.longControlState = LongCtrlState.pid if sm['carState'].vEgo > 0.1 else LongCtrlState.stopping CC.cruiseControl.resume = actuators.accel > 0.0 if CC.latActive: diff --git a/openpilot/tools/longitudinal_maneuvers/maneuversd.py b/openpilot/tools/longitudinal_maneuvers/maneuversd.py index 8cdc2f26cf..0e20840274 100755 --- a/openpilot/tools/longitudinal_maneuvers/maneuversd.py +++ b/openpilot/tools/longitudinal_maneuvers/maneuversd.py @@ -3,11 +3,11 @@ import numpy as np from dataclasses import dataclass from openpilot.cereal import messaging -from opendbc.car.structs import car from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.controls.lib.drive_helpers import should_stop @dataclass @@ -139,8 +139,8 @@ MANEUVERS = [ def main(): params = Params() - cloudlog.info("joystickd is waiting for CarParams") - CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + cloudlog.info("maneuversd is waiting for CarParams") + params.get("CarParams", block=True) sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'selfdriveState', 'modelV2'], poll='modelV2') pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'alertDebug']) @@ -178,7 +178,7 @@ def main(): pm.send('alertDebug', alert_msg) longitudinalPlan.aTarget = accel - longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2 + longitudinalPlan.shouldStop = should_stop(v_ego, accel) longitudinalPlan.allowBrake = True longitudinalPlan.allowThrottle = True