mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-09 07:32:06 +08:00
BUTTONS YEEEAAAA
This commit is contained in:
@@ -66,7 +66,9 @@ class CarController(CarControllerBase):
|
||||
self.malibu_cancel_phase = 0
|
||||
self.malibu_cancel_last_ts = 0.0
|
||||
self.malibu_cancel_frame = 0
|
||||
self.malibu_cancel_frame_offset = 0
|
||||
self.malibu_button_phase = 0
|
||||
self.malibu_last_button_ts_nanos = 0
|
||||
|
||||
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint]['pt'])
|
||||
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint]['radar'])
|
||||
@@ -229,6 +231,19 @@ class CarController(CarControllerBase):
|
||||
self.pedal_steady = pedal_gas
|
||||
return pedal_gas, press_regen_paddle
|
||||
|
||||
def _update_malibu_button_slot(self, cs):
|
||||
button_ts = cs.steering_button_ts_nanos
|
||||
if button_ts != 0 and button_ts != self.malibu_last_button_ts_nanos:
|
||||
self.malibu_last_button_ts_nanos = button_ts
|
||||
return True
|
||||
return False
|
||||
|
||||
def _sync_malibu_phase_from_oem(self, cs):
|
||||
phase_map = gmcan.malibu_phase_map_for_acc(cs.cruise_buttons)
|
||||
if phase_map and cs.steering_button_checksum in phase_map:
|
||||
phase = phase_map[cs.steering_button_checksum]
|
||||
self.malibu_cancel_phase = phase
|
||||
self.malibu_button_phase = phase
|
||||
|
||||
def update(self, CC, CS, now_nanos, frogpilot_toggles):
|
||||
self.CS = CS
|
||||
@@ -286,12 +301,11 @@ class CarController(CarControllerBase):
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
|
||||
malibu_oem_button_slot = False
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
||||
phase_map = gmcan.malibu_phase_map_for_acc(CS.cruise_buttons)
|
||||
if phase_map and CS.steering_button_checksum in phase_map:
|
||||
phase = (phase_map[CS.steering_button_checksum] + 1) % 4
|
||||
self.malibu_cancel_phase = phase
|
||||
self.malibu_button_phase = phase
|
||||
malibu_oem_button_slot = self._update_malibu_button_slot(CS)
|
||||
if malibu_oem_button_slot:
|
||||
self._sync_malibu_phase_from_oem(CS)
|
||||
|
||||
raw_regen_active = (
|
||||
self.CP.carFingerprint in CC_REGEN_PADDLE_CAR and
|
||||
@@ -369,6 +383,8 @@ class CarController(CarControllerBase):
|
||||
can_sends.append(gmcan.create_ecm_cruise_control_command(
|
||||
self.packer_pt, CanBus.POWERTRAIN, spoof_enabled, spoof_set_speed_kph))
|
||||
|
||||
malibu_cancel_requested = False
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
|
||||
# Gas/regen, brakes, and UI commands - all at 25Hz
|
||||
@@ -547,17 +563,16 @@ class CarController(CarControllerBase):
|
||||
stock_cc_active = CS.out.cruiseState.enabled or CS.pcm_acc_status != AccState.OFF
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL:
|
||||
stock_cc_active = CS.out.cruiseState.enabled
|
||||
pedal_cancel = self.CP.flags & GMFlags.PEDAL_LONG.value
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
||||
pedal_cancel = pedal_cancel and CC.longActive
|
||||
|
||||
if (
|
||||
(self.CP.flags & GMFlags.PEDAL_LONG.value) # Always cancel stock CC when using pedal interceptor
|
||||
pedal_cancel
|
||||
or (self.CP.flags & GMFlags.CC_LONG.value and not CC.enabled) # Cancel stock CC if OP is not active
|
||||
) and stock_cc_active:
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
||||
# Match 33 Hz cadence (every 3 frames) and align phase to the last seen checksum.
|
||||
if self.malibu_cancel_frame % 3 == 0:
|
||||
can_sends.append(gmcan.create_buttons_malibu_cancel(
|
||||
CanBus.POWERTRAIN, self.malibu_cancel_phase, CS.steering_button_prefix))
|
||||
self.malibu_cancel_phase = (self.malibu_cancel_phase + 1) % 4
|
||||
self.malibu_cancel_frame += 1
|
||||
malibu_cancel_requested = True
|
||||
else:
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
||||
self.last_button_frame = self.frame
|
||||
@@ -570,20 +585,24 @@ class CarController(CarControllerBase):
|
||||
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
|
||||
|
||||
# Stock longitudinal, integrated at camera
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
||||
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
|
||||
malibu_cancel_requested = True
|
||||
elif (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
||||
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
|
||||
self.last_button_frame = self.frame
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
||||
if self.malibu_cancel_frame % 3 == 0:
|
||||
can_sends.append(gmcan.create_buttons_malibu_cancel(
|
||||
CanBus.POWERTRAIN, self.malibu_cancel_phase, CS.steering_button_prefix))
|
||||
self.malibu_cancel_phase = (self.malibu_cancel_phase + 1) % 4
|
||||
self.malibu_cancel_frame += 1
|
||||
elif self.CP.carFingerprint in SDGM_CAR and self.CP.carFingerprint not in (volt_like | {CAR.CHEVROLET_BLAZER, CAR.CHEVROLET_MALIBU_SDGM, CAR.CHEVROLET_TRAVERSE}):
|
||||
if self.CP.carFingerprint in SDGM_CAR and self.CP.carFingerprint not in (volt_like | {CAR.CHEVROLET_BLAZER, CAR.CHEVROLET_MALIBU_SDGM, CAR.CHEVROLET_TRAVERSE}):
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
else:
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC:
|
||||
# Malibu has no reliable rolling counter in 0x1E1; lock cancel send to each observed OEM 0x1E1 slot.
|
||||
if malibu_cancel_requested and malibu_oem_button_slot:
|
||||
can_sends.append(gmcan.create_buttons_malibu_cancel(
|
||||
CanBus.POWERTRAIN, (self.malibu_cancel_phase + 1) % 4, CS.steering_button_prefix))
|
||||
self.malibu_cancel_phase = (self.malibu_cancel_phase + 1) % 4
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
|
||||
if self.frame % 10 == 0:
|
||||
|
||||
@@ -28,6 +28,7 @@ class CarState(CarStateBase):
|
||||
self.buttons_counter = 0
|
||||
self.steering_button_checksum = 0
|
||||
self.steering_button_prefix = 0x01
|
||||
self.steering_button_ts_nanos = 0
|
||||
|
||||
self.prev_distance_button = 0
|
||||
self.distance_button = 0
|
||||
@@ -56,6 +57,7 @@ class CarState(CarStateBase):
|
||||
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
|
||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
self.steering_button_checksum = pt_cp.vl["ASCMSteeringButton"]["SteeringButtonChecksum"]
|
||||
self.steering_button_ts_nanos = pt_cp.ts_nanos["ASCMSteeringButton"]["ACCButtons"]
|
||||
acc_always_one = pt_cp.vl["ASCMSteeringButton"]["ACCAlwaysOne"]
|
||||
acc_hidden_bit = pt_cp.vl["ASCMSteeringButton"].get("ACCHiddenBit", 0)
|
||||
self.steering_button_prefix = (int(acc_always_one) & 1) | ((int(acc_hidden_bit) & 1) << 6)
|
||||
@@ -63,6 +65,7 @@ class CarState(CarStateBase):
|
||||
self.cruise_buttons = cam_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.distance_button = cam_cp.vl["ASCMSteeringButton"]["DistanceButton"]
|
||||
self.buttons_counter = cam_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
self.steering_button_ts_nanos = cam_cp.ts_nanos["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
|
||||
# This is to avoid a fault where you engage while still moving backwards after shifting to D.
|
||||
# An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2)
|
||||
|
||||
@@ -7,10 +7,10 @@ from openpilot.selfdrive.car import make_can_msg
|
||||
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CanBus
|
||||
|
||||
MALIBU_BUTTON_TABLE = {
|
||||
0: [0x2FBC, 0x25DE, 0x15EE, 0x1FCC],
|
||||
0: [0x10FF, 0x15EE, 0x1ADD, 0x1FCC],
|
||||
1: [0x55AE, 0x5F8C, 0x6F7C, 0x659E],
|
||||
4: [0x2ACD, 0x20EF, 0x1ADD, 0x10FF],
|
||||
5: [0x50BF, 0x5A9D, 0x60AF, 0x6A8D],
|
||||
5: [0x60AF, 0x659E, 0x6A8D, 0x6F7C],
|
||||
}
|
||||
|
||||
MALIBU_BUTTON_MAP = {
|
||||
@@ -78,10 +78,10 @@ def create_buttons_malibu_cancel(bus, phase, prefix=0x41):
|
||||
data = bytearray(7)
|
||||
data[3] = prefix & 0xFF
|
||||
data[4] = 0x00
|
||||
cancel_bytes = (0x60, 0xAF, 0x65, 0x9E, 0x6A, 0x8D, 0x6F, 0x7C)
|
||||
idx = ((phase + 2) % 4) * 2
|
||||
data[5] = cancel_bytes[idx]
|
||||
data[6] = cancel_bytes[idx + 1]
|
||||
cancel_seq = MALIBU_BUTTON_TABLE[5]
|
||||
val = cancel_seq[phase % len(cancel_seq)]
|
||||
data[5] = (val >> 8) & 0xFF
|
||||
data[6] = val & 0xFF
|
||||
return make_can_msg(0x1e1, bytes(data), bus)
|
||||
|
||||
|
||||
|
||||
@@ -537,6 +537,9 @@ class CarInterface(CarInterfaceBase):
|
||||
# Keep Malibu Hybrid pedal on the same longitudinal tune as Bolt pedal cars.
|
||||
ret.longitudinalTuning.kpBP = [0.0, 5.0, 15.0, 35.0]
|
||||
ret.longitudinalTuning.kpV = [0.095, 0.085, 0.065, 0.050]
|
||||
# Reuse Bolt pedal cancel-allow safety path so 0x1E1 cancel isn't blocked
|
||||
# when stock cruise state differs from pedal-long control state.
|
||||
gm_safety_cfg.safetyParam |= Panda.FLAG_GM_BOLT_2022_PEDAL
|
||||
ret.longitudinalTuning.kiBP = [0.0, 3.0, 6.0, 35.0]
|
||||
ret.longitudinalTuning.kiV = [0.07, 0.10, 0.15, 0.24]
|
||||
ret.longitudinalTuning.kfDEPRECATED = 0.20
|
||||
|
||||
Reference in New Issue
Block a user