wip
This commit is contained in:
Your Name
@@ -70,7 +70,16 @@ class CarState(CarStateBase):
|
||||
speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
|
||||
# print(speed_limit_bus.vl, sys.stderr)
|
||||
# sys.stderr.write(str({k: v for k, v in vars(speed_limit_bus).items() if isinstance(v, (int, float, str, bool))}) + '\n')
|
||||
return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
|
||||
# Clearpilot fix
|
||||
best_speed = 0
|
||||
if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]):
|
||||
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_3"]
|
||||
if (speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
|
||||
if (best_speed == 0 or best_speed < speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]):
|
||||
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_2"]
|
||||
if (best_speed == 0):
|
||||
best_speed = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
|
||||
return best_speed
|
||||
else:
|
||||
if "SpeedLim_Nav_Clu" not in cp.vl["Navi_HU"]:
|
||||
return 0
|
||||
@@ -199,11 +208,15 @@ class CarState(CarStateBase):
|
||||
self.lkas_previously_enabled = self.lkas_enabled
|
||||
self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
|
||||
|
||||
self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
|
||||
|
||||
return ret
|
||||
|
||||
def update_canfd(self, cp, cp_cam, frogpilot_variables):
|
||||
|
||||
# ---- Preexisting unsorted frogpilot code ----
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
|
||||
@@ -280,28 +293,28 @@ class CarState(CarStateBase):
|
||||
self.prev_main_buttons = self.main_buttons[-1]
|
||||
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
|
||||
|
||||
# testme
|
||||
# if ret.cruiseState.speed > 1 and self.main_enabled == False:
|
||||
# self.main_enabled = True
|
||||
# ------ End old frogpilot code ------
|
||||
|
||||
# test if this blocks trying to engage while pressin brakes
|
||||
# Clearpilot Activation button fixes:
|
||||
|
||||
# This code tracks the engaged state of openpilot based on when the user clicks the
|
||||
# cruise button. However, cruise only engages when the break is not pressed,
|
||||
# and cruise can already be enabled when openpilot starts. This compensates for that behavior.
|
||||
|
||||
# Block attempt to engage if already engaged according to cruise state
|
||||
if ret.cruiseState > 0 and self.main_enabled == False:
|
||||
self.main_buttons[-1] = 0
|
||||
|
||||
# Block attempt to engage if breaks are pressed
|
||||
if self.main_buttons[-1] != 0 and ret.brakePressed and self.main_enabled == False:
|
||||
self.main_buttons[-1] = 0
|
||||
# Suprisingly this works. Test with op long and submit.
|
||||
|
||||
# But enable always on lateral if self.main_enabled = false
|
||||
# I think this may require an override variable?
|
||||
# It looks like always on lat is only set when cruise control is enabled
|
||||
# We need a temp version that enables or disables it if cruise is off via button
|
||||
# and then the temp state removes when the button is pressed again
|
||||
|
||||
# Swicth to enabled based on button if above conditions pass
|
||||
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
|
||||
self.main_enabled = not self.main_enabled
|
||||
# if self.fix_main_enabled_check:
|
||||
# self.fix_main_enabled_check = False
|
||||
# if ret.cruiseState.speed > 1 and self.main_enabled == False:
|
||||
# self.fix_main_enabled_cancel_main = True
|
||||
# self.fix_main_enabled_executed = True
|
||||
|
||||
# --------- Resume preexisting frogpilot code ------------
|
||||
|
||||
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
|
||||
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
||||
|
||||
@@ -346,6 +359,58 @@ class CarState(CarStateBase):
|
||||
# ret: cruiseState.speed > 0 = and cruiseState.enabled = false = idle cruise.
|
||||
# press cruise to cancel it if not op engaged
|
||||
|
||||
# engaged at 25
|
||||
# ret from carstate:
|
||||
# . cruiseState = (
|
||||
# enabled = true,
|
||||
# speed = 11.176, # above 0 if disengaged but set
|
||||
# available = true,
|
||||
# speedOffset = 0,
|
||||
# standstill = false,
|
||||
# nonAdaptive = false,
|
||||
# speedCluster = 0 ),
|
||||
# .standstill = false,
|
||||
# steeringPressed = false,
|
||||
|
||||
# Interesting values:
|
||||
# 416.ACC_ObjDist - distance to lead radar?
|
||||
# 506.SPEED_LIMIT_2
|
||||
# print(speed_limit_bus.vl, sys.stderr)
|
||||
# {416: {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0, 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3':
|
||||
# 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0, 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0,
|
||||
# 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0, 'COUNTER': 173.0,
|
||||
# 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0, 'NEW_SIGNAL_2': 0.0,
|
||||
# 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0, 'DISTANCE_SETTING': 0.0,
|
||||
# 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0, 'aReqValue': 0.0, 'ZEROS_7': 0.0,
|
||||
# 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002, 'JerkLowerLimit': 0.0, 'StopReq': 0.0,
|
||||
# 'NEW_SIGNAL_15': 204.60000000000002, 'VSetDis': 0.0, 'MainMode_ACC': 0.0,
|
||||
# 'JerkUpperLimit': 0.0}, 'SCC_CONTROL': {'CHECKSUM': 34571.0, 'NEW_SIGNAL_1': 0.0,
|
||||
# 'NEW_SIGNAL_8': 0.0, 'ZEROS': 0.0, 'ZEROS_3': 0.0, 'ZEROS_4': 0.0, 'ZEROS_6': 256.0,
|
||||
# 'ZEROS_8': 0.0, 'NEW_SIGNAL_3': 0.0, 'SET_ME_TMP_64': 100.0, 'SET_ME_2': 4.0, 'NEW_SIGNAL_6': 0.0,
|
||||
# 'COUNTER': 173.0, 'ZEROS_9': 0.0, 'ZEROS_10': 0.0, 'SET_ME_3': 3.0, 'ObjValid': 0.0,
|
||||
# 'NEW_SIGNAL_2': 0.0, 'OBJ_STATUS': 0.0, 'ACC_ObjDist': 204.60000000000002, 'ZEROS_5': 0.0,
|
||||
# 'DISTANCE_SETTING': 0.0, 'ZEROS_2': 0.0, 'CRUISE_STANDSTILL': 0.0, 'aReqRaw': 0.0,
|
||||
# 'aReqValue': 0.0, 'ZEROS_7': 0.0, 'ACCMode': 0.0, 'NEW_SIGNAL_12': 16.400000000000002,
|
||||
# 'JerkLowerLimit': 0.0, 'StopReq': 0.0, 'NEW_SIGNAL_15': 204.60000000000002,
|
||||
# 'VSetDis': 0.0, 'MainMode_ACC': 0.0, 'JerkUpperLimit': 0.0},
|
||||
# 506: {'SPEED_LIMIT_3': 38.0,
|
||||
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
|
||||
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
|
||||
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0},
|
||||
# 'CLUSTER_SPEED_LIMIT': {'SPEED_LIMIT_3': 38.0,
|
||||
# 'SPEED_LIMIT_2': 35.0, 'SPEED_LIMIT_1': 35.0, 'SPEED_CHANGE_BLINKING': 0.0, 'CHIME_2': 0.0,
|
||||
# 'CHIME_1': 0.0, 'ARROW_DOWN': 0.0, 'ARROW_UP': 0.0, 'SECONDARY_LIMIT_1': 0.0,
|
||||
# 'SECONDARY_LIMIT_2': 0.0, 'SCHOOL_ZONE': 0.0}}
|
||||
# <_io.TextIOWrapper name='<stderr>' mode='w' encoding='utf-8'>
|
||||
|
||||
# ( enabled = false,
|
||||
# speed = 9.83488,
|
||||
# available = false,
|
||||
# speedOffset = 0,
|
||||
# standstill = false,
|
||||
# nonAdaptive = false,
|
||||
# speedCluster = 0 )
|
||||
|
||||
|
||||
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user