mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-23 20:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

super fast take off!!!

Browse Source
This commit is contained in:
Jason Wen
2024-07-23 23:30:01 -04:00
parent 3f25d6f530
commit c288a7792c
1 changed files with 15 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+15 -4
View File
@@ -105,6 +105,17 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
def get_stopped_equivalence_factor_krkeegen(v_lead, v_ego):
# KRKeegan this offset rapidly decreases the following distance when the lead pulls
# away, resulting in an early demand for acceleration.
v_diff_offset = 0
if np.all(v_lead - v_ego > 0):
v_diff_offset = ((v_lead - v_ego) * 1.)
v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2)
v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0)
distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
return distance
def get_safe_obstacle_distance(v_ego, t_follow):
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
@@ -302,11 +313,11 @@ class LongitudinalMpc:
def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
jerk_factor = get_jerk_factor(personality)
if self.mode == 'acc':
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 40
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
elif self.mode == 'blended':
a_change_cost = 40.0 if prev_accel_constraint else 0
a_change_cost = 40.0 if prev_accel_constraint else 40
cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, 50.0]
else:
@@ -369,8 +380,8 @@ class LongitudinalMpc:
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
# and then treat that as a stopped car/obstacle at this new distance.
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_0[:,1], v_ego)
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor_krkeegen(lead_xv_1[:,1], v_ego)
cruise_target_e2ex = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
e2e_xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])