brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
concordia
2024-02-10 14:46:06 -06:00
parent da6553c24d
commit 829b75411b
2 changed files with 34 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
selfdrive/car/hyundai/carcontroller.py
View File

@@ -146,7 +146,7 @@ class CarController:
self.accel_last = accel
else:
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False, set_speed_in_units = None))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
@@ -209,7 +209,7 @@ class CarController:
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool):
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool, set_speed_in_units = None):
can_sends = []
# Test me.
@@ -268,6 +268,29 @@ class CarController:
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
elif set_speed_in_units is not None and not self.CP.openpilotLongitudinalControl and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
cslcSetSpeed = set_speed_in_units
self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
if self.cruise_button != Buttons.NONE:
# if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# send_freq = 1
# # send resume at a max freq of 10Hz
# if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# self.last_button_frame = self.frame
if self.frame % 2 == 0:
for _ in range(20):
if self.CP.carFingerprint in CANFD_CAR:
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
else:
can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
self.last_button_frame = self.frame
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25)
return can_sends
@@ -283,14 +306,15 @@ def get_set_speed(self, hud_v_cruise):
v_cruise = v_cruise_slc
return v_cruise
def get_cslc_button(self, cslcSetSpeed, CS):
def get_cslc_button(self, cslcSetSpeed, CS, CC):
cruiseBtn = Buttons.NONE
speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH))
if cslcSetSpeed < speedSetPoint and speedSetPoint > 25:
if cslcSetSpeed < speedSetPoint and speedSetPoint > 25 and CC.enabled and CC.experimental_mode:
cruiseBtn = Buttons.SET_DECEL
elif cslcSetSpeed > speedSetPoint and speedSetPoint < 75:
elif cslcSetSpeed > speedSetPoint and speedSetPoint < 85 and CC.enabled:
cruiseBtn = Buttons.RES_ACCEL
else:
cruiseBtn = Buttons.NONE
cruiseBtn = Buttons.SET_DECEL
# cruiseBtn = Buttons.NONE
return cruiseBtn

5
selfdrive/car/hyundai/hyundaicanfd.py
View File

@@ -41,7 +41,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"LKA_ICON": 2 if enabled else 1 if lat_active else 0,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
@@ -116,7 +116,8 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
def create_lfahda_cluster(packer, CAN, enabled):
values = {
"HDA_ICON": 1 if enabled else 0,
"LFA_ICON": 3 if enabled else 0, # was green before i think in 2? 3 should be flashing?
# 0 off, 1 gray, 2 green, 3 blinking (wheel icon)
"LFA_ICON": 0 if enabled else 0, # was green before i think in 2? 3 should be flashing?
# "LFA_ICON": 2 if enabled else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)