mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-09-12 19:33:43 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/sunnypilot/sunnypilot

Browse Source
This commit is contained in:
infiniteCable2
2026-08-19 09:03:54 +02:00
parent 161c5dae48 ba29a38507
commit 17efbfb550
11 changed files with 277 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/longitudinal_planner.py
+10 -12
View File
@@ -57,9 +57,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
@@ -73,10 +72,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
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.output_v_target = 0.0
@@ -114,7 +112,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
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))
@@ -122,11 +121,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
# Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
# Get new v_cruise and a_target from Smart Cruise Control and Speed Limit Assist
v_cruise, self.output_a_target = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.output_a_target, v_cruise)
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)
@@ -139,7 +138,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
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,
@@ -164,7 +163,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
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
self.output_v_target = get_speed_from_plan(self.v_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t)