Enhanced Speed Control: Remove acceleration solution

selfdrive/controls/lib/longitudinal_planner.py

@@ -151,13 +151,13 @@ class LongitudinalPlanner:
     if force_slow_decel:
       v_cruise = 0.0
 
-    # Get acceleration and active solutions for custom long mpc.
-    self.cruise_source, a_min_sol, v_cruise_sol = self.cruise_solutions(
+    # Get active solutions for custom long mpc.
+    self.cruise_source, v_cruise_sol = self.cruise_solutions(
       not reset_state and (self.CP.openpilotLongitudinalControl or not self.CP.pcmCruiseSpeed), self.v_desired_filter.x,
       self.a_desired, v_cruise, sm)
 
     # clip limits, cannot init MPC outside of bounds
-    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol)
+    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
 
     self.mpc.set_weights(prev_accel_constraint, personality=self.personality)
@@ -247,24 +247,20 @@ class LongitudinalPlanner:
     self.turn_speed_controller.update(enabled, v_ego, a_ego, sm)
 
     # Pick solution with the lowest velocity target.
-    a_solutions = {'cruise': float("inf")}
     v_solutions = {'cruise': v_cruise}
 
     if self.vision_turn_controller.is_active:
-      a_solutions['turn'] = self.vision_turn_controller.a_target
       v_solutions['turn'] = self.vision_turn_controller.v_turn
 
     if self.speed_limit_controller.is_active:
-      a_solutions['limit'] = self.speed_limit_controller.a_target
       v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted
 
     if self.turn_speed_controller.is_active:
-      a_solutions['turnlimit'] = self.turn_speed_controller.a_target
       v_solutions['turnlimit'] = self.turn_speed_controller.speed_limit
 
     source = min(v_solutions, key=v_solutions.get)
 
-    return source, a_solutions[source], v_solutions[source]
+    return source, v_solutions[source]
 
   def e2e_events(self, sm):
     e2e_long_status = sm['e2eLongStateSP'].status

selfdrive/controls/lib/sunnypilot/speed_limit_controller.py

@@ -42,7 +42,6 @@ class SpeedLimitController:
     self._state = SpeedLimitControlState.inactive
     self._state_prev = SpeedLimitControlState.inactive
     self._gas_pressed = False
-    self._a_target = 0.
 
     self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8'))
     self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8'))
@@ -75,10 +74,6 @@ class SpeedLimitController:
       SpeedLimitControlState.preActive: self.get_current_acceleration_as_target,
     }
 
-  @property
-  def a_target(self):
-    return self._a_target if self.is_active else self._a_ego
-
   @property
   def state(self):
     return self._state
@@ -262,12 +257,6 @@ class SpeedLimitController:
     """ In active state, aim to keep speed constant around control time horizon """
     return self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
 
-  def _update_solution(self):
-    a_target = self.acceleration_solutions[self.state]()
-
-    # Keep solution limited.
-    self._a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC)
-
   def _update_events(self, events):
     if not self.is_active:
       if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \
@@ -301,5 +290,4 @@ class SpeedLimitController:
     self._update_params(CP)
     self._update_calculations()
     self._state_transition()
-    self._update_solution()
     self._update_events(events)

selfdrive/controls/lib/turn_speed_controller.py

@@ -52,12 +52,6 @@ class TurnSpeedController():
 
     self._next_speed_limit_prev = 0.
 
-    self._a_target = 0.
-
-  @property
-  def a_target(self):
-    return self._a_target if self.is_active else self._a_ego
-
   @property
   def state(self):
     return self._state
@@ -209,26 +203,6 @@ class TurnSpeedController():
       if self._v_offset < LIMIT_SPEED_OFFSET_TH and self.distance > 0.:
         self.state = TurnSpeedControlState.adapting
 
-  def _update_solution(self):
-    # inactive or tempInactive state
-    if self.state <= TurnSpeedControlState.tempInactive:
-      # Preserve current values
-      a_target = self._a_ego
-    # adapting
-    elif self.state == TurnSpeedControlState.adapting:
-      # When adapting we target to achieve the speed limit on the distance.
-      a_target = (self.speed_limit**2 - self._v_ego**2) / (2. * self.distance)
-      a_target = np.clip(a_target, LIMIT_MIN_ACC, LIMIT_MAX_ACC)
-    # active
-    elif self.state == TurnSpeedControlState.active:
-      # When active we are trying to keep the speed constant around the control time horizon.
-      # but under constrained acceleration limits since we are in a turn.
-      a_target = self._v_offset / ModelConstants.T_IDXS[CONTROL_N]
-      a_target = np.clip(a_target, _ACTIVE_LIMIT_MIN_ACC, _ACTIVE_LIMIT_MAX_ACC)
-
-    # update solution values.
-    self._a_target = a_target
-
   def update(self, enabled, v_ego, a_ego, sm):
     self._op_enabled = enabled
     self._v_ego = v_ego
@@ -240,4 +214,3 @@ class TurnSpeedController():
     self._update_params()
     self._update_calculations()
     self._state_transition(sm)
-    self._update_solution()

selfdrive/controls/lib/vision_turn_controller.py

@@ -103,7 +103,6 @@ class VisionTurnController():
     self._v_cruise_setpoint = 0.
     self._v_ego = 0.
     self._a_ego = 0.
-    self._a_target = 0.
     self._v_overshoot = 0.
     self._state = VisionTurnControllerState.disabled
 
@@ -121,16 +120,12 @@ class VisionTurnController():
         self._reset()
     self._state = value
 
-  @property
-  def a_target(self):
-    return self._a_target if self.is_active else self._a_ego
-
   @property
   def v_turn(self):
     if not self.is_active:
       return self._v_cruise_setpoint
     return self._v_overshoot if self._lat_acc_overshoot_ahead \
-      else self._v_ego + self._a_target * _NO_OVERSHOOT_TIME_HORIZON
+      else self._v_ego
 
   @property
   def is_active(self):
@@ -260,34 +255,6 @@ class VisionTurnController():
       elif self._current_lat_acc < _FINISH_LAT_ACC_TH:
         self.state = VisionTurnControllerState.disabled
 
-  def _update_solution(self):
-    # DISABLED
-    if self.state == VisionTurnControllerState.disabled:
-      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
-        # the smooth deceleration.
-      a_target = self._a_ego
-    # ENTERING
-    elif self.state == VisionTurnControllerState.entering:
-      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
-      a_target = interp(self._max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
-      if self._lat_acc_overshoot_ahead:
-        # when overshooting, target the acceleration needed to achieve the overshoot speed at
-        # the required distance
-        a_target = min((self._v_overshoot**2 - self._v_ego**2) / (2 * self._v_overshoot_distance), a_target)
-      _debug(f'TVC Entering: Overshooting: {self._lat_acc_overshoot_ahead}')
-      _debug(f'    Decel: {a_target:.2f}, target v: {self.v_turn * CV.MS_TO_KPH}')
-    # TURNING
-    elif self.state == VisionTurnControllerState.turning:
-      # When turning we provide a target acceleration that is comfortable for the lateral accelearation felt.
-      a_target = interp(self._current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
-    # LEAVING
-    elif self.state == VisionTurnControllerState.leaving:
-      # When leaving we provide a comfortable acceleration to regain speed.
-      a_target = _LEAVING_ACC
-
-    # update solution values.
-    self._a_target = a_target
-
   def update(self, enabled, v_ego, a_ego, v_cruise_setpoint, sm):
     self._op_enabled = enabled
     self._gas_pressed = sm['carState'].gasPressed
@@ -298,4 +265,3 @@ class VisionTurnController():
     self._update_params()
     self._update_calculations(sm)
     self._state_transition()
-    self._update_solution()