mawei
/
ods777
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Controls - Experimental Mode Activation - Double Click LKAS 

Enable/disable 'Experimental Mode' by double clicking the 'LKAS' button on your steering wheel.
This commit is contained in:
FrogAi 2024-06-10 06:47:01 -07:00
parent c90095fe12
commit e07e1ee6bf
25 changed files with 120 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
opendbc/chrysler_pacifica_2017_hybrid_generated.dbc
View File

@ -312,6 +312,7 @@ BO_ 764 ACCEL_RELATED_2FC: 8 XXX
BO_ 816 TRACTION_BUTTON: 8 XXX
SG_ TRACTION_OFF : 19|1@0+ (1,0) [0|3] "" XXX
SG_ TOGGLE_PARKSENSE : 52|1@0+ (1,0) [0|3] "" XXX
SG_ TOGGLE_LKAS : 53|1@0+ (1,0) [0|1] "" XXX
BO_ 878 ACCEL_RELATED_36E: 8 XXX
SG_ ACCEL_OR_RPM_2 : 15|8@0+ (1,0) [0|255] "" XXX

6
opendbc/chrysler_ram_hd_generated.dbc
View File

@ -207,3 +207,9 @@ BO_ 630 LKAS_COMMAND: 8 XXX
SG_ COUNTER : 55|4@0+ (1,0) [0|15] "" XXX
SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
BO_ 650 Center_Stack_2: 8 XXX
SG_ LKAS_Button : 57|1@1+ (1,0) [0|0] "" XXX
BO_ 816 Center_Stack_1: 8 XXX
SG_ LKAS_Button : 53|1@1+ (1,0) [0|0] "" XXX
SG_ Traction_Button : 54|1@0+ (1,0) [0|1] "" XXX

1
panda/python/__init__.py
View File

@ -209,6 +209,7 @@ class Panda:
FLAG_HYUNDAI_CANFD_ALT_BUTTONS = 32
FLAG_HYUNDAI_ALT_LIMITS = 64
FLAG_HYUNDAI_CANFD_HDA2_ALT_STEERING = 128
FLAG_HYUNDAI_LFA_BTN = 256
FLAG_TESLA_POWERTRAIN = 1
FLAG_TESLA_LONG_CONTROL = 2

10
selfdrive/car/chrysler/carstate.py
View File

@ -102,6 +102,13 @@ class CarState(CarStateBase):
self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"]
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
if self.CP.carFingerprint in RAM_CARS:
self.lkas_enabled = cp.vl["Center_Stack_2"]["LKAS_Button"] or cp.vl["Center_Stack_1"]["LKAS_Button"]
else:
self.lkas_enabled = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1
return ret, fp_ret
@staticmethod
@ -135,11 +142,14 @@ class CarState(CarStateBase):
("ESP_8", 50),
("EPS_3", 50),
("Transmission_Status", 50),
("Center_Stack_1", 1),
("Center_Stack_2", 1),
]
else:
messages += [
("GEAR", 50),
("SPEED_1", 100),
("TRACTION_BUTTON", 1),
]
messages += CarState.get_cruise_messages()

6
selfdrive/car/chrysler/interface.py
View File

@ -6,6 +6,7 @@ from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CAR
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
class CarInterface(CarInterfaceBase):
@ -79,7 +80,10 @@ class CarInterface(CarInterfaceBase):
def _update(self, c, frogpilot_toggles):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
ret.buttonEvents = [
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low])

4
selfdrive/car/ford/carstate.py
View File

@ -108,6 +108,10 @@ class CarState(CarStateBase):
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = ret.genericToggle
return ret, fp_ret
@staticmethod

6
selfdrive/car/ford/interface.py
View File

@ -7,6 +7,7 @@ from openpilot.selfdrive.car.ford.values import Ecu, FordFlags
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter
@ -70,7 +71,10 @@ class CarInterface(CarInterfaceBase):
def _update(self, c, frogpilot_toggles):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
ret.buttonEvents = [
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic])
if not self.CS.vehicle_sensors_valid:

6
selfdrive/car/gm/carstate.py
View File

@ -172,6 +172,12 @@ class CarState(CarStateBase):
# FrogPilot CarState functions
fp_ret.hasMenu = not (self.CP.flags & GMFlags.NO_CAMERA.value or self.CP.carFingerprint in CC_ONLY_CAR)
self.lkas_previously_enabled = self.lkas_enabled
if self.CP.carFingerprint in SDGM_CAR:
self.lkas_enabled = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
else:
self.lkas_enabled = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
return ret, fp_ret
@staticmethod

5
selfdrive/car/gm/interface.py
View File

@ -13,6 +13,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLater
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
GearShifter = car.CarState.GearShifter
TransmissionType = car.CarParams.TransmissionType
@ -318,7 +319,9 @@ class CarInterface(CarInterfaceBase):
*create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT,
unpressed_btn=CruiseButtons.UNPRESS),
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
{1: ButtonType.gapAdjustCruise})
{1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled,
{1: FrogPilotButtonType.lkas}),
]
# The ECM allows enabling on falling edge of set, but only rising edge of resume

4
selfdrive/car/honda/carstate.py
View File

@ -273,6 +273,10 @@ class CarState(CarStateBase):
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = self.cruise_setting == 1
return ret, fp_ret
def get_can_parser(self, CP):

2
selfdrive/car/honda/interface.py
View File

@ -12,6 +12,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
TransmissionType = car.CarParams.TransmissionType
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
@ -253,6 +254,7 @@ class CarInterface(CarInterfaceBase):
ret.buttonEvents = [
*create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
# events

12
selfdrive/car/hyundai/carstate.py
View File

@ -172,6 +172,11 @@ class CarState(CarStateBase):
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
# FrogPilot CarState functions
if self.CP.flags & HyundaiFlags.CAN_LFA_BTN:
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
return ret, fp_ret
def update_canfd(self, cp, cp_cam, frogpilot_toggles):
@ -258,6 +263,10 @@ class CarState(CarStateBase):
self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
else cp_cam.vl["CAM_0x2a4"])
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
return ret, fp_ret
def get_can_parser(self, CP):
@ -308,6 +317,9 @@ class CarState(CarStateBase):
else:
messages.append(("LVR12", 100))
if CP.flags & HyundaiFlags.CAN_LFA_BTN:
messages.append(("BCM_PO_11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod

9
selfdrive/car/hyundai/interface.py
View File

@ -124,6 +124,10 @@ class CarInterface(CarInterfaceBase):
if candidate in CAMERA_SCC_CAR:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
if 0x391 in fingerprint[0]:
ret.flags |= HyundaiFlags.CAN_LFA_BTN.value
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN
if ret.openpilotLongitudinalControl:
ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
if ret.flags & HyundaiFlags.HYBRID:
@ -155,7 +159,10 @@ class CarInterface(CarInterfaceBase):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
if self.CS.CP.openpilotLongitudinalControl:
ret.buttonEvents = create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT)
ret.buttonEvents = [
*create_button_events(self.CS.cruise_buttons[-1], self.CS.prev_cruise_buttons, BUTTONS_DICT),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
# On some newer model years, the CANCEL button acts as a pause/resume button based on the PCM state
# To avoid re-engaging when openpilot cancels, check user engagement intention via buttons

2
selfdrive/car/hyundai/values.py
View File

@ -95,6 +95,8 @@ class HyundaiFlags(IntFlag):
MIN_STEER_32_MPH = 2 ** 23
# FrogPilot HKG flags
CAN_LFA_BTN = 2 ** 24
class Footnote(Enum):
CANFD = CarFootnote(

4
selfdrive/car/interfaces.py
View File

@ -389,6 +389,10 @@ class CarStateBase(ABC):
K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R)
self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K)
# FrogPilot variables
self.lkas_enabled = False
self.lkas_previously_enabled = False
def update_speed_kf(self, v_ego_raw):
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.set_x([[v_ego_raw], [0.0]])

4
selfdrive/car/mazda/carstate.py
View File

@ -111,6 +111,10 @@ class CarState(CarStateBase):
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
# FrogPilot CarState functions
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = not self.lkas_disabled
return ret, fp_ret
@staticmethod

6
selfdrive/car/mazda/interface.py
View File

@ -6,6 +6,7 @@ from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
class CarInterface(CarInterfaceBase):
@ -35,7 +36,10 @@ class CarInterface(CarInterfaceBase):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
# TODO: add button types for inc and dec
ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
ret.buttonEvents = [
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
# events
events = self.create_common_events(ret)

1
selfdrive/car/nissan/carstate.py
View File

@ -108,6 +108,7 @@ class CarState(CarStateBase):
can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
self.lkas_previously_enabled = self.lkas_enabled
if self.CP.carFingerprint == CAR.NISSAN_ALTIMA:
self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"])
else:

6
selfdrive/car/nissan/interface.py
View File

@ -5,6 +5,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.nissan.values import CAR
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
class CarInterface(CarInterfaceBase):
@ -32,7 +33,10 @@ class CarInterface(CarInterfaceBase):
def _update(self, c, frogpilot_toggles):
ret, fp_ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam, frogpilot_toggles)
ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
ret.buttonEvents = [
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake])

7
selfdrive/car/subaru/carstate.py
View File

@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.subaru.values import DBC, CanBus, SubaruFlags
from openpilot.selfdrive.car.subaru.values import DBC, CanBus, PREGLOBAL_CARS, SubaruFlags
from openpilot.selfdrive.car import CanSignalRateCalculator
@ -126,6 +126,11 @@ class CarState(CarStateBase):
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"])
# FrogPilot CarState functions
if self.car_fingerprint not in PREGLOBAL_CARS:
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"]
return ret, fp_ret
@staticmethod

5
selfdrive/car/subaru/interface.py
View File

@ -5,6 +5,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
class CarInterface(CarInterfaceBase):
@ -101,6 +102,10 @@ class CarInterface(CarInterfaceBase):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, self.cp_body, frogpilot_toggles)
ret.buttonEvents = [
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
ret.events = self.create_common_events(ret).to_msg()
return ret, fp_ret

2
selfdrive/car/subaru/values.py
View File

@ -209,6 +209,8 @@ class CAR(Platforms):
)
PREGLOBAL_CARS = {CAR.SUBARU_FORESTER_PREGLOBAL, CAR.SUBARU_LEGACY_PREGLOBAL, CAR.SUBARU_OUTBACK_PREGLOBAL, CAR.SUBARU_OUTBACK_PREGLOBAL_2018}
SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \

4
selfdrive/car/toyota/carstate.py
View File

@ -191,6 +191,10 @@ class CarState(CarStateBase):
self.distance_button = cp.vl["SDSU"]["FD_BUTTON"]
# FrogPilot CarState functions
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
self.lkas_previously_enabled = self.lkas_enabled
message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys)
# ZSS Support - Credit goes to the DragonPilot team!
if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count < ZSS_THRESHOLD_COUNT:

6
selfdrive/car/toyota/interface.py
View File

@ -8,6 +8,7 @@ from openpilot.selfdrive.car.disable_ecu import disable_ecu
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
SteerControlType = car.CarParams.SteerControlType
@ -155,7 +156,10 @@ class CarInterface(CarInterfaceBase):
ret, fp_ret = self.CS.update(self.cp, self.cp_cam, frogpilot_toggles)
if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
ret.buttonEvents = [
*create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
*create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}),
]
# events
events = self.create_common_events(ret)

9
selfdrive/controls/controlsd.py
View File

@ -52,6 +52,7 @@ LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
EventName = car.CarEvent.EventName
ButtonType = car.CarState.ButtonEvent.Type
FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
SafetyModel = car.CarParams.SafetyModel
@ -923,6 +924,14 @@ class Controls:
if self.frogpilot_toggles.conditional_experimental_mode:
self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimentalActive
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.frogpilot_toggles.experimental_mode_via_lkas:
if self.frogpilot_toggles.conditional_experimental_mode:
conditional_status = self.params_memory.get_int("CEStatus")
override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4
self.params_memory.put_int("CEStatus", override_value)
else:
self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode)
FPCC = custom.FrogPilotCarControl.new_message()
FPCC.alwaysOnLateral = self.always_on_lateral_active