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

Acceleration profiles

Browse Source 
Added toggle to use DragonPilot's acceleration profiles.

Credit goes to DragonPilot!

https: //github.com/dragonpilot-community/dragonpilot
Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent 7308c1b35c
commit 3d6097b6a6
35 changed files with 141 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,7 +1,33 @@
import cereal.messaging as messaging
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.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel
# Acceleration profiles - Credit goes to the DragonPilot team!
# MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123]
A_CRUISE_MIN_BP_CUSTOM = [0., 2.0, 2.01, 11., 11.01, 18., 18.01, 28., 28.01, 33., 55.]
# MPH = [0., 6.71, 13.4, 17.9, 24.6, 33.6, 44.7, 55.9, 67.1, 123]
A_CRUISE_MAX_BP_CUSTOM = [0., 3, 6., 8., 11., 15., 20., 25., 30., 55.]
A_CRUISE_MIN_VALS_ECO_TUNE = [-0.480, -0.480, -0.40, -0.40, -0.40, -0.36, -0.32, -0.28, -0.28, -0.25, -0.25]
A_CRUISE_MAX_VALS_ECO_TUNE = [3.5, 3.3, 1.7, 1.1, .76, .62, .47, .36, .28, .09]
A_CRUISE_MIN_VALS_SPORT_TUNE = [-0.500, -0.500, -0.42, -0.42, -0.42, -0.42, -0.40, -0.35, -0.35, -0.30, -0.30]
A_CRUISE_MAX_VALS_SPORT_TUNE = [3.5, 3.5, 3.0, 2.6, 1.4, 1.0, 0.7, 0.6, .38, .2]
def get_min_accel_eco_tune(v_ego):
return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO_TUNE)
def get_max_accel_eco_tune(v_ego):
return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO_TUNE)
def get_min_accel_sport_tune(v_ego):
return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT_TUNE)
def get_max_accel_sport_tune(v_ego):
return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_TUNE)
class FrogPilotPlanner:
@@ -19,6 +45,14 @@ class FrogPilotPlanner:
v_cruise = v_cruise_kph * CV.KPH_TO_MS
v_ego = carState.vEgo
# Acceleration profiles
if self.acceleration_profile == 1:
self.accel_limits = [get_min_accel_eco_tune(v_ego), get_max_accel_eco_tune(v_ego)]
elif self.acceleration_profile in (2, 3):
self.accel_limits = [get_min_accel_sport_tune(v_ego), get_max_accel_sport_tune(v_ego)]
else:
self.accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego)
def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego):
@@ -45,3 +79,4 @@ class FrogPilotPlanner:
lateral_tune = params.get_bool("LateralTune")
longitudinal_tune = params.get_bool("LongitudinalTune")
self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0