diff --git a/common/params.py b/common/params.py index d1e939a46..cb8bc70a4 100755 --- a/common/params.py +++ b/common/params.py @@ -165,6 +165,7 @@ keys = { "DragonAutoLCDelay": [TxType.PERSISTENT], "DragonBTG": [TxType.PERSISTENT], "DragonBootHotspot": [TxType.PERSISTENT], + "DragonEnableFastAccel": [TxType.PERSISTENT], } diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 0b709dde8..6c491b201 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -25,12 +25,18 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract _A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30] _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.] +# dragonpilot +_A_CRUISE_MIN_V_SPORT = [-3.0, -3.5, -4.0, -4.0, -4.0] + # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits _A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4] _A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4] _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] +# dragonpilot +_A_CRUISE_MAX_V_SPORT = [3.0, 3.5, 4.0, 4.0, 4.0] + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] @@ -39,13 +45,19 @@ _A_TOTAL_MAX_BP = [20., 40.] SPEED_PERCENTILE_IDX = 7 -def calc_cruise_accel_limits(v_ego, following): - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) +def calc_cruise_accel_limits(v_ego, following, fast_accel): + if not following and fast_accel: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) + else: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) if following: a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) else: - a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) + if fast_accel: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_SPORT) + else: + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) return np.vstack([a_cruise_min, a_cruise_max]) @@ -89,6 +101,7 @@ class Planner(): # dragonpilot self.dragon_slow_on_curve = True + self.dragon_fast_accel = False self.last_ts = 0. def choose_solution(self, v_cruise_setpoint, enabled): @@ -125,8 +138,9 @@ class Planner(): # dragonpilot # update variable status every 5 secs - if cur_time - self.last_ts > 5.: + if cur_time - self.last_ts >= 5.: self.dragon_slow_on_curve = False if self.params.get("DragonEnableSlowOnCurve", encoding='utf8') == "0" else True + self.dragon_fast_accel = True if self.params.get("DragonEnableFastAccel", encoding='utf8') == "1" else False self.last_ts = cur_time long_control_state = sm['controlsState'].longControlState @@ -160,7 +174,7 @@ class Planner(): # Calculate speed for normal cruise control if enabled and not self.first_loop: - accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following, self.dragon_fast_accel)] jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 87fb015c3..368ea2a08 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -64,6 +64,7 @@ default_conf = { 'DragonAutoLCDelay': 2, 'DragonBTG': 0, 'DragonBootHotspot': 0, + 'DragonEnableFastAccel': '0', } deprecated_conf = {