brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Calculate average desired curvature using planned distance

Browse Source 
Added toggle to calculate average desired curvature using planned distance instead of the car's speed.

Credit goes to Pfeiferj!

https: //github.com/pfeiferj
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 8baf713117
commit b4550a13ba
8 changed files with 35 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
selfdrive/controls/controlsd.py
View File

@@ -649,7 +649,9 @@ class Controls:
self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
lat_plan.psis,
lat_plan.curvatures,
lat_plan.curvatureRates)
lat_plan.curvatureRates,
frogpilot_long_plan.distances,
self.average_desired_curvature)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited, self.desired_curvature,
self.desired_curvature_rate, self.sm['liveLocationKalman'])
@@ -940,6 +942,8 @@ class Controls:
if hasattr(obj, 'update_frogpilot_params'):
obj.update_frogpilot_params(self.params)
self.average_desired_curvature = self.params.get_bool("AverageCurvature")
longitudinal_tune = self.params.get_bool("LongitudinalTune")
self.sport_plus = self.params.get_int("AccelerationProfile") == 3 and longitudinal_tune

11
selfdrive/controls/lib/drive_helpers.py
View File

@@ -16,6 +16,7 @@ V_CRUISE_INITIAL = 40
V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors
MIN_DIST = 0.001
MIN_SPEED = 1.0
CONTROL_N = 17
CAR_ROTATION_RADIUS = 0.0
@@ -163,11 +164,12 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
if len(psis) != CONTROL_N:
def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates, distances, average_desired_curvature):
if len(psis) != CONTROL_N or len(distances) != CONTROL_N:
psis = [0.0]*CONTROL_N
curvatures = [0.0]*CONTROL_N
curvature_rates = [0.0]*CONTROL_N
distances = [0.0]*CONTROL_N
v_ego = max(MIN_SPEED, v_ego)
# TODO this needs more thought, use .2s extra for now to estimate other delays
@@ -178,7 +180,10 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
# psi to calculate a simple linearization of desired curvature
current_curvature_desired = curvatures[0]
psi = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
average_curvature_desired = psi / (v_ego * delay)
# Pfeiferj's #28118 PR - https://github.com/commaai/openpilot/pull/28118
distance = interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances)
distance = max(MIN_DIST, distance)
average_curvature_desired = psi / distance if average_desired_curvature else psi / (v_ego * delay)
desired_curvature = 2 * average_curvature_desired - current_curvature_desired
# This is the "desired rate of the setpoint" not an actual desired rate

5
selfdrive/controls/lib/lateral_planner.py
View File

@@ -35,7 +35,10 @@ class LateralPlanner:
if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
if frogpilot_planner.average_desired_curvature:
car_speed = np.array(md.velocity.x) - get_speed_error(md, v_ego_car)
else:
car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
self.v_ego = self.v_plan[0]
self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate])

2
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -230,6 +230,7 @@ class LongitudinalMpc:
# self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
self.solver.reset()
# self.solver.options_set('print_level', 2)
self.x_solution = np.zeros(N+1)
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.prev_a = np.array(self.a_solution)
@@ -442,6 +443,7 @@ class LongitudinalMpc:
for i in range(N):
self.u_sol[i] = self.solver.get(i, 'u')
self.x_solution = self.x_sol[:,0]
self.v_solution = self.x_sol[:,1]
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]