diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 399679d03c..b557ef659b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -18,9 +18,17 @@ enum LongitudinalPersonalitySP { relaxed @3; } +enum AccelerationProfile { + stock @0; + eco @1; + normal @2; + sport @3; +} + struct ControlsStateSP @0x81c2f05a394cf4af { lateralState @0 :Text; personality @8 :LongitudinalPersonalitySP; + accelProfile @9 :AccelerationProfile; lateralControlState :union { indiState @1 :LateralINDIState; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21bf73cf9a..c4ed4bd71b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -187,6 +187,8 @@ class Controls: model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution + self.accel_profile = self.read_accel_profile_param() + self.can_log_mono_time = 0 self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0) @@ -858,6 +860,7 @@ class Controls: controlsStateSP.lateralState = lat_tuning controlsStateSP.personality = self.personality + controlsStateSP.accelProfile = self.accel_profile if self.enable_nnff and lat_tuning == 'torque': controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp @@ -906,11 +909,18 @@ class Controls: except (ValueError, TypeError): return custom.LongitudinalPersonalitySP.standard + def read_accel_profile_param(self): + try: + return int(self.params.get("AccelProfile")) + except (ValueError, TypeError): + return custom.AccelerationProfile.stock + def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl self.personality = self.read_personality_param() + self.accel_profile = self.read_accel_profile_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 29e21378ce..c74f93c006 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -106,7 +106,6 @@ 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() @@ -157,7 +156,7 @@ class LongitudinalPlanner: accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] # override accel using Accel Controller - if self.accel_controller.is_enabled(): + if self.accel_controller.is_enabled(accel_profile=sm['controlsStateSP'].accelProfile): # get min, max from accel controller min_limit, max_limit = self.accel_controller.get_accel_limits(v_ego, accel_limits) if self.mpc.mode == 'acc': diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py index 95e5174660..ec006d3310 100644 --- a/selfdrive/controls/lib/sunnypilot/accel_controller.py +++ b/selfdrive/controls/lib/sunnypilot/accel_controller.py @@ -23,8 +23,11 @@ # Last updated: June 5, 2024 +from cereal import custom from openpilot.common.numpy_fast import interp +AccelProfile = custom.AccelerationProfile + # 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] @@ -37,32 +40,15 @@ _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._profile = DPAccel.STOCK - - def set_profile(self, profile: int): - try: - self._profile = profile if profile in DPAccel.accel_val() else DPAccel.STOCK - except (ValueError, TypeError): - self._profile = DPAccel.STOCK + self._profile = AccelProfile.stock def _dp_calc_cruise_accel_limits(self, v_ego: float): - if self._profile == DPAccel.ECO: + if self._profile == AccelProfile.eco: min_v = _DP_CRUISE_MIN_V_ECO max_v = _DP_CRUISE_MAX_V_ECO - elif self._profile == DPAccel.SPORT: + elif self._profile == AccelProfile.sport: min_v = _DP_CRUISE_MIN_V_SPORT max_v = _DP_CRUISE_MAX_V_SPORT else: @@ -75,7 +61,8 @@ class AccelController: return a_cruise_min, a_cruise_max def get_accel_limits(self, v_ego: float, accel_limits: list[float]): - return accel_limits if self._profile == DPAccel.STOCK else self._dp_calc_cruise_accel_limits(v_ego) + return accel_limits if self._profile == AccelProfile.stock else self._dp_calc_cruise_accel_limits(v_ego) - def is_enabled(self): - return self._profile != DPAccel.STOCK + def is_enabled(self, accel_profile: int = AccelProfile.stock): + self._profile = accel_profile + return self._profile != AccelProfile.stock