|
|
@ -2,7 +2,7 @@ import math |
|
|
|
|
|
|
|
|
|
|
|
from cereal import car, log |
|
|
|
from cereal import car, log |
|
|
|
from openpilot.common.conversions import Conversions as CV |
|
|
|
from openpilot.common.conversions import Conversions as CV |
|
|
|
from openpilot.common.numpy_fast import clip, interp |
|
|
|
from openpilot.common.numpy_fast import clip |
|
|
|
from openpilot.common.realtime import DT_CTRL |
|
|
|
from openpilot.common.realtime import DT_CTRL |
|
|
|
|
|
|
|
|
|
|
|
# WARNING: this value was determined based on the model's training distribution, |
|
|
|
# WARNING: this value was determined based on the model's training distribution, |
|
|
@ -141,16 +141,6 @@ class VCruiseHelper: |
|
|
|
self.v_cruise_cluster_kph = self.v_cruise_kph |
|
|
|
self.v_cruise_cluster_kph = self.v_cruise_kph |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def apply_center_deadzone(error, deadzone): |
|
|
|
|
|
|
|
if (error > - deadzone) and (error < deadzone): |
|
|
|
|
|
|
|
error = 0. |
|
|
|
|
|
|
|
return error |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def rate_limit(new_value, last_value, dw_step, up_step): |
|
|
|
|
|
|
|
return clip(new_value, last_value + dw_step, last_value + up_step) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature): |
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature): |
|
|
|
v_ego = max(MIN_SPEED, v_ego) |
|
|
|
v_ego = max(MIN_SPEED, v_ego) |
|
|
|
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 |
|
|
|
max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 |
|
|
@ -161,17 +151,6 @@ def clip_curvature(v_ego, prev_curvature, new_curvature): |
|
|
|
return safe_desired_curvature |
|
|
|
return safe_desired_curvature |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, |
|
|
|
|
|
|
|
torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float: |
|
|
|
|
|
|
|
friction_interp = interp( |
|
|
|
|
|
|
|
apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), |
|
|
|
|
|
|
|
[-friction_threshold, friction_threshold], |
|
|
|
|
|
|
|
[-torque_params.friction, torque_params.friction] |
|
|
|
|
|
|
|
) |
|
|
|
|
|
|
|
friction = float(friction_interp) if friction_compensation else 0.0 |
|
|
|
|
|
|
|
return friction |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: |
|
|
|
def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: |
|
|
|
# ToDo: Try relative error, and absolute speed |
|
|
|
# ToDo: Try relative error, and absolute speed |
|
|
|
if len(modelV2.temporalPose.trans): |
|
|
|
if len(modelV2.temporalPose.trans): |
|
|
|