mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-07 04:53:47 +08:00
Harald Schäfer
d2e4202aaf
* Add gas override boost, mici boost bar, and Rivian maximum set speed * Refactor acceleration boost and gate accumulation on model limit * Fade acceleration boost with model acceleration * Limit boost buildup per gas override * Latch boost eligibility before gas override * Move model limiting check into AccelBoost * Use DT_MDL directly in AccelBoost * Clarify acceleration boost state names * Scale acceleration boost by speed instead of decaying it * Enable Experimental Mode by default * Scale boost buildup with the application speed ramp * Fix boost serialization and model-limit eligibility * Remove boost UI and Experimental Mode default changes * Combine boost update and application Original-upstream-commit: ab8c84c3989bab34969d83e78c5fad3ee5b95112 Upstream: https://github.com/commaai/openpilot/commit/ab8c84c3989bab34969d83e78c5fad3ee5b95112
40 lines
1.5 KiB
Python
40 lines
1.5 KiB
Python
import numpy as np
|
|
from openpilot.common.constants import CV
|
|
from openpilot.common.realtime import DT_MDL
|
|
|
|
ACCEL_BOOST_MAX = 0.2
|
|
ACCEL_BOOST_RATE = 0.025
|
|
ACCEL_BOOST_PER_OVERRIDE = 0.05
|
|
ACCEL_BOOST_MIN_SPEED = 10 * CV.MPH_TO_MS
|
|
|
|
|
|
class AccelBoost:
|
|
def __init__(self):
|
|
self.total_boost = 0.0
|
|
self.boost_this_override = 0.0
|
|
self.boost_eligible = False
|
|
|
|
def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
|
|
enabled = sm['selfdriveState'].enabled
|
|
gas_pressed = sm['carState'].gasPressed
|
|
v_ego = sm['carState'].vEgo
|
|
speed_scale = np.interp(v_ego, [ACCEL_BOOST_MIN_SPEED, 2 * ACCEL_BOOST_MIN_SPEED], [0.0, 1.0])
|
|
boost_scale = speed_scale * np.interp(output_a_target_e2e, [-1.0, -0.5, 5.0], [0.0, 1.0, 1.0], right=0.0)
|
|
model_limited = (sm['selfdriveState'].experimentalMode and
|
|
output_a_target_e2e + self.total_boost * boost_scale < min(output_a_target_mpc, a_cruise) - 0.1)
|
|
|
|
if not enabled or not gas_pressed:
|
|
self.boost_this_override = 0.0
|
|
# Latch eligibility before the override changes the lead plan.
|
|
self.boost_eligible = enabled and model_limited
|
|
|
|
if not enabled:
|
|
self.total_boost = 0.0
|
|
elif gas_pressed and self.boost_eligible:
|
|
increase = min(ACCEL_BOOST_RATE * DT_MDL * speed_scale,
|
|
ACCEL_BOOST_PER_OVERRIDE - self.boost_this_override, ACCEL_BOOST_MAX - self.total_boost)
|
|
self.total_boost += increase
|
|
self.boost_this_override += increase
|
|
|
|
return output_a_target_e2e + self.total_boost * boost_scale
|