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

38
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.
@@ -267,7 +267,30 @@ class CarController:
for _ in range(20):
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