diff --git a/opendbc/gm_global_a_powertrain_volt.dbc b/opendbc/gm_global_a_powertrain_volt.dbc index 98e7cfbd8..9361c7569 100644 --- a/opendbc/gm_global_a_powertrain_volt.dbc +++ b/opendbc/gm_global_a_powertrain_volt.dbc @@ -188,6 +188,9 @@ BO_ 497 BCMGeneralPlatformStatus: 8 K9_BCM SG_ SystemBackUpPowerMode : 5|2@0+ (1,0) [0|3] "" XXX SG_ ParkBrakeSwActive : 36|1@0+ (1,0) [0|3] "" XXX +BO_ 500 SportMode: 6 XXX + SG_ SportMode : 15|1@0+ (1,0) [0|1] "" XXX + BO_ 501 ECMPRDNL2: 8 K20_ECM SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index efbcccc86..0b4a7cfae 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -249,6 +249,7 @@ BO_ 956 GEAR_PACKET: 8 XXX SG_ SPORT_GEAR_ON : 33|1@0+ (1,0) [0|1] "" XXX SG_ SPORT_GEAR : 38|3@0+ (1,0) [0|7] "" XXX SG_ ECON_ON : 40|1@0+ (1,0) [0|1] "" XXX + SG_ SPORT_ON_2 : 55|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 @@ -531,6 +532,7 @@ 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"; VAL_ 956 ECON_ON 0 "off" 1 "on"; +VAL_ 956 SPORT_ON_2 0 "off" 1 "on"; VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on"; VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on"; VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines"; diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 06e68a97e..b1728bd39 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -180,6 +180,8 @@ class CarState(CarStateBase): self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] + fp_ret.sportGear = pt_cp.vl["SportMode"]["SportMode"] == 1 + return ret, fp_ret @staticmethod @@ -219,6 +221,7 @@ class CarState(CarStateBase): ("EBCMFrictionBrakeStatus", 20), ("PSCMSteeringAngle", 100), ("ECMAcceleratorPos", 80), + ("SportMode", 0), ] if CP.carFingerprint in SDGM_CAR: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 37bc6ee47..f1f32bdca 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -55,6 +55,9 @@ class CarState(CarStateBase): # FrogPilot variables self.main_enabled = False + self.active_mode = 0 + self.drive_mode_prev = 0 + def update(self, cp, cp_cam, frogpilot_toggles): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam, frogpilot_toggles) @@ -270,6 +273,15 @@ class CarState(CarStateBase): self.prev_distance_button = self.distance_button self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST + drive_mode = cp.vl["DRIVE_MODE"]["DRIVE_MODE2"] + + if drive_mode != 0 and drive_mode != self.drive_mode_prev: + self.active_mode = drive_mode if drive_mode in (2, 3) else 1 + self.drive_mode_prev = drive_mode + + fp_ret.ecoGear = self.active_mode == 2 + fp_ret.sportGear = self.active_mode == 3 + self.lkas_previously_enabled = self.lkas_enabled self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"] @@ -359,6 +371,7 @@ class CarState(CarStateBase): ("CRUISE_BUTTONS_ALT", 50), ("BLINKERS", 4), ("DOORS_SEATBELTS", 4), + ("DRIVE_MODE", 0), ] if CP.flags & HyundaiFlags.EV: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 569bb0e87..aac83c4d6 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -420,6 +420,8 @@ class CarInterfaceBase(ABC): # Add any additional frogpilotCarStates fp_ret.alwaysOnLateralDisabled = self.always_on_lateral_disabled fp_ret.distanceLongPressed = self.frogpilot_distance_functions(frogpilot_toggles) + fp_ret.ecoGear |= ret.gearShifter == GearShifter.eco + fp_ret.sportGear |= ret.gearShifter == GearShifter.sport fp_ret.trafficModeActive = frogpilot_toggles.traffic_mode and self.traffic_mode_active # copy back for next iteration diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 11adbb58e..5cabf2374 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -191,6 +191,9 @@ class CarState(CarStateBase): self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] # FrogPilot CarState functions + 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 + if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: self.lkas_previously_enabled = self.lkas_enabled message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"] diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 7351a14cb..ef500a52b 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -111,18 +111,30 @@ class FrogPilotPlanner: self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles) def set_acceleration(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles): + eco_gear = frogpilotCarState.ecoGear + sport_gear = frogpilotCarState.sportGear + if self.tracking_lead and frogpilot_toggles.aggressive_acceleration: self.max_accel = clip(self.lead_one.aLeadK, get_max_accel_sport_plus(v_ego), 2.0 if v_ego >= 20 else 4.0) - elif frogpilot_toggles.acceleration_profile == 1: - self.max_accel = get_max_accel_eco(v_ego) - elif frogpilot_toggles.acceleration_profile == 2: - self.max_accel = get_max_accel_sport(v_ego) - elif frogpilot_toggles.acceleration_profile == 3: - self.max_accel = get_max_accel_sport_plus(v_ego) - elif controlsState.experimentalMode: - self.max_accel = ACCEL_MAX + elif frogpilot_toggles.map_acceleration and (eco_gear or sport_gear): + if eco_gear: + self.max_accel = get_max_accel_eco(v_ego) + else: + if frogpilot_toggles.acceleration_profile == 3: + self.max_accel = get_max_accel_sport_plus(v_ego) + else: + self.max_accel = get_max_accel_sport(v_ego) else: - self.max_accel = get_max_accel(v_ego) + if frogpilot_toggles.acceleration_profile == 1: + self.max_accel = get_max_accel_eco(v_ego) + elif frogpilot_toggles.acceleration_profile == 2: + self.max_accel = get_max_accel_sport(v_ego) + elif frogpilot_toggles.acceleration_profile == 3: + self.max_accel = get_max_accel_sport_plus(v_ego) + elif controlsState.experimentalMode: + self.max_accel = ACCEL_MAX + else: + self.max_accel = get_max_accel(v_ego) if not self.tracking_lead: self.max_accel = min(self.max_accel, self.max_accel * (self.v_cruise / CITY_SPEED_LIMIT)) @@ -131,12 +143,18 @@ class FrogPilotPlanner: self.min_accel = ACCEL_MIN elif self.mtsc_target < v_cruise: self.min_accel = A_CRUISE_MIN - elif frogpilot_toggles.deceleration_profile == 1: - self.min_accel = A_CRUISE_MIN_ECO - elif frogpilot_toggles.deceleration_profile == 2: - self.min_accel = A_CRUISE_MIN_SPORT + elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear): + if eco_gear: + self.min_accel = A_CRUISE_MIN_ECO + else: + self.min_accel = A_CRUISE_MIN_SPORT else: - self.min_accel = A_CRUISE_MIN + if frogpilot_toggles.deceleration_profile == 1: + self.min_accel = A_CRUISE_MIN_ECO + elif frogpilot_toggles.deceleration_profile == 2: + self.min_accel = A_CRUISE_MIN_SPORT + else: + self.min_accel = A_CRUISE_MIN def set_follow_values(self, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): if frogpilotCarState.trafficModeActive: