diff --git a/opendbc/gm_global_a_powertrain_volt.dbc b/opendbc/gm_global_a_powertrain_volt.dbc index 98e7cfb..9361c75 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 fe744a2..cb290fd 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -246,6 +246,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 @@ -528,6 +529,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 93934f2..2828e7c 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -172,6 +172,8 @@ class CarState(CarStateBase): # FrogPilot carstate functions fp_ret.hasCamera = not (self.CP.flags & GMFlags.NO_CAMERA.value) and self.CP.carFingerprint not in CC_ONLY_CAR + fp_ret.sportGear = pt_cp.vl["SportMode"]["SportMode"] == 1 + self.lkas_previously_enabled = self.lkas_enabled if self.CP.carFingerprint in SDGM_CAR: self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"] @@ -217,6 +219,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 851e43f..136d307 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_variables): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam, frogpilot_variables) @@ -269,6 +272,15 @@ class CarState(CarStateBase): self.prev_distance_button = self.distance_button self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0 + 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"] @@ -357,6 +369,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/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 5a12c42..ca64eb4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -190,6 +190,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.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 dadfe61..78c1d82 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -85,25 +85,40 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel - distance_offset stopping_distance = STOP_DISTANCE + distance_offset - if frogpilot_toggles.acceleration_profile == 1: - self.max_accel = get_max_accel_eco(v_ego) - elif frogpilot_toggles.acceleration_profile in (2, 3): - self.max_accel = get_max_accel_sport(v_ego) - elif controlsState.experimentalMode: - self.max_accel = ACCEL_MAX + eco_gear = carState.gearShifter == GearShifter.eco or frogpilotCarState.ecoGear + sport_gear = carState.gearShifter == GearShifter.sport or frogpilotCarState.sportGear + + if frogpilot_toggles.map_acceleration and (eco_gear or sport_gear): + if eco_gear: + self.max_accel = get_max_accel_eco(v_ego) + elif sport_gear: + 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 in (2, 3): + self.max_accel = get_max_accel_sport(v_ego) + elif controlsState.experimentalMode: + self.max_accel = ACCEL_MAX + else: + self.max_accel = get_max_accel(v_ego) if controlsState.experimentalMode: self.min_accel = ACCEL_MIN elif v_cruise_changed: self.min_accel = A_CRUISE_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) + elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear): + if eco_gear: + self.min_accel = get_min_accel_eco(v_ego) + elif sport_gear: + self.min_accel = get_min_accel_sport(v_ego) else: - self.min_accel = A_CRUISE_MIN + if 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: