diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 8e446e0e81..c15518a7a1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py b/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py index 4b9391f547..d3e966bfb1 100644 --- a/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py +++ b/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): diff --git a/selfdrive/controls/lib/turn_speed_controller.py b/selfdrive/controls/lib/turn_speed_controller.py index b2818c6c96..28ecf10593 100644 --- a/selfdrive/controls/lib/turn_speed_controller.py +++ b/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): diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py index 63d3c1df9a..5d90a5578a 100644 --- a/selfdrive/controls/lib/vision_turn_controller.py +++ b/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):