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

Acceleration/deceleration profiles

Browse Source 
Added toggle to use DragonPilot's acceleration/deceleration 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-03-06 17:50:06 -07:00
parent f01d5fb25a
commit 121fd2d246
30 changed files with 259 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
selfdrive/controls/controlsd.py
View File

@@ -35,6 +35,8 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.system.hardware import HARDWARE
from openpilot.system.version import get_short_branch
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
@@ -195,6 +197,8 @@ class Controls:
if not self.disengage_on_accelerator:
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX
# read params
self.is_metric = self.params.get_bool("IsMetric")
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
@@ -706,7 +710,7 @@ class Controls:
if not self.joystick_mode:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS)
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS, self.frogpilot_variables)
t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
@@ -988,6 +992,7 @@ class Controls:
lateral_tune = self.params.get_bool("LateralTune")
longitudinal_tune = self.params.get_bool("LongitudinalTune")
self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3
quality_of_life = self.params.get_bool("QOLControls")

12
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -17,11 +17,16 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED
LON_MPC_STEP = 0.2 # first step is 0.2s
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
ACCEL_MAX_PLUS = [ACCEL_MAX, 4.0]
ACCEL_MAX_PLUS_BP = [0., CRUISING_SPEED]
# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
@@ -107,17 +112,18 @@ class LongitudinalPlanner:
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
accel_limits = frogpilot_planner.accel_limits
if self.mpc.mode == 'acc':
accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
else:
accel_limits = [ACCEL_MIN, ACCEL_MAX]
accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
if reset_state:
self.v_desired_filter.x = v_ego
# Slowly ramp up the acceleration to prevent jerky behaviors from a standstill
max_acceleration = min(interp(v_ego, ACCEL_MAX_PLUS_BP, ACCEL_MAX_PLUS), accel_limits[1])
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], max_acceleration)
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))