diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 0daece94..b91703bc 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -3,6 +3,36 @@ import numpy as np from openpilot.selfdrive.controls.lib.longitudinal_planner import ACCEL_MIN, get_max_accel +A_CRUISE_MIN_ECO = ACCEL_MIN / 2 +A_CRUISE_MIN_SPORT = ACCEL_MIN * 2 + + # MPH = [0.0, 11, 22, 34, 45, 56, 89] +A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.] +A_CRUISE_MAX_VALS_ECO = [2.0, 1.5, 1.0, 0.8, 0.6, 0.4, 0.2] +A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.5, 1.0, 0.8, 0.6] + +ACCELERATION_PROFILES = { + "STANDARD": 0, + "ECO": 1, + "SPORT": 2, + "SPORT_PLUS": 3 +} + +DECELERATION_PROFILES = { + "STANDARD": 0, + "ECO": 1, + "SPORT": 2 +} + +def get_max_accel_eco(v_ego): + return np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO) + +def get_max_accel_sport(v_ego): + return np.interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT) + +def get_max_allowed_accel(v_ego): + return np.interp(v_ego, [0., 5., 20.], [4.0, 4.0, 2.0]) # ISO 15622:2018 + class FrogPilotAcceleration: def __init__(self, FrogPilotPlanner): self.frogpilot_planner = FrogPilotPlanner @@ -11,9 +41,38 @@ class FrogPilotAcceleration: self.min_accel = 0 def update(self, v_ego, sm, frogpilot_toggles): - self.max_accel = get_max_accel(v_ego) + eco_gear = sm["frogpilotCarState"].ecoGear + sport_gear = sm["frogpilotCarState"].sportGear + + if (eco_gear or sport_gear) and frogpilot_toggles.map_acceleration: + if eco_gear: + self.max_accel = get_max_accel_eco(v_ego) + else: + if frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT"]: + self.max_accel = get_max_accel_sport(v_ego) + else: + self.max_accel = get_max_allowed_accel(v_ego) + else: + if frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["ECO"]: + self.max_accel = get_max_accel_eco(v_ego) + elif frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT"]: + self.max_accel = get_max_accel_sport(v_ego) + elif frogpilot_toggles.acceleration_profile == ACCELERATION_PROFILES["SPORT_PLUS"]: + self.max_accel = get_max_allowed_accel(v_ego) + else: + self.max_accel = get_max_accel(v_ego) if self.frogpilot_planner.tracking_lead: self.min_accel = ACCEL_MIN + elif (eco_gear or sport_gear) and frogpilot_toggles.map_deceleration: + if eco_gear: + self.min_accel = A_CRUISE_MIN_ECO + else: + self.min_accel = A_CRUISE_MIN_SPORT else: - self.min_accel = ACCEL_MIN + if frogpilot_toggles.deceleration_profile == DECELERATION_PROFILES["ECO"]: + self.min_accel = A_CRUISE_MIN_ECO + elif frogpilot_toggles.deceleration_profile == DECELERATION_PROFILES["SPORT"]: + self.min_accel = A_CRUISE_MIN_SPORT + else: + self.min_accel = ACCEL_MIN diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index d8b2663b..69e1d391 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -314,6 +314,8 @@ class CarInterfaceBase(ABC): self.CS.out = ret # FrogPilot variables + fp_ret.ecoGear |= ret.gearShifter == GearShifter.eco + fp_ret.sportGear |= ret.gearShifter == GearShifter.sport return ret, fp_ret diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index db060cbb..e46dba88 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -204,6 +204,10 @@ class CarState(CarStateBase): # FrogPilot variables fp_ret = custom.FrogPilotCarState.new_message() + if not self.CP.flags & ToyotaFlags.SECOC.value: + fp_ret.ecoGear = cp.vl["GEAR_PACKET"]["ECON_ON"] == 1 + fp_ret.sportGear = cp.vl["GEAR_PACKET"]["SPORT_ON_2" if self.CP.flags & ToyotaFlags.NO_DSU else "SPORT_ON"] == 1 + ret.buttonEvents = buttonEvents return ret, fp_ret diff --git a/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_2017.dbc b/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_2017.dbc index 4524b1ea..338756e8 100644 --- a/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_2017.dbc +++ b/opendbc_repo/opendbc/dbc/generator/toyota/_toyota_2017.dbc @@ -217,6 +217,7 @@ BO_ 956 GEAR_PACKET: 8 XXX SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX SG_ B_GEAR_ENGAGED : 41|1@0+ (1,0) [0|1] "" XXX SG_ DRIVE_ENGAGED : 47|1@0+ (1,0) [0|1] "" XXX + SG_ SPORT_ON_2 : 55|1@0+ (1,0) [0|1] "" XXX BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX @@ -522,6 +523,7 @@ VAL_ 898 AIR_CONDITIONER_STATE 20 "CLIMATE OFF"; VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted"; VAL_ 921 DISTANCE_LINES 0 "not displayed" 1 "close" 2 "medium" 3 "far"; VAL_ 956 SPORT_ON 0 "off" 1 "on"; +VAL_ 956 SPORT_ON_2 0 "off" 1 "on"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on"; VAL_ 956 SPORT_GEAR 1 "S1" 2 "S2" 3 "S3" 4 "S4" 5 "S5" 6 "S6"; diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c3d15537..329b4e1b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -15,6 +15,8 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog +from openpilot.frogpilot.common.frogpilot_variables import MINIMUM_LATERAL_ACCELERATION + LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] @@ -43,7 +45,11 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): # The lookup table for turns should also be updated if we do this a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) + + if abs(a_y) > MINIMUM_LATERAL_ACCELERATION: + a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) + else: + a_x_allowed = a_target[1] return [a_target[0], min(a_target[1], a_x_allowed)] @@ -116,7 +122,7 @@ class LongitudinalPlanner: prev_accel_constraint = not (reset_state or sm['carState'].standstill) if mode == 'acc': - accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] + accel_clip = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) else: