forked from IQ.Lvbs/IQ.Pilot
94 lines
3.4 KiB
Python
94 lines
3.4 KiB
Python
"""
|
|
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
|
|
|
IQ.Pilot Subaru stop-and-go: nudges the ACC out of a standstill it would
|
|
otherwise hold. Two variants gated by user flags — an electronic-parking-brake
|
|
resume pulse (distance/lead triggered) and a manual-parking-brake hold-timer
|
|
resume. Both work by spoofing the camera-bus throttle/brake frames.
|
|
"""
|
|
import copy
|
|
from enum import StrEnum
|
|
|
|
from iqdbc.car import Bus, structs, DT_CTRL
|
|
from iqdbc.car.can_definitions import CanData
|
|
from iqdbc.car.interfaces import CarStateBase
|
|
from iqdbc.car.subaru.values import SubaruFlags
|
|
|
|
from iqdbc.lvbs.car.subaru import iq_subarucan
|
|
from iqdbc.lvbs.car.subaru.iq_values import SubaruFlagsIQ
|
|
from iqdbc.can.parser import CANParser
|
|
|
|
# EPB resume fires only while the lead is pulling away within this gap band (m).
|
|
_RESUME_GAP_MIN = 3.0
|
|
_RESUME_GAP_MAX = 4.5
|
|
_EPB_PULSE_FRAMES = 15
|
|
|
|
|
|
class IQStopAndGoController:
|
|
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
|
self.CP = CP
|
|
self.CP_IQ = CP_IQ
|
|
self.enabled = bool(CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE))
|
|
self.manual_parking_brake = bool(CP_IQ.flags & SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE)
|
|
|
|
self.standstill_since = 0
|
|
self._pulse_left = 0
|
|
self.prev_gap = 0.0
|
|
|
|
def _epb_pulse(self, trigger: bool) -> bool:
|
|
# A trigger arms a fixed-length resume pulse; the pulse then plays out frame by frame.
|
|
if self.manual_parking_brake:
|
|
return False
|
|
if trigger:
|
|
self._pulse_left = _EPB_PULSE_FRAMES
|
|
if self._pulse_left > 0:
|
|
self._pulse_left -= 1
|
|
return True
|
|
return False
|
|
|
|
def _want_resume(self, CC: structs.CarControl, CS: CarStateBase, frame: int) -> bool:
|
|
if not CC.enabled or not CC.hudControl.leadVisible:
|
|
return False
|
|
|
|
gap = CS.es_distance_msg["Close_Distance"]
|
|
standing = CS.out.standstill
|
|
if not standing:
|
|
self.standstill_since = frame
|
|
|
|
hold_arm, hold_reset = (0.75, 0.8) if self.CP.flags & SubaruFlags.PREGLOBAL else (0.5, 0.55)
|
|
held_for = (frame - self.standstill_since) * DT_CTRL
|
|
held_long_enough = held_for > hold_arm
|
|
if held_for >= hold_reset:
|
|
self.standstill_since = frame
|
|
|
|
lead_pulling_away = _RESUME_GAP_MIN < gap < _RESUME_GAP_MAX and gap > self.prev_gap
|
|
self.prev_gap = gap
|
|
|
|
if self.manual_parking_brake:
|
|
return held_long_enough
|
|
return self._epb_pulse(standing and lead_pulling_away)
|
|
|
|
def create_creep_assist(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]:
|
|
if not self.enabled:
|
|
return []
|
|
|
|
resume = self._want_resume(CC, CS, frame)
|
|
can_sends = [iq_subarucan.create_throttle(packer, self.CP, CS.throttle_msg, resume and not self.manual_parking_brake)]
|
|
if frame % 2 == 0:
|
|
can_sends.append(iq_subarucan.create_brake_pedal(packer, self.CP, CS.brake_pedal_msg, resume and self.manual_parking_brake))
|
|
return can_sends
|
|
|
|
|
|
class IQStopAndGoState:
|
|
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
|
self.CP = CP
|
|
self.CP_IQ = CP_IQ
|
|
self.brake_pedal_msg: dict[str, float] = {}
|
|
self.throttle_msg: dict[str, float] = {}
|
|
|
|
def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
|
cp = can_parsers[Bus.pt]
|
|
self.brake_pedal_msg = copy.copy(cp.vl["Brake_Pedal"])
|
|
if not self.CP.flags & SubaruFlags.HYBRID:
|
|
self.throttle_msg = copy.copy(cp.vl["Throttle"])
|