def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.cp_adas.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_adas.can_valid and self.cp_cam.can_valid
buttonEvents = []
be = car.CarState.ButtonEvent.new_message()
be.type = car.CarState.ButtonEvent.Type.accelCruise
buttonEvents.append(be)
events = self.create_common_events(ret)
if self.CS.lkas_enabled:
events.add(car.CarEvent.EventName.invalidLkasSetting)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
if dragonconf.dpToyotaLowestCruiseOverride and ret.cruiseState.speed < dragonconf.dpToyotaLowestCruiseOverrideAt * CV.KPH_TO_MS:
ret.cruiseState.speed = dragonconf.dpToyotaLowestCruiseOverrideSpeed * CV.KPH_TO_MS
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# events
events = self.create_common_events(ret)
if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera and not self.CP.isPandaBlack:
events.add(EventName.invalidGiraffeToyota)
if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl:
events.add(EventName.lowSpeedLockout)
if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:
events.add(EventName.belowEngageSpeed)
if c.actuators.gas > 0.1:
# some margin on the actuator to not false trigger cancellation while stopping
events.add(EventName.speedTooLow)
if ret.vEgo < 0.001:
# while in standstill, send a user alert
events.add(EventName.manualRestart)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# TODO: button presses
ret.buttonEvents = []
# events
events = self.create_common_events(ret)
if self.CS.low_speed_lockout:
events.add(EventName.belowEngageSpeed)
if self.CS.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# TODO: button presses
ret.buttonEvents = []
events = self.create_common_events(ret)
#TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed +
2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
if ret.vEgo >= self.CP.minSteerSpeed:
ret.cruiseState.enabled = common_interface_atl(
ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
events = self.create_common_events(ret)
if dragonconf.dpAtl:
if ret.vEgo < self.CP.minSteerSpeed:
events.add(car.CarEvent.EventName.belowSteerSpeed)
else:
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed +
2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# events
events = self.create_common_events(
ret,
extra_gears=[car.CarState.GearShifter.low],
gas_resume_speed=2.)
if ret.vEgo < self.CP.minSteerSpeed:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
# copy back carState packet to CS
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
buttonEvents = []
# Process the most recent CAN message traffic, and check for validity
# The camera CAN has no signals we use at this time, but we process it
# anyway so we can test connectivity with can_valid
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# TODO: add a field for this to carState, car interface code shouldn't write params
# Update the device metric configuration to match the car at first startup,
# or if there's been a change.
#if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
# put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates:
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
be = car.CarState.ButtonEvent.new_message()
be.type = button
be.pressed = self.CS.buttonStates[button]
buttonEvents.append(be)
events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic])
# Vehicle health and operation safety checks
if self.CS.parkingBrakeSet:
events.add(EventName.parkBrake)
# Low speed steer alert hysteresis logic
if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):
self.low_speed_alert = True
elif ret.vEgo > (self.CP.minSteerSpeed + 2.):
self.low_speed_alert = False
if self.low_speed_alert:
events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg()
ret.buttonEvents = buttonEvents
# update previous car states
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
# low speed re-write
if ret.cruiseState.enabled and dragonconf.dpToyotaCruiseOverride and ret.cruiseState.speed < dragonconf.dpToyotaCruiseOverrideAt * CV.KPH_TO_MS:
if dragonconf.dpToyotaCruiseOverrideVego:
if self.dp_cruise_speed == 0.:
ret.cruiseState.speed = self.dp_cruise_speed = max(
dragonconf.dpToyotaCruiseOverrideSpeed * CV.KPH_TO_MS,
ret.vEgo)
else:
ret.cruiseState.speed = self.dp_cruise_speed
else:
ret.cruiseState.speed = dragonconf.dpToyotaCruiseOverrideSpeed * CV.KPH_TO_MS
else:
self.dp_cruise_speed = 0.
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# gear except P, R
extra_gears = [
GearShifter.neutral, GearShifter.eco, GearShifter.manumatic,
GearShifter.drive, GearShifter.sport, GearShifter.low,
GearShifter.brake, GearShifter.unknown
]
# events
events = self.create_common_events(ret, extra_gears)
if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl:
events.add(EventName.lowSpeedLockout)
if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:
events.add(EventName.belowEngageSpeed)
if c.actuators.accel > 0.3:
# some margin on the actuator to not false trigger cancellation while stopping
events.add(EventName.speedTooLow)
if ret.vEgo < 0.001:
# while in standstill, send a user alert
events.add(EventName.manualRestart)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.events = self.create_common_events(ret).to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
if ret.vEgo >= self.CP.minSteerSpeed:
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
events = self.create_common_events(ret)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
ret = self.CS.update(self.cp)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid
# events
events = self.create_common_events(ret)
if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled:
events.add(car.CarEvent.EventName.steerTempUnavailable)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
# dp
self.dragonconf = dragonconf
if ret.vEgo >= self.CP.minSteerSpeed:
ret.cruiseState.enabled = common_interface_atl(
ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
events = self.create_common_events(ret,
pcm_enable=self.CS.CP.pcmCruise)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
if self.CS.park_brake:
events.add(EventName.parkBrake)
if self.CS.CP.openpilotLongitudinalControl:
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == Buttons.RES_ACCEL:
be.type = ButtonType.accelCruise
elif but == Buttons.SET_DECEL:
be.type = ButtonType.decelCruise
elif but == Buttons.GAP_DIST:
be.type = ButtonType.gapAdjustCruise
elif but == Buttons.CANCEL:
be.type = ButtonType.cancel
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in [
ButtonType.accelCruise, ButtonType.decelCruise
] and not b.pressed:
events.add(EventName.buttonEnable)
# do disable on button down
if b.type == ButtonType.cancel and b.pressed:
events.add(EventName.buttonCancel)
if dragonconf.dpAtl:
if ret.vEgo < self.CP.minSteerSpeed:
events.add(car.CarEvent.EventName.belowSteerSpeed)
else:
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed +
2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
if self.low_speed_alert:
events.add(car.CarEvent.EventName.belowSteerSpeed)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
self.cp.update_strings(can_strings)
ret = self.CS.update(self.cp)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_buttons != CruiseButtons.UNPRESS:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
if not (ret.cruiseState.enabled and ret.standstill):
be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET:
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = ButtonType.altButton3
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
events = self.create_common_events(ret, pcm_enable=False)
if ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
if self.CS.park_brake:
events.add(EventName.parkBrake)
if ret.cruiseState.standstill:
events.add(EventName.resumeRequired)
if self.CS.pcm_acc_status == AccState.FAULTED:
events.add(EventName.accFaulted)
if ret.vEgo < self.CP.minSteerSpeed:
events.add(car.CarEvent.EventName.belowSteerSpeed)
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
events.add(EventName.buttonEnable)
# do disable on button down
if b.type == ButtonType.cancel and b.pressed:
events.add(EventName.buttonCancel)
ret.events = events.to_msg()
# copy back carState packet to CS
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
buttonEvents = []
# Process the most recent CAN message traffic, and check for validity
# The camera CAN has no signals we use at this time, but we process it
# anyway so we can test connectivity with can_valid
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext,
self.CP.transmissionType)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# TODO: add a field for this to carState, car interface code shouldn't write params
# Update the device metric configuration to match the car at first startup,
# or if there's been a change.
#if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
# put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates:
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
be = car.CarState.ButtonEvent.new_message()
be.type = button
be.pressed = self.CS.buttonStates[button]
buttonEvents.append(be)
events = self.create_common_events(ret,
extra_gears=[
GearShifter.eco,
GearShifter.sport,
GearShifter.manumatic
])
# Vehicle health and operation safety checks
if self.CS.parkingBrakeSet:
events.add(EventName.parkBrake)
# Engagement and longitudinal control using stock ACC. Make sure OP is
# disengaged if stock ACC is disengaged.
if not ret.cruiseState.enabled:
events.add(EventName.pcmDisable)
# Attempt OP engagement only on rising edge of stock ACC engagement.
elif not self.cruise_enabled_prev:
events.add(EventName.pcmEnable)
if dragonconf.dpVwTimebombAssist:
ret.stopSteering = False
if ret.cruiseState.enabled:
self.timebomb_counter += 1
else:
self.timebomb_counter = 0
self.timebomb_bypass_counter = 0
if self.timebomb_counter >= 33000: # 330*100 time in seconds until counter threshold for timebombWarn alert
if not self.wheel_grabbed:
events.add(EventName.timebombWarn)
if self.wheel_grabbed or ret.steeringPressed:
self.wheel_grabbed = True
ret.stopSteering = True
self.timebomb_bypass_counter += 1
if self.timebomb_bypass_counter >= 300: # 3*100 time alloted for bypass
self.wheel_grabbed = False
self.timebomb_counter = 0
self.timebomb_bypass_counter = 0
events.add(EventName.timebombBypassed)
else:
events.add(EventName.timebombBypassing)
ret.events = events.to_msg()
ret.buttonEvents = buttonEvents
# update previous car states
self.cruise_enabled_prev = ret.cruiseState.enabled
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
buttonEvents = []
# Process the most recent CAN message traffic, and check for validity
# The camera CAN has no signals we use at this time, but we process it
# anyway so we can test connectivity with can_valid
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam, self.cp_acc, self.CP.transmissionType)
ret.canValid = self.cp.can_valid # FIXME: Restore cp_cam valid check after proper LKAS camera detect
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# TODO: add a field for this to carState, car interface code shouldn't write params
# Update the device metric configuration to match the car at first startup,
# or if there's been a change.
#if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
# put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
for button in self.CS.buttonStates:
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
be = car.CarState.ButtonEvent.new_message()
be.type = button
be.pressed = self.CS.buttonStates[button]
buttonEvents.append(be)
events = self.create_common_events(ret, extra_gears=[GEAR.eco, GEAR.sport])
# Vehicle health and operation safety checks
if self.CS.parkingBrakeSet:
events.add(EventName.parkBrake)
if self.CS.steeringFault:
events.add(EventName.steerTempUnavailable)
# Engagement and longitudinal control using stock ACC. Make sure OP is
# disengaged if stock ACC is disengaged.
if not ret.cruiseState.enabled:
events.add(EventName.pcmDisable)
# Attempt OP engagement only on rising edge of stock ACC engagement.
elif not self.cruise_enabled_prev:
events.add(EventName.pcmEnable)
ret.events = events.to_msg()
ret.buttonEvents = buttonEvents
# update previous car states
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
if self.cp_body:
self.cp_body.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.lkMode = self.CS.lkMode
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (
self.cp_body is None or self.cp_body.can_valid)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD,
ret.vEgo)
#dp
ret.engineRPM = self.CS.engineRPM
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = ButtonType.accelCruise
elif but == CruiseButtons.DECEL_SET:
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = ButtonType.altButton3
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_setting != 0:
be.pressed = True
but = self.CS.cruise_setting
else:
be.pressed = False
but = self.CS.prev_cruise_setting
if but == 1:
be.type = ButtonType.altButton1
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
events = self.create_common_events(ret, pcm_enable=False)
if not self.CS.lkMode or (dragonconf.dpAtl
and ret.vEgo <= self.CP.minEnableSpeed):
events.add(EventName.manualSteeringRequired)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
if self.CS.park_brake:
events.add(EventName.parkBrake)
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
if self.CP.pcmCruise:
# we engage when pcm is active (rising edge)
if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
events.add(EventName.pcmEnable)
elif not ret.cruiseState.enabled and (
c.actuators.brake <= 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 ret.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.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.add(EventName.manualRestart)
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise
] and not b.pressed:
if not self.CP.pcmCruise:
events.add(EventName.buttonEnable)
# do disable on button down
if b.type == ButtonType.cancel and b.pressed:
events.add(EventName.buttonCancel)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings, dragonconf):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
if self.cp_body:
self.cp_body.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
# dp
self.dragonconf = dragonconf
ret.cruiseState.enabled = common_interface_atl(ret, dragonconf.dpAtl)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (
self.cp_body is None or self.cp_body.can_valid)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD,
ret.vEgo)
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
ret.brakeLights = bool(self.CS.brake_switch or
(self.CP.openpilotLongitudinalControl
and c.actuators.brake > brakelights_threshold))
# dp
ret.lkMode = self.CS.lkMode
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = ButtonType.accelCruise
elif but == CruiseButtons.DECEL_SET:
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = ButtonType.altButton3
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_setting != 0:
be.pressed = True
but = self.CS.cruise_setting
else:
be.pressed = False
but = self.CS.prev_cruise_setting
if but == 1:
be.type = ButtonType.altButton1
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
events = self.create_common_events(ret, pcm_enable=False)
if not self.CS.lkMode or (dragonconf.dpAtl
and ret.vEgo <= self.CP.minEnableSpeed):
events.add(EventName.manualSteeringRequired)
if self.CS.brake_error:
events.add(EventName.brakeUnavailable)
if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
events.add(EventName.brakeHold)
if self.CS.park_brake:
events.add(EventName.parkBrake)
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
# 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 self.CP.enableCruise and not ret.cruiseState.enabled \
and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl):
# non loud alert if cruise disables below 25mph as expected (+ a little margin)
if ret.vEgo < self.CP.minEnableSpeed + 2.:
# events.add(EventName.speedTooLow)
# else:
events.add(EventName.cruiseDisabled)
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.add(EventName.manualRestart)
cur_time = self.frame * DT_CTRL
enable_pressed = False
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise
] and not b.pressed:
self.last_enable_pressed = cur_time
enable_pressed = True
# do disable on button down
if b.type == "cancel" and b.pressed:
events.add(EventName.buttonCancel)
if self.CP.enableCruise:
# KEEP THIS EVENT LAST! send enable event if button is pressed and there are
# NO_ENTRY events, so controlsd will display alerts. Also not send enable events
# too close in time, so a no_entry will not be followed by another one.
# TODO: button press should be the only thing that triggers enable
if ((cur_time - self.last_enable_pressed) < 0.2 and
(cur_time - self.last_enable_sent) > 0.2 and
ret.cruiseState.enabled) or \
(enable_pressed and events.any(ET.NO_ENTRY)):
events.add(EventName.buttonEnable)
self.last_enable_sent = cur_time
elif enable_pressed:
events.add(EventName.buttonEnable)
ret.events = events.to_msg()
self.CS.out = ret.as_reader()
return self.CS.out