sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

long planner: formatting

Browse Source
pull/32972/head
Shane Smiskol 10 months ago
parent 11db7b683b
commit 71dd1e2ff6
1 changed files with 15 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 31
      selfdrive/controls/lib/longitudinal_planner.py

31
selfdrive/controls/lib/longitudinal_planner.py
Unescape View File

@ -45,22 +45,21 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
def get_accel_from_plan(CP, speeds, accels): def get_accel_from_plan(CP, speeds, accels):
if len(speeds) == CONTROL_N: if len(speeds) == CONTROL_N:
v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds) v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels) a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels)
v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds) v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds) v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
else: else:
v_target = 0.0 v_target = 0.0
v_target_now = 0.0 v_target_1sec = 0.0
v_target_1sec = 0.0 a_target = 0.0
a_target = 0.0 should_stop = (v_target < CP.vEgoStopping and
should_stop = (v_target < CP.vEgoStopping and v_target_1sec < CP.vEgoStopping)
v_target_1sec < CP.vEgoStopping) return a_target, should_stop
return a_target, should_stop
class LongitudinalPlanner: class LongitudinalPlanner:
@ -82,8 +81,8 @@ class LongitudinalPlanner:
@staticmethod @staticmethod
def parse_model(model_msg, model_error): def parse_model(model_msg, model_error):
if (len(model_msg.position.x) == ModelConstants.IDX_N and if (len(model_msg.position.x) == ModelConstants.IDX_N and
len(model_msg.velocity.x) == ModelConstants.IDX_N and len(model_msg.velocity.x) == ModelConstants.IDX_N and
len(model_msg.acceleration.x) == ModelConstants.IDX_N): len(model_msg.acceleration.x) == ModelConstants.IDX_N):
x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error
a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)

Write Preview
Loading…
Cancel
Save