diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 047ed435b5..1c0e2419db 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py b/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py index f70f7cd536..7897cb78ed 100644 --- a/selfdrive/controls/lib/sunnypilot/speed_limit_controller.py +++ b/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) diff --git a/selfdrive/controls/lib/turn_speed_controller.py b/selfdrive/controls/lib/turn_speed_controller.py index 9fc35913b1..7d1051d314 100644 --- a/selfdrive/controls/lib/turn_speed_controller.py +++ b/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() diff --git a/selfdrive/controls/lib/vision_turn_controller.py b/selfdrive/controls/lib/vision_turn_controller.py index e3e4286cc4..07a6c790ce 100644 --- a/selfdrive/controls/lib/vision_turn_controller.py +++ b/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()