diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5bc9ff8..0e277b8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -99,7 +99,7 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - accel_limits = [A_CRUISE_MIN, sm['frogpilotPlan'].maxAcceleration] + accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] if self.mpc.mode == 'acc': accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) else: diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 1712621..4f15371 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -21,16 +21,26 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_ GearShifter = car.CarState.GearShifter # Acceleration profiles - Credit goes to the DragonPilot team! + # MPH = [0., 18, 36, 63, 94] +A_CRUISE_MIN_BP_CUSTOM = [0., 8., 16., 28., 42.] # MPH = [0., 6.71, 13.4, 17.9, 24.6, 33.6, 44.7, 55.9, 89.5] A_CRUISE_MAX_BP_CUSTOM = [0., 3, 6., 8., 11., 15., 20., 25., 40.] +A_CRUISE_MIN_VALS_ECO = [-0.001, -0.010, -0.28, -0.56, -0.56] A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30] +A_CRUISE_MIN_VALS_SPORT = [-0.50, -0.52, -0.55, -0.57, -0.60] A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, 0.75, 0.65, 0.6] +def get_min_accel_eco(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO) + def get_max_accel_eco(v_ego): return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO) +def get_min_accel_sport(v_ego): + return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT) + def get_max_accel_sport(v_ego): return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT) @@ -67,6 +77,15 @@ class FrogPilotPlanner: else: self.max_accel = get_max_accel(v_ego) + if controlsState.experimentalMode: + self.min_accel = ACCEL_MIN + elif frogpilot_toggles.deceleration_profile == 1: + self.min_accel = get_min_accel_eco(v_ego) + elif frogpilot_toggles.deceleration_profile == 2: + self.min_accel = get_min_accel_sport(v_ego) + else: + self.min_accel = A_CRUISE_MIN + check_lane_width = frogpilot_toggles.lane_detection if check_lane_width and v_ego >= frogpilot_toggles.minimum_lane_change_speed: self.lane_width_left = float(calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])) @@ -130,6 +149,7 @@ class FrogPilotPlanner: frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.maxAcceleration = self.max_accel + frogpilotPlan.minAcceleration = self.min_accel frogpilotPlan.vCruise = float(self.v_cruise)