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

13
selfdrive/frogpilot/functions/frogpilot_planner.py
View File

@@ -1,9 +1,12 @@
import cereal.messaging as messaging
import numpy as np
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import interp
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, V_CRUISE_MAX
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel
from openpilot.selfdrive.modeld.constants import ModelConstants
# Acceleration profiles - Credit goes to the DragonPilot team!
# MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123]
@@ -34,6 +37,8 @@ class FrogPilotPlanner:
def __init__(self, params):
self.v_cruise = 0
self.x_desired_trajectory = np.zeros(CONTROL_N)
self.update_frogpilot_params(params)
def update(self, sm, mpc):
@@ -55,6 +60,9 @@ class FrogPilotPlanner:
self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego)
self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution)
self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N]
def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego):
v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0)
return v_cruise - v_ego_diff
@@ -71,12 +79,15 @@ class FrogPilotPlanner:
frogpilot_longitudinal_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
frogpilotLongitudinalPlan = frogpilot_longitudinal_plan_send.frogpilotLongitudinalPlan
frogpilotLongitudinalPlan.distances = self.x_desired_trajectory.tolist()
pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send)
def update_frogpilot_params(self, params):
self.is_metric = params.get_bool("IsMetric")
lateral_tune = params.get_bool("LateralTune")
self.average_desired_curvature = params.get_bool("AverageCurvature") and lateral_tune
longitudinal_tune = params.get_bool("LongitudinalTune")
self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0