mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-08 12:55:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

some refactors

Browse Source
This commit is contained in:
Jason Wen
2024-07-01 11:51:01 -04:00
parent 6098dd300a
commit 6e6de6742e
2 changed files with 23 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+4 -2
View File
@@ -106,8 +106,10 @@ class LongitudinalPlanner:
def read_param(self):
try:
self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl"))
self.accel_controller.set_profile(int(self.params.get("AccelProfile")))
except AttributeError:
self.dynamic_experimental_controller = DynamicExperimentalController()
self.accel_controller = AccelController()
@staticmethod
def parse_model(model_msg, model_error):
@@ -129,7 +131,6 @@ class LongitudinalPlanner:
if self.param_read_counter % 50 == 0:
self.read_param()
self.param_read_counter += 1
self.accel_controller.set_profile(self.params.get("AccelProfile", encoding='utf-8'))
if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
else:
@@ -169,7 +170,8 @@ class LongitudinalPlanner:
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
else:
# blended, just give it max min (-3.5) and max from accel controller
accel_limits = accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
accel_limits = [ACCEL_MIN, ACCEL_MAX]
accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
if reset_state:
self.v_desired_filter.x = v_ego
selfdrive/controls/lib/sunnypilot/accel_controller.py
+19 -14
View File
@@ -25,11 +25,6 @@
from openpilot.common.numpy_fast import interp
DP_ACCEL_STOCK = 0
DP_ACCEL_ECO = 1
DP_ACCEL_NORMAL = 2
DP_ACCEL_SPORT = 3
# accel profile by @arne182 modified by cgw
_DP_CRUISE_MIN_V = [-1.00, -1.00, -0.99, -0.90, -0.90, -0.88, -0.88, -0.82]
_DP_CRUISE_MIN_V_ECO = [-1.00, -1.00, -0.98, -0.88, -0.88, -0.86, -0.86, -0.80]
@@ -42,22 +37,32 @@ _DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 2.8, 2.4, 1.4, 1.0, .89, .75, .50, .2]
_DP_CRUISE_MAX_BP = [0., 1., 6., 8., 11., 15., 20., 25., 30., 55.]
class DPAccel:
STOCK = 0
ECO = 1
NORMAL = 2
SPORT = 3
@classmethod
def accel_val(cls):
return cls.STOCK, cls.ECO, cls.NORMAL, cls.SPORT
class AccelController:
def __init__(self):
# self._params = Params()
self._profile = DP_ACCEL_STOCK
self._profile = DPAccel.STOCK
def set_profile(self, profile):
try:
self._profile = int(profile) if int(profile) in [DP_ACCEL_STOCK, DP_ACCEL_ECO, DP_ACCEL_NORMAL, DP_ACCEL_SPORT] else DP_ACCEL_STOCK
except:
self._profile = DP_ACCEL_STOCK
self._profile = profile if profile in DPAccel.accel_val() else DPAccel.STOCK
except (ValueError, TypeError):
self._profile = DPAccel.STOCK
def _dp_calc_cruise_accel_limits(self, v_ego):
if self._profile == DP_ACCEL_ECO:
if self._profile == DPAccel.ECO:
min_v = _DP_CRUISE_MIN_V_ECO
max_v = _DP_CRUISE_MAX_V_ECO
elif self._profile == DP_ACCEL_SPORT:
elif self._profile == DPAccel.SPORT:
min_v = _DP_CRUISE_MIN_V_SPORT
max_v = _DP_CRUISE_MAX_V_SPORT
else:
@@ -69,7 +74,7 @@ class AccelController:
return a_cruise_min, a_cruise_max
def get_accel_limits(self, v_ego, accel_limits):
return accel_limits if self._profile == DP_ACCEL_STOCK else self._dp_calc_cruise_accel_limits(v_ego)
return accel_limits if self._profile == DPAccel.STOCK else self._dp_calc_cruise_accel_limits(v_ego)
def is_enabled(self):
return self._profile != DP_ACCEL_STOCK
return self._profile != DPAccel.STOCK