mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-28 04:23:43 +08:00
Harald Schäfer
0d57eadd74
* Remove obsolete Chill mode code * Remove obsolete Chill mode test variants * Use measured ego motion for stock ACC planning * Restore roadmap entry * Preserve stock ACC path colors and camera view * remove allowThrottle * Deprecate unused experimentalMode field --------- Co-authored-by: Shane Smiskol <shane@smiskol.com>
39 lines
1.5 KiB
Python
39 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 = 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
|