forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ f2a861c
This commit is contained in:
IQ.Lvbs CI [bot]
189
iqpilot/selfdrive/car/car_specific.py
Normal file
189
iqpilot/selfdrive/car/car_specific.py
Normal file
@@ -0,0 +1,189 @@
|
||||
from iqpilot.cereal import car, log
|
||||
from iqdbc.car import DT_CTRL, structs
|
||||
from iqdbc.car.car_helpers import interfaces
|
||||
from iqdbc.car.interfaces import MAX_CTRL_SPEED
|
||||
from iqdbc.car.toyota.values import ToyotaFlags
|
||||
|
||||
from iqpilot.selfdrive.selfdrived.events import Events
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
EventName = log.OnroadEvent.EventName
|
||||
NetworkLocation = structs.CarParams.NetworkLocation
|
||||
|
||||
|
||||
class CarSpecificEvents:
|
||||
def __init__(self, CP: structs.CarParams):
|
||||
self.CP = CP
|
||||
|
||||
self.steering_unpressed = 0
|
||||
self.low_speed_alert = False
|
||||
self.no_steer_warning = False
|
||||
self.silent_steer_warning = True
|
||||
|
||||
def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl):
|
||||
if self.CP.brand in ('body', 'mock'):
|
||||
return Events()
|
||||
|
||||
events = self.create_common_events(CS, CS_prev)
|
||||
|
||||
if self.CP.brand == 'chrysler':
|
||||
# Low speed steer alert hysteresis logic
|
||||
if self.CP.minSteerSpeed > 0. and CS.vEgo < (self.CP.minSteerSpeed + 0.5):
|
||||
self.low_speed_alert = True
|
||||
elif CS.vEgo > (self.CP.minSteerSpeed + 1.):
|
||||
self.low_speed_alert = False
|
||||
if self.low_speed_alert:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
|
||||
elif self.CP.brand == 'honda':
|
||||
if self.CP.pcmCruise and CS.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
|
||||
if self.CP.pcmCruise:
|
||||
# we engage when pcm is active (rising edge)
|
||||
if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
|
||||
events.add(EventName.pcmEnable)
|
||||
elif not CS.cruiseState.enabled and (CC.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl):
|
||||
# it can happen that car cruise disables while comma system is enabled: need to
|
||||
# keep braking if needed or if the speed is very low
|
||||
if CS.vEgo < self.CP.minEnableSpeed + 2.:
|
||||
# non loud alert if cruise disables below 25mph as expected (+ a little margin)
|
||||
events.add(EventName.speedTooLow)
|
||||
else:
|
||||
events.add(EventName.cruiseDisabled)
|
||||
if self.CP.minEnableSpeed > 0 and CS.vEgo < 0.001:
|
||||
events.add(EventName.manualRestart)
|
||||
|
||||
elif self.CP.brand == 'toyota':
|
||||
# TODO: when we check for unexpected disengagement, check gear not S1, S2, S3
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# Only can leave standstill when planner wants to move
|
||||
if CS.cruiseState.standstill and not CS.brakePressed and (CC.cruiseControl.resume or self.CP.flags & ToyotaFlags.HYBRID.value):
|
||||
events.add(EventName.resumeRequired)
|
||||
if CS.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if CC.actuators.accel > 0.3:
|
||||
# some margin on the actuator to not false trigger cancellation while stopping
|
||||
events.add(EventName.speedTooLow)
|
||||
if CS.vEgo < 0.001:
|
||||
# while in standstill, send a user alert
|
||||
events.add(EventName.manualRestart)
|
||||
|
||||
elif self.CP.brand == 'gm':
|
||||
# Enabling at a standstill with brake is allowed
|
||||
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
|
||||
if CS.vEgo < self.CP.minEnableSpeed and not (CS.standstill and CS.brake >= 20 and
|
||||
self.CP.networkLocation == NetworkLocation.fwdCamera):
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if CS.cruiseState.standstill:
|
||||
events.add(EventName.resumeRequired)
|
||||
|
||||
elif self.CP.brand == 'volkswagen':
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if CS.vEgo < self.CP.minEnableSpeed + 0.5:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if CC.enabled and CS.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.speedTooLow)
|
||||
|
||||
# TODO: this needs to be implemented generically in carState struct
|
||||
# if CC.eps_timer_soft_disable_alert:
|
||||
# events.add(EventName.steerTimeLimit)
|
||||
|
||||
return events
|
||||
|
||||
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState):
|
||||
events = Events()
|
||||
|
||||
CI = interfaces[self.CP.carFingerprint]
|
||||
# TODO: cleanup the honda-specific logic
|
||||
pcm_enable = self.CP.pcmCruise and self.CP.brand != 'honda'
|
||||
# TODO: on some hyundai cars, the cancel button is also the pause/resume button,
|
||||
# so only use it for cancel when running openpilot longitudinal
|
||||
allow_button_cancel = self.CP.brand != 'hyundai'
|
||||
|
||||
if CS.doorOpen:
|
||||
events.add(EventName.doorOpen)
|
||||
if CS.seatbeltUnlatched:
|
||||
events.add(EventName.seatbeltNotLatched)
|
||||
if CS.gearShifter != GearShifter.drive and CS.gearShifter not in CI.DRIVABLE_GEARS:
|
||||
events.add(EventName.wrongGear)
|
||||
if CS.gearShifter == GearShifter.reverse:
|
||||
events.add(EventName.reverseGear)
|
||||
if not CS.cruiseState.available and not getattr(CS, 'cruiseFaultLateralMode', False):
|
||||
events.add(EventName.wrongCarMode)
|
||||
if CS.espDisabled:
|
||||
events.add(EventName.espDisabled)
|
||||
if CS.espActive:
|
||||
events.add(EventName.espActive)
|
||||
if CS.stockFcw:
|
||||
events.add(EventName.stockFcw)
|
||||
if CS.stockAeb:
|
||||
events.add(EventName.stockAeb)
|
||||
if CS.stockLkas:
|
||||
events.add(EventName.stockLkas)
|
||||
if CS.vEgo > MAX_CTRL_SPEED:
|
||||
events.add(EventName.speedTooHigh)
|
||||
if CS.cruiseState.nonAdaptive:
|
||||
events.add(EventName.wrongCruiseMode)
|
||||
if CS.brakeHoldActive and self.CP.openpilotLongitudinalControl:
|
||||
events.add(EventName.brakeHold)
|
||||
if CS.parkingBrake:
|
||||
events.add(EventName.parkBrake)
|
||||
if getattr(CS, 'cruiseFaultLateralMode', False):
|
||||
events.add(EventName.cruiseFaultLateralAllowed)
|
||||
elif CS.accFaulted:
|
||||
events.add(EventName.accFaulted)
|
||||
if CS.steeringPressed:
|
||||
events.add(EventName.steerOverride)
|
||||
if CS.steeringDisengage and not CS_prev.steeringDisengage:
|
||||
events.add(EventName.steerDisengage)
|
||||
if CS.brakePressed and CS.standstill:
|
||||
events.add(EventName.preEnableStandstill)
|
||||
if CS.gasPressed:
|
||||
events.add(EventName.gasPressedOverride)
|
||||
if CS.vehicleSensorsInvalid:
|
||||
events.add(EventName.vehicleSensorsInvalid)
|
||||
if CS.invalidLkasSetting:
|
||||
events.add(EventName.invalidLkasSetting)
|
||||
if CS.lowSpeedAlert:
|
||||
events.add(EventName.belowSteerSpeed)
|
||||
if CS.buttonEnable:
|
||||
events.add(EventName.buttonEnable)
|
||||
|
||||
# Handle cancel button presses
|
||||
for b in CS.buttonEvents:
|
||||
# Disable on rising and falling edge of cancel for both stock and OP long
|
||||
# TODO: only check the cancel button with openpilot longitudinal on all brands to match panda safety
|
||||
if b.type == ButtonType.cancel and (allow_button_cancel or not self.CP.pcmCruise):
|
||||
events.add(EventName.buttonCancel)
|
||||
|
||||
# Handle permanent and temporary steering faults
|
||||
self.steering_unpressed = 0 if CS.steeringPressed else self.steering_unpressed + 1
|
||||
if CS.steerFaultTemporary:
|
||||
if CS.steeringPressed and (not CS_prev.steerFaultTemporary or self.no_steer_warning):
|
||||
self.no_steer_warning = True
|
||||
else:
|
||||
self.no_steer_warning = False
|
||||
|
||||
# if the user overrode recently, show a less harsh alert
|
||||
if self.silent_steer_warning or CS.standstill or self.steering_unpressed < int(1.5 / DT_CTRL):
|
||||
self.silent_steer_warning = True
|
||||
events.add(EventName.steerTempUnavailableSilent)
|
||||
else:
|
||||
events.add(EventName.steerTempUnavailable)
|
||||
else:
|
||||
self.no_steer_warning = False
|
||||
self.silent_steer_warning = False
|
||||
if CS.steerFaultPermanent:
|
||||
events.add(EventName.steerUnavailable)
|
||||
|
||||
# we engage when pcm is active (rising edge)
|
||||
# enabling can optionally be blocked by the car interface
|
||||
if pcm_enable:
|
||||
if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled and not CS.blockPcmEnable:
|
||||
events.add(EventName.pcmEnable)
|
||||
elif not CS.cruiseState.enabled and not getattr(CS, 'cruiseFaultLateralMode', False):
|
||||
events.add(EventName.pcmDisable)
|
||||
|
||||
return events
|
||||
Reference in New Issue
Block a user