Longitudinal: Fix planner not getting up to set speed

selfdrive/controls/lib/longitudinal_planner.py

@@ -153,8 +153,8 @@ class LongitudinalPlanner:
       v_cruise = 0.0
 
     # Get active solutions for custom long mpc.
-    self.cruise_source, v_cruise_sol = self.cruise_solutions(
-      prev_accel_constraint and (self.CP.openpilotLongitudinalControl or not self.CP.pcmCruiseSpeed),
+    v_cruise = 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
@@ -245,21 +245,14 @@ class LongitudinalPlanner:
     self.speed_limit_controller.update(enabled, v_ego, a_ego, sm, v_cruise, self.events)
     self.turn_speed_controller.update(enabled, v_ego, sm, v_cruise)
 
+    v_tsc_target = self.vision_turn_controller.v_target if self.vision_turn_controller.is_active else 255
+    slc_target = self.speed_limit_controller.speed_limit_offseted if self.speed_limit_controller.is_active else 255
+    m_tsc_target = self.turn_speed_controller.v_target if self.turn_speed_controller.is_active else 255
+
     # Pick solution with the lowest velocity target.
-    v_solutions = {'cruise': v_cruise}
+    v_solutions = min(v_tsc_target, slc_target, m_tsc_target)
 
-    if self.vision_turn_controller.is_active:
-      v_solutions['turn'] = self.vision_turn_controller.v_target
-
-    if self.speed_limit_controller.is_active:
-      v_solutions['limit'] = self.speed_limit_controller.speed_limit_offseted
-
-    if self.turn_speed_controller.is_active:
-      v_solutions['turnlimit'] = self.turn_speed_controller.v_target
-
-    source = min(v_solutions, key=v_solutions.get)
-
-    return source, v_solutions[source]
+    return v_solutions
 
   def e2e_events(self, sm):
     e2e_long_status = sm['e2eLongStateSP'].status

selfdrive/controls/lib/sunnypilot/speed_limit_controller.py

@@ -96,7 +96,7 @@ class SpeedLimitController:
 
   @property
   def is_active(self):
-    return self.state in ACTIVE_STATES
+    return self.state in ACTIVE_STATES and self._is_enabled
 
   @property
   def speed_limit_offseted(self):

selfdrive/controls/lib/turn_speed_controller.py

@@ -69,7 +69,7 @@ class TurnSpeedController:
 
   @property
   def is_active(self):
-    return self.state > TurnSpeedControlState.tempInactive
+    return self.state > TurnSpeedControlState.tempInactive and self.enabled
 
   @property
   def v_target(self):

selfdrive/controls/lib/vision_turn_controller.py

@@ -67,7 +67,7 @@ class VisionTurnController:
 
   @property
   def is_active(self):
-    return self._state != VisionTurnControllerState.disabled
+    return self._state != VisionTurnControllerState.disabled and self._is_enabled
 
   @property
   def current_lat_acc(self):
@@ -105,7 +105,7 @@ class VisionTurnController:
 
     # Get the target velocity for the maximum curve
     self._v_target = (TARGET_LAT_A / max_curve) ** 0.5
-    self._v_target = max(self.v_target, MIN_TARGET_V)
+    self._v_target = max(self._v_target, MIN_TARGET_V)
 
   def _state_transition(self):
     if not self._op_enabled or not self._is_enabled or self._gas_pressed or self._v_ego < MIN_TARGET_V:
@@ -114,11 +114,11 @@ class VisionTurnController:
 
     # DISABLED
     if self.state == VisionTurnControllerState.disabled:
-      if self._v_cruise > self.v_target:
+      if self._v_cruise > self._v_target:
         self.state = VisionTurnControllerState.turning
     # TURNING
     elif self.state == VisionTurnControllerState.turning:
-      if not (self._v_cruise > self.v_target):
+      if not (self._v_cruise > self._v_target):
         self.state = VisionTurnControllerState.disabled
 
   def update(self, enabled, v_ego, v_cruise, sm):