diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index edbb6dbaa583a4..b7c48b7ae3f91f 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -26,10 +26,19 @@ LOW_SPEED_Y = [15, 13, 10, 5] LOW_SPEED_Y_NN = [12, 3, 1, 0] +LAT_PLAN_MIN_IDX = 5 + +def get_predicted_lateral_jerk(lat_accels, t_diffs): + # compute finite difference between subsequent model_data.acceleration.y values + # this is just two calls of np.diff followed by an element-wise division + lat_accel_diffs = np.diff(lat_accels) + lat_jerk = lat_accel_diffs / t_diffs + # return as python list + return lat_jerk.tolist() + def sign(x): return 1.0 if x > 0.0 else (-1.0 if x < 0.0 else 0.0) -LAT_PLAN_MIN_IDX = 5 def get_lookahead_value(future_vals, current_val): if len(future_vals) == 0: return current_val @@ -88,6 +97,10 @@ def __init__(self, CP, CI): self.roll_deque = deque(maxlen=history_check_frames[0]) self.error_deque = deque(maxlen=history_check_frames[0]) self.past_future_len = len(self.past_times) + len(self.nn_future_times) + + # precompute time differences between ModelConstants.T_IDXS + self.t_diffs = np.diff(ModelConstants.T_IDXS) + self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3 # Setup adjustable parameters @@ -147,8 +160,8 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, desir setpoint = desired_lateral_accel + low_speed_factor * desired_curvature measurement = actual_lateral_accel + low_speed_factor * actual_curvature - model_planner_good = None not in [lat_plan, model_data] and all([len(i) >= CONTROL_N for i in [model_data.orientation.x, lat_plan.curvatures]]) - if self.use_nn and model_planner_good: + model_good = model_data is not None and len(model_data.orientation.x) >= CONTROL_N + if self.use_nn and model_good: # update past data roll = params.roll pitch = self.pitch.update(llk.calibratedOrientationNED.value[1]) @@ -159,8 +172,9 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, desir # prepare "look-ahead" desired lateral jerk lookahead = interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v) friction_upper_idx = next((i for i, val in enumerate(ModelConstants.T_IDXS) if val > lookahead), 16) - lookahead_curvature_rate = get_lookahead_value(list(lat_plan.curvatureRates)[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_curvature_rate) - lookahead_lateral_jerk = lookahead_curvature_rate * CS.vEgo**2 + predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs) + desired_lateral_jerk = (interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, model_data.acceleration.y) - actual_lateral_accel) / 0.3 + lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk) # prepare past and future values # adjust future times to account for longitudinal acceleration @@ -168,7 +182,7 @@ def update(self, active, CS, VM, params, steer_limited, desired_curvature, desir past_rolls = [self.roll_deque[min(len(self.roll_deque)-1, i)] for i in self.history_frame_offsets] future_rolls = [roll_pitch_adjust(interp(t, ModelConstants.T_IDXS, model_data.orientation.x) + roll, interp(t, ModelConstants.T_IDXS, model_data.orientation.y) + pitch) for t in adjusted_future_times] past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque)-1, i)] for i in self.history_frame_offsets] - future_planned_lateral_accels = [interp(t, ModelConstants.T_IDXS[:CONTROL_N], lat_plan.curvatures) * CS.vEgo ** 2 for t in adjusted_future_times] + future_planned_lateral_accels = [interp(t, ModelConstants.T_IDXS[:CONTROL_N], model_data.acceleration.y) for t in adjusted_future_times] # compute NN error response. lookahead_lateral_jerk = apply_deadzone(lookahead_lateral_jerk, self.lat_jerk_deadzone)