deacel

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -35,8 +35,12 @@ def get_max_accel(v_ego):
 def get_coast_accel(pitch):
   return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
 
-def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
-  max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
+def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
+                      max_accel_override=None):
+  if max_accel_override is not None:
+    max_accel = max_accel_override
+  else:
+    max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
   if not e2e:
     a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
     a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
@@ -141,11 +145,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     is_e2e = self.is_e2e(sm)
 
+    max_accel_override = self.get_max_accel_override(v_ego)
+    v_cruise = self.get_cruise_target_override(v_ego, v_cruise, force_decel)
     a_cruise_prev = self.a_cruise
     gated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
-                                    self.CP, self.dt, accel_coast, self.allow_throttle)
+                                    self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override)
     ungated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
-                                      self.CP, self.dt, accel_coast, True)
+                                      self.CP, self.dt, accel_coast, True, max_accel_override)
     self.a_cruise = self.arbitrate_cruise_candidate(
       sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source,
       allow_throttle=self.allow_throttle, e2e=is_e2e, force_decel=force_decel,
@@ -157,11 +163,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     if is_e2e:
       candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
 
-    output_a_target, self.mpc.source, self.output_should_stop = min(candidates, key=lambda candidate: candidate[0])
-    output_a_target = self.accel_controller.limit_accel(output_a_target, v_ego)
-
+    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 = self.is_accel_controller_active(force_decel, self.output_a_target)
+    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

@@ -19,6 +19,16 @@ MAX_ACCEL_PROFILES = {
   AccelProfile.normal: [1.90, 1.70, 1.42, 0.99, 0.80, 0.66, 0.52],
   AccelProfile.sport:  [2.00, 2.00, 1.86, 1.30, 1.02, 0.86, 0.72],
 }
+CRUISE_DECEL_RESPONSE_TIME = {  # seconds
+  AccelProfile.eco: 4.0,
+  AccelProfile.normal: 3.5,
+  AccelProfile.sport: 3.0,
+}
+CRUISE_DECEL_ACCEL = {  # m/s^2; comfort-first cruise deceleration target
+  AccelProfile.eco: -0.35,
+  AccelProfile.normal: -0.50,
+  AccelProfile.sport: -0.65,
+}
 
 
 class AccelController:
@@ -40,7 +50,12 @@ class AccelController:
   def get_max_accel(self, v_ego: float) -> float:
     return float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
 
-  def limit_accel(self, accel: float, v_ego: float) -> float:
-    if not self.is_enabled() or accel <= 0.0:
-      return accel
-    return min(accel, self.get_max_accel(v_ego))
+  def get_cruise_target(self, v_ego: float, v_target: float) -> float:
+    if not np.isfinite(v_target) or v_target <= 0.0 or v_target >= v_ego:
+      return v_target
+
+    response_time = CRUISE_DECEL_RESPONSE_TIME[self._profile]
+    target_delta = v_target - v_ego
+    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/longitudinal_planner.py

@@ -56,8 +56,21 @@ class LongitudinalPlannerSP:
 
     return False
 
-  def is_accel_controller_active(self, force_decel: bool, accel_target: float) -> bool:
-    return bool(self.accel_controller.is_enabled() and not force_decel and accel_target >= 0.0)
+  def get_max_accel_override(self, v_ego: float) -> float | None:
+    if not self.accel_controller.is_enabled():
+      return None
+
+    return self.accel_controller.get_max_accel(v_ego)
+
+  def get_cruise_target_override(self, v_ego: float, v_target: float, force_decel: bool) -> float:
+    if not self.accel_controller.is_enabled() or force_decel or self.source != LongitudinalPlanSource.cruise:
+      return v_target
+
+    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)