mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-21 04:53:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

cruise planner: fix decel jerk from cruise (#38653)

Browse Source 
* cruise planner: fix decel jerk from cruise

* dead variable
This commit is contained in:
Harald Schäfer
2026-08-18 14:15:20 -07:00
committed by GitHub
parent b7657f6553
commit 3d09a47a47
1 changed files with 8 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/longitudinal_planner.py
+8 -10
View File
@@ -47,9 +47,8 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt,
max_accel = min(max_accel, coast_limit)
target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
if not e2e:
j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)
target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt))
j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)
target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt))
return target_accel
@@ -62,10 +61,9 @@ class LongitudinalPlanner:
self.dt = dt
self.allow_throttle = True
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
self.a_cruise = 0.0
self.output_a_target = 0.0
self.a_cruise = init_a
self.output_a_target = init_a
self.output_should_stop = False
self.v_desired_trajectory = np.zeros(CONTROL_N)
@@ -100,7 +98,8 @@ class LongitudinalPlanner:
if reset_state:
self.v_desired_filter.x = v_ego
self.a_desired = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
self.output_a_target = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
self.a_cruise = self.output_a_target
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -109,7 +108,7 @@ class LongitudinalPlanner:
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target)
self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -122,7 +121,7 @@ class LongitudinalPlanner:
cloudlog.info("FCW triggered")
# Save starting point for next iteration
a_prev = self.a_desired
a_prev = self.output_a_target
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
output_a_target_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
@@ -145,7 +144,6 @@ class LongitudinalPlanner:
self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
self.a_desired = float(self.output_a_target)
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
def publish(self, sm, pm):