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

dynamically change *coast @cydia2020

Browse Source
This commit is contained in:
rav4kumar
2024-08-03 21:47:38 -07:00
parent 58f994496c
commit c831b5f2af
2 changed files with 68 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+67 -5
View File
@@ -3,7 +3,8 @@ import os
import time
import numpy as np
from cereal import custom
from openpilot.common.numpy_fast import clip
from openpilot.common.numpy_fast import clip, interp
from openpilot.common.conversions import Conversions as CV
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
# WARNING: imports outside of constants will not trigger a rebuild
@@ -101,6 +102,61 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.
return np.interp(v_ego, x_vel, y_dist)
# multiplier for A_CHANGE_COST = 200.
def get_a_change_cost_multiplier(v_ego, v_lead0, v_lead1, personality=custom.LongitudinalPersonalitySP.standard):
if personality==custom.LongitudinalPersonalitySP.relaxed:
a_change_cost_multiplier_follow_distance = 1.0
elif personality==custom.LongitudinalPersonalitySP.standard:
a_change_cost_multiplier_follow_distance = 0.5
elif personality==custom.LongitudinalPersonalitySP.moderate:
a_change_cost_multiplier_follow_distance = 0.5
elif personality==custom.LongitudinalPersonalitySP.aggressive:
a_change_cost_multiplier_follow_distance = 0.1
else:
raise NotImplementedError("Longitudinal personality not supported")
# stolen from @KRKeegan
# values used for interpolation
# start with a small a_change_multiplier_values during interpolation to allow for faster change in accel
A_CHANGE_COST_MULTIPLIER_BP = [0., 10.] # vEgo, in m/s
A_CHANGE_COST_MULTIPLIER_V = [.05, 1.] # multiplier values
# when lead is pulling away, and speed is between 0 and 10 m/s, interpolate a_change_cost_multiplier_v_ego
a_change_cost_multiplier_v_ego = 1.
if (v_lead0 - v_ego > 1e-3) and (v_lead1 - v_ego > 1e-3):
a_change_cost_multiplier_v_ego = interp(v_ego, A_CHANGE_COST_MULTIPLIER_BP, A_CHANGE_COST_MULTIPLIER_V)
# get the minimum between a_change_multiplier based on driving personality, and a_change_multiplier based
# on v_ego
a_change_multiplier = min(a_change_cost_multiplier_follow_distance, a_change_cost_multiplier_v_ego)
# and pass it on as the final result
return a_change_multiplier
# multiplier for DANGER_ZONE_COST = 100.
def get_danger_zone_cost_multiplier(personality=custom.LongitudinalPersonalitySP.standard):
if personality==custom.LongitudinalPersonalitySP.relaxed:
return 1.6
elif personality==custom.LongitudinalPersonalitySP.standard:
return 1.3
elif personality==custom.LongitudinalPersonalitySP.moderate:
return 1.3
elif personality==custom.LongitudinalPersonalitySP.aggressive:
return 1.0
else:
raise NotImplementedError("Longitudinal personality not supported")
#def get_STOP_DISTANCE(personality=custom.LongitudinalPersonalitySP.standard):
# if personality==log.LongitudinalPersonality.relaxed:
# return 6.0
# elif personality==log.LongitudinalPersonality.standard:
# return 5.5
# elif personality==log.LongitudinalPersonality.aggressive:
# return 4.5
# else:
# raise NotImplementedError("dynamic stop distance not supported")
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
@@ -299,12 +355,16 @@ class LongitudinalMpc:
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, prev_accel_constraint=True, personality=custom.LongitudinalPersonalitySP.standard):
def set_weights(self, prev_accel_constraint=True, v_lead0 = 0., v_lead1 = 0., personality=custom.LongitudinalPersonalitySP.standard):
v_ego = self.x0[1]
jerk_factor = get_jerk_factor(personality)
a_change_cost_multiplier = get_a_change_cost_multiplier(v_ego, v_lead0, v_lead1, personality)
danger_zone_cost_multiplier = get_danger_zone_cost_multiplier(personality)
if self.mode == 'acc':
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
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]
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost_multiplier \
* a_change_cost, jerk_factor * J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST * danger_zone_cost_multiplier]
elif self.mode == 'blended':
a_change_cost = 40.0 if prev_accel_constraint else 0
cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
@@ -358,7 +418,7 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False):
def update(self, radarstate, v_cruise, prev_accel_constraint, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False):
v_ego = self.x0[1]
t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality)
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -366,6 +426,8 @@ class LongitudinalMpc:
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
self.set_weights(prev_accel_constraint=prev_accel_constraint, v_lead0=lead_xv_0[0, 1], v_lead1=lead_xv_1[0, 1], personality=personality)
# 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.
selfdrive/controls/lib/longitudinal_planner.py
+1 -1
View File
@@ -193,7 +193,7 @@ class LongitudinalPlanner:
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality)
self.mpc.update(sm['radarState'], v_cruise, prev_accel_constraint, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)