deaccel

ref

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -166,8 +166,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
     self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
     self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
-    self.accel_controller_active = bool(self.accel_controller.is_enabled() and not force_decel and
-                                        self.mpc.source == LongitudinalPlanSource.cruise)
+    self.accel_controller_active = self.is_accel_controller_active(force_decel)
 
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
 

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py

@@ -9,7 +9,6 @@ import numpy as np
 
 from openpilot.cereal import custom
 from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot import get_sanitize_int_param
 
 AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
@@ -35,15 +34,11 @@ CRUISE_DECEL_ACCEL = {  # m/s^2; comfort-first cruise deceleration target
 class AccelController:
   def __init__(self):
     self.params = Params()
-    self.frame = 0
-    self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
-    self._enabled = self.params.get_bool("AccelPersonalityEnabled")
+    self.update()
 
   def update(self) -> None:
-    self.frame += 1
-    if self.frame % int(1.0 / DT_MDL) == 0:
-      self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
-      self._enabled = self.params.get_bool("AccelPersonalityEnabled")
+    self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+    self._enabled = self.params.get_bool("AccelPersonalityEnabled")
 
   @property
   def profile(self) -> int:
@@ -61,5 +56,6 @@ class AccelController:
 
     response_time = CRUISE_DECEL_RESPONSE_TIME[self._profile]
     target_delta = v_target - v_ego
-    target_delta = max(target_delta / response_time, CRUISE_DECEL_ACCEL[self._profile] * response_time)
-    return float(v_ego + target_delta)
+    if target_delta < CRUISE_DECEL_ACCEL[self._profile] * response_time * 2.0:
+      return v_target
+    return float(v_ego + target_delta / response_time)

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py

@@ -134,28 +134,21 @@ class TestAccelController(OpenpilotTestCase):
   def test_profile_change_refreshes_ceiling(self):
     controller = self.set_profile(AccelProfile.normal)
     self.params.put("AccelPersonality", AccelProfile.sport, block=True)
-    controller.frame = int(1.0 / DT_MDL) - 1
     controller.update()
     index = MAX_ACCEL_BREAKPOINTS.index(10.0)
     assert controller.get_max_accel(10.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][index]
 
-  def test_params_refresh_once_per_second(self):
+  def test_params_refresh_every_update(self):
     controller = self.set_profile(AccelProfile.normal)
     self.params.put("AccelPersonality", AccelProfile.sport, block=True)
     controller.update()
-    assert controller.profile == AccelProfile.normal
-    controller.frame = int(1.0 / DT_MDL) - 1
-    controller.update()
     assert controller.profile == AccelProfile.sport
-    assert controller.frame == 0
 
   def test_enabled_param_refresh(self):
     controller = self.set_profile(AccelProfile.normal)
     self.params.put_bool("AccelPersonalityEnabled", False, block=True)
-    controller.frame = int(1.0 / DT_MDL) - 1
     controller.update()
     assert not controller.is_enabled()
-    assert controller.frame == 0
 
 
 class TestPlannerIntegration(OpenpilotTestCase):
@@ -241,7 +234,6 @@ class TestPlannerIntegration(OpenpilotTestCase):
     planner.a_cruise = planner.accel_controller.get_max_accel(v_ego)
 
     self.params.put("AccelPersonality", AccelProfile.eco, block=True)
-    planner.accel_controller.frame = int(1.0 / DT_MDL) - 1
     planner.accel_controller.update()
     ceiling = planner.get_max_accel_override(v_ego)
     previous = planner.a_cruise

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -70,6 +70,10 @@ class LongitudinalPlannerSP:
 
     return self.accel_controller.get_cruise_target(v_ego, v_target)
 
+  def is_accel_controller_active(self, force_decel: bool) -> bool:
+    return bool(self.accel_controller.is_enabled() and not force_decel and
+                self.mpc.source == MpcPlanSource.cruise)
+
   def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool:
     radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
     return radar_valid and ((source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or