def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor,
poller, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params, plan, path_plan):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
CS = CI.update(CC)
events = list(CS.events)
enabled = isEnabled(state)
# Receive from sockets
td = None
cal = None
hh = None
dm = None
for socket, event in poller.poll(0):
if socket is thermal:
td = messaging.recv_one(socket)
elif socket is calibration:
cal = messaging.recv_one(socket)
elif socket is health:
hh = messaging.recv_one(socket)
elif socket is driver_monitor:
dm = messaging.recv_one(socket)
elif socket is plan_sock:
plan = messaging.recv_one(socket)
elif socket is path_plan_sock:
path_plan = messaging.recv_one(socket)
if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
free_space = td.thermal.freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = td.thermal.batteryPercent < 1 # at zero percent battery, OP should not be allowed
# Create events for battery, temperature and disk space
if low_battery:
events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# Handle calibration
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket than the CAN messages, therefore one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 2:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if dm is not None:
driver_status.get_pose(dm.driverMonitoring, params)
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
if self.CP.carFingerprint == CAR.RAV4H:
# ignore standstill in hybrid rav4, since pcm allows to restart without
# receiving any special command
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.genericToggle = self.CS.generic_toggle
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if not ret.gearShifter == 'drive' and self.CP.enableDsu:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled and self.CP.enableDsu:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on and self.CP.enableDsu:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse' and self.CP.enableDsu:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if self.CS.low_speed_lockout and self.CP.enableDsu:
events.append(
create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT]))
if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if c.actuators.gas > 0.1:
# some margin on the actuator to not false trigger cancellation while stopping
events.append(
create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
if ret.vEgo < 0.001:
# while in standstill, send a user alert
events.append(create_event('manualRestart', [ET.WARNING]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric", encoding='utf8') == "1"
is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1"
passive = passive or not openpilot_enabled_toggle
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams'])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
'model', 'gpsLocation'], ignore_alive=['gpsLocation'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
community_feature_disallowed = CP.communityFeature and not community_feature_toggle
read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd = bool(int(is_rhd))
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
last_blinker_frame = 0
events_prev = []
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1.
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
prof = Profiler(False) # off by default
while True:
start_time = sec_since_boot()
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_perc, mismatch_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Create alerts
if not sm.all_alive_and_valid():
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
if not sm['plan'].radarValid:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC,
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
def update(self, c):
self.pt_cp.update(int(sec_since_boot() * 1e9), False)
self.ch_cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.pt_cp, self.ch_cp)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal information.
ret.gas = self.CS.pedal_gas / 254.0
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake / 0xd0
ret.brakePressed = self.CS.brake_pressed
ret.brakeLights = self.CS.frictionBrakesActive
# steering wheel
ret.steeringAngle = self.CS.angle_steers + self.angleSteersoffset
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF
ret.cruiseState.enabled = cruiseEnabled
ret.cruiseState.standstill = False
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.gearShifter = self.CS.gear_shifter
ret.readdistancelines = self.CS.follow_level
ret.genericToggle = False
ret.laneDepartureToggle = False
ret.distanceToggle = self.CS.follow_level
ret.accSlowToggle = False
ret.blindspot = self.CS.blind_spot_on
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = '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 (cruiseEnabled and self.CS.standstill):
be.type = 'accelCruise' # Suppress resume button if we're resuming from stop so we don't adjust speed.
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
if not cruiseEnabled and not self.CS.lkMode:
self.CS.lkMode = True
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
if self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button:
if self.CS.lkMode:
self.CS.lkMode = False
else:
self.CS.lkMode = True
if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button:
self.CS.follow_level -= 1
if self.CS.follow_level < 1:
self.CS.follow_level = 3
kegman.save({'lastTrMode': int(self.CS.follow_level)
}) # write last distance bar setting to file
ret.gasbuttonstatus = self.CS.gasMode
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
disengage_event = True
else:
disengage_event = False
if self.CS.steer_error:
events.append(
create_event(
'steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if ret.doorOpen and disengage_event:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched and disengage_event:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.car_fingerprint in SUPERCRUISE_CARS:
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
if self.CS.brake_error:
events.append(
create_event(
'brakeUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.gear_shifter == 3:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if self.CS.park_brake:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if ret.cruiseState.standstill:
events.append(create_event('resumeRequired', [ET.WARNING]))
if self.CS.pcm_acc_status == AccState.FAULTED:
events.append(
create_event('controlsFailed',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == "cancel" and b.pressed:
events.append(
create_event('buttonCancel', [ET.USER_DISABLE]))
ret.events = events
# update previous brake/gas pressed
self.acc_active_prev = self.CS.acc_active
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = True #self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
if self.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press
ret.gasPressed = self.CS.pedal_gas > 15
else:
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringTorqueEps = self.CS.steer_torque_motor
ret.steeringPressed = self.CS.steer_override
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_active
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command
# also if interceptor is detected
ret.cruiseState.standstill = False
else:
ret.cruiseState.standstill = self.CS.pcm_acc_status == 7
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.genericToggle = self.CS.generic_toggle
ret.stockAeb = self.CS.stock_aeb
# events
events = []
# if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera:
# events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT]))
if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on and self.CP.openpilotLongitudinalControl:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl:
events.append(
create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT]))
if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if c.actuators.gas > 0.1:
# some margin on the actuator to not false trigger cancellation while stopping
events.append(
create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
if ret.vEgo < 0.001:
# while in standstill, send a user alert
events.append(create_event('manualRestart', [ET.WARNING]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp2.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp2, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.canValid = self.cp.can_valid # TODO: check cp_cam validity
ret.canValid = True
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
# most HKG cars has no long control, it is safer and easier to engage by main on
ret.cruiseState.enabled = (
self.CS.pcm_acc_status != 0) if self.CC.longcontrol else bool(
self.CS.main_on)
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
# Optima only has blinker flash signal
if self.CP.carFingerprint in [CAR.KIA_OPTIMA, CAR.KIA_OPTIMA_H]:
self.CS.left_blinker_on = self.CS.left_blinker_flash or self.CS.prev_left_blinker_on and self.CC.turning_signal_timer
self.CS.right_blinker_on = self.CS.right_blinker_flash or self.CS.prev_right_blinker_on and self.CC.turning_signal_timer
ret.lcaLeft = self.CS.lca_left != 0
ret.lcaRight = self.CS.lca_right != 0
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
#ret.seatbeltUnlatched = self.CS.seatbelt
ret.seatbeltUnlatched = False
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed +
0.2) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 0.7):
self.low_speed_alert = False
# turning indicator alert logic
self.turning_indicator_alert = True if self.CC.turning_signal_timer and self.CS.v_ego < 16.7 else False
# LKAS button alert logic
self.lkas_button_alert = True if not self.CC.lkas_button else False
events = []
if not ret.gearShifter == GearShifter.drive:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(
create_event('reverseGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.steer_error or abs(self.CS.angle_steers) > 90.:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if ((ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1))) and self.CC.longcontrol:
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed and self.CC.longcontrol:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.low_speed_alert and not self.CS.mdps_bus:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
if self.turning_indicator_alert:
events.append(create_event('turningIndicatorOn', [ET.WARNING]))
if self.lkas_button_alert:
events.append(create_event('lkasButtonOff', [ET.WARNING]))
#TODO Varible for min Speed for LCA
if ret.rightBlinker and ret.lcaRight and self.CS.v_ego > (
45 * CV.MPH_TO_MS):
events.append(create_event('rightLCAbsm', [ET.WARNING]))
if ret.leftBlinker and ret.lcaLeft and self.CS.v_ego > (45 *
CV.MPH_TO_MS):
events.append(create_event('leftLCAbsm', [ET.WARNING]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status # same as main_on
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = self.CS.main_on
ret.cruiseState.speedOffset = 0.
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
ret.genericToggle = self.CS.generic_toggle
#ret.lkasCounter = self.CS.lkas_counter
#ret.lkasCarModel = self.CS.lkas_car_model
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
disengage_event = True
else:
disengage_event = False
# events
events = []
if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)):
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen and disengage_event:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched and disengage_event:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event(
'steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC
# from a 3+ second stop.
#if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0):
# events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.low_speed_alert:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def update(self, c, can_strings):
ret_arne182 = arne182.CarStateArne182.new_message()
self.cp.update_strings(can_strings)
ret = self.CS.update(self.cp)
ret.canValid = self.cp.can_valid
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD,
ret.vEgo)
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, eventsArne182 = self.create_common_events(ret,
pcm_enable=False)
if ret.brakePressed:
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if self.CS.park_brake:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.cruiseState.standstill:
events.append(create_event('resumeRequired', [ET.WARNING]))
if self.CS.pcm_acc_status == AccState.FAULTED:
events.append(
create_event('controlsFailed',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# 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.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == ButtonType.cancel and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
ret.events = events
ret_arne182.events = eventsArne182
# copy back carState packet to CS
self.CS.out = ret.as_reader()
return self.CS.out, ret_arne182.as_reader()
def update(self, c, can_strings):
self.pt_cp.update_strings(can_strings)
self.CS.update(self.pt_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = self.pt_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal information.
ret.gas = self.CS.pedal_gas / 254.0
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake / 0xd0
ret.brakePressed = self.CS.brake_pressed
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF
if not self.cruise_enabled_prev:
ret.cruiseState.enabled = cruiseEnabled
else:
ret.cruiseState.enabled = bool(self.CS.main_on)
if not cruiseEnabled:
ret.brakePressed = True
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.gearShifter = self.CS.gear_shifter
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
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 != 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 (cruiseEnabled and self.CS.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
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
disengage_event = True
else:
disengage_event = False
events = []
if self.CS.steer_error:
events.append(
create_event(
'steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if ret.doorOpen and disengage_event:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched and disengage_event:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.car_fingerprint in SUPERCRUISE_CARS:
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
if self.CS.brake_error:
events.append(
create_event(
'brakeUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.gear_shifter == 3:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if self.CS.park_brake:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if ((ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed)) and disengage_event: # and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed and disengage_event:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if ret.cruiseState.standstill:
events.append(create_event('resumeRequired', [ET.WARNING]))
if self.CS.pcm_acc_status == AccState.FAULTED:
events.append(
create_event('controlsFailed',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# 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.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == ButtonType.cancel and b.pressed:
events.append(
create_event('buttonCancel', [ET.USER_DISABLE]))
ret.events = events
# update previous brake/gas pressed
self.acc_active_prev = self.CS.acc_active
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, CS, LaC, LoC, v_cruise_kph, force_slow_decel):
cur_time = sec_since_boot()
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
md = None
l20 = None
gps_planner_plan = None
for socket, event in self.poller.poll(0):
if socket is self.model:
md = messaging.recv_one(socket)
elif socket is self.live20:
l20 = messaging.recv_one(socket)
elif socket is self.gps_planner_plan:
gps_planner_plan = messaging.recv_one(socket)
if gps_planner_plan is not None:
self.last_gps_planner_plan = gps_planner_plan
if md is not None:
self.last_md_ts = md.logMonoTime
self.last_model = cur_time
self.model_dead = False
self.PP.update(CS.vEgo, md)
if self.last_gps_planner_plan is not None:
plan = self.last_gps_planner_plan.gpsPlannerPlan
self.gps_planner_active = plan.valid
if plan.valid:
self.PP.d_poly = plan.poly
self.PP.p_poly = plan.poly
self.PP.c_poly = plan.poly
self.PP.l_prob = 0.0
self.PP.r_prob = 0.0
self.PP.c_prob = 1.0
if l20 is not None:
self.perception_state = copy(l20.live20)
self.last_l20_ts = l20.logMonoTime
self.last_l20 = cur_time
self.radar_dead = False
self.radar_errors = list(l20.live20.radarErrors)
self.v_acc_start = self.v_acc_sol
self.a_acc_start = self.a_acc_sol
self.acc_start_time = cur_time
self.lead_1 = l20.live20.leadOne
self.lead_2 = l20.live20.leadTwo
enabled = (LoC.long_control_state
== LongCtrlState.pid) or (LoC.long_control_state
== LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > CS.vEgo and self.lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(
float, calc_cruise_accel_limits(CS.vEgo, following))
# TODO: make a separate lookup for jerk tuning
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
]
accel_limits = limit_accel_in_turns(CS.vEgo, CS.steeringAngle,
accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = LoC.long_control_state == LongCtrlState.starting
a_ego = min(CS.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else CS.vEgo
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.v_acc_sol = reset_speed
self.a_acc_sol = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.leftBlinker or CS.rightBlinker
self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, CS.vEgo, CS.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat,
self.lead_1.fcw, blinkers) \
and not CS.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
if cur_time - self.last_model > 0.5:
self.model_dead = True
if cur_time - self.last_l20 > 0.5:
self.radar_dead = True
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
events = []
if self.model_dead:
events.append(
create_event('modelCommIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.radar_dead or 'commIssue' in self.radar_errors:
events.append(
create_event('radarCommIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(
create_event('radarFault',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if LaC.mpc_solution[
0].cost > 10000. or LaC.mpc_nans: # TODO: find a better way to detect when MPC did not converge
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Interpolation of trajectory
dt = min(
cur_time - self.acc_start_time, _DT_MPC + _DT
) + _DT # no greater than dt mpc + dt, to prevent too high extraps
self.a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
self.v_acc_sol = self.v_acc_start + dt * (self.a_acc_sol +
self.a_acc_start) / 2.0
plan_send.plan.events = events
plan_send.plan.mdMonoTime = self.last_md_ts
plan_send.plan.l20MonoTime = self.last_l20_ts
# lateral plan
plan_send.plan.lateralValid = not self.model_dead
plan_send.plan.dPoly = map(float, self.PP.d_poly)
plan_send.plan.laneWidth = float(self.PP.lane_width)
# longitudal plan
plan_send.plan.longitudinalValid = not self.radar_dead
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vTarget = self.v_acc_sol
plan_send.plan.aTarget = self.a_acc_sol
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.gpsPlannerActive = self.gps_planner_active
# Send out fcw
fcw = self.fcw and (self.fcw_enabled
or LoC.long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
return plan_send
def update(self, c, can_strings):
canMonoTimes = []
events = []
buttonEvents = []
params = Params()
ret = car.CarState.new_message()
# 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.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings)
self.CS.update(self.pt_cp, self.cam_cp)
ret.canValid = self.pt_cp.can_valid
# Wheel and vehicle speed, yaw rate
ret.wheelSpeeds.fl = self.CS.wheelSpeedFL
ret.wheelSpeeds.fr = self.CS.wheelSpeedFR
ret.wheelSpeeds.rl = self.CS.wheelSpeedRL
ret.wheelSpeeds.rr = self.CS.wheelSpeedRR
ret.vEgoRaw = self.CS.vEgoRaw
ret.vEgo = self.CS.vEgo
ret.aEgo = self.CS.aEgo
ret.standstill = self.CS.standstill
# Steering wheel position, movement, yaw rate, and driver input
ret.steeringAngle = self.CS.steeringAngle
ret.steeringRate = self.CS.steeringRate
ret.steeringTorque = self.CS.steeringTorque
ret.steeringPressed = self.CS.steeringPressed
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.yawRate = self.CS.yawRate
# Gas, brakes and shifting
ret.gas = self.CS.gas
ret.gasPressed = self.CS.gasPressed
ret.brake = self.CS.brake
ret.brakePressed = self.CS.brakePressed
ret.brakeLights = self.CS.brakeLights
ret.gearShifter = self.CS.gearShifter
# Doors open, seatbelt unfastened
ret.doorOpen = self.CS.doorOpen
ret.seatbeltUnlatched = self.CS.seatbeltUnlatched
# Update the EON metric configuration to match the car at first startup,
# or if there's been a change.
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Blinker switch updates
ret.leftBlinker = self.CS.buttonStates["leftBlinker"]
ret.rightBlinker = self.CS.buttonStates["rightBlinker"]
# ACC cruise state
ret.cruiseState.available = self.CS.accAvailable
ret.cruiseState.enabled = self.CS.accEnabled
ret.cruiseState.speed = self.CS.accSetSpeed
# 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)
# Vehicle operation safety checks and events
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.gearShifter == GEAR.reverse:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not ret.gearShifter in [GEAR.drive, GEAR.eco, GEAR.sport]:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.stabilityControlDisabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.parkingBrakeSet:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
# Vehicle health and operation safety checks
if self.CS.steeringFault:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
# Per the Comma safety model, disable on pedals rising edge or when brake
# is pressed and speed isn't zero.
if (ret.gasPressed and not self.gasPressedPrev) or \
(ret.brakePressed and (not self.brakePressedPrev or not ret.standstill)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# Engagement and longitudinal control using stock ACC. Make sure OP is
# disengaged if stock ACC is disengaged.
if not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# Attempt OP engagement only on rising edge of stock ACC engagement.
elif not self.cruiseStateEnabledPrev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
ret.events = events
ret.buttonEvents = buttonEvents
ret.canMonoTimes = canMonoTimes
# update previous car states
self.gasPressedPrev = ret.gasPressed
self.brakePressedPrev = ret.brakePressed
self.cruiseStateEnabledPrev = ret.cruiseState.enabled
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.CS.yaw_rate
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
if self.CP.carFingerprint in FEATURES["use_cluster_gears"]:
ret.gearShifter = self.CS.gear_shifter_cluster
elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]:
ret.gearShifter = self.CS.gear_tcu
else:
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate # it's unsigned
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
if self.CS.pcm_acc_status != 0:
ret.cruiseState.speed = self.CS.cruise_set_speed
else:
ret.cruiseState.speed = 0
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
# 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
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if not ret.gearShifter == 'drive':
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.low_speed_alert:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def controlsd_thread(sm=None, pm=None, can_sock=None):
gc.disable()
# start the loop
set_realtime_priority(3)
params = Params()
is_metric = params.get("IsMetric", encoding='utf8') == "1"
is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1"
passive = params.get("Passive", encoding='utf8') == "1"
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle",
encoding='utf8') == "1"
community_feature_toggle = params.get("CommunityFeaturesToggle",
encoding='utf8') == "1"
passive = passive or not openpilot_enabled_toggle
# Passive if internet needed
internet_needed = params.get("Offroad_ConnectivityNeeded",
encoding='utf8') is not None
passive = passive or internet_needed
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster([
'sendcan', 'controlsState', 'carState', 'carControl', 'carEvents',
'carParams'
])
if sm is None:
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \
'model'])
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for health and CAN packets
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
print("Waiting for CAN messages...")
messaging.get_one_can(can_sock)
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
car_recognized = CP.carName != 'mock'
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
controller_available = CP.enableCamera and CI.CC is not None and not passive
community_feature_disallowed = CP.communityFeature and not community_feature_toggle
read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed
if read_only:
CP.safetyModel = car.CarParams.SafetyModel.noOutput
# Write CarParams for radard and boardd safety mode
cp_bytes = CP.to_bytes()
params.put("CarParams", cp_bytes)
put_nonblocking("CarParamsCache", cp_bytes)
put_nonblocking("LongitudinalControl",
"1" if CP.openpilotLongitudinalControl else "0")
CC = car.CarControl.new_message()
AM = AlertManager()
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
elif CP.lateralTuning.which() == 'indi':
LaC = LatControlINDI(CP)
elif CP.lateralTuning.which() == 'lqr':
LaC = LatControlLQR(CP)
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
saturated_count = 0
events_prev = []
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['pathPlan'].sensorValid = True
sm['pathPlan'].posenetValid = True
sm['thermal'].freeSpace = 1.
sm['dMonitoringState'].events = []
sm['dMonitoringState'].awarenessStatus = 1.
sm['dMonitoringState'].faceDetected = False
# detect sound card presence
sounds_available = not os.path.isfile('/EON') or (
os.path.isdir('/proc/asound/card0')
and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
prof = Profiler(False) # off by default
# dp
ts_last_check = 0.
dragon_toyota_stock_dsu = False
dragon_lat_control = True
dragon_display_steering_limit_alert = True
dragon_stopped_has_lead_count = 0
dragon_lead_car_moving_alert = False
dp_last_modified = None
dp_camera_offset = CAMERA_OFFSET
hyundai_lkas = read_only
while True:
start_time = sec_since_boot()
# dp
ts = start_time
if ts - ts_last_check >= 5.:
modified = get_last_modified()
if dp_last_modified != modified:
try:
dp_camera_offset = int(
params.get("DragonCameraOffset",
encoding='utf8')) * 0.01
except (TypeError, ValueError):
dp_camera_offset = CAMERA_OFFSET
dragon_toyota_stock_dsu = True if params.get(
"DragonToyotaStockDSU", encoding='utf8') == "1" else False
dragon_lat_control = False if params.get(
"DragonLatCtrl", encoding='utf8') == "0" else True
if dragon_lat_control:
dragon_display_steering_limit_alert = False if params.get(
"DragonDisplaySteeringLimitAlert",
encoding='utf8') == "0" else True
else:
dragon_display_steering_limit_alert = False
dragon_lead_car_moving_alert = True if params.get(
"DragonEnableLeadCarMovingAlert",
encoding='utf8') == "1" else False
dp_last_modified = modified
ts_last_check = ts
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(
CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter,
params, dragon_toyota_stock_dsu)
if read_only:
hyundai_lkas = read_only
elif CS.cruiseState.enabled:
# elif state == State.enabled:
hyundai_lkas = False
prof.checkpoint("Sample")
# Create alerts
if not sm.alive['plan'] and sm.alive[
'pathPlan']: # only plan not being received: radar not communicating
events.append(
create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
elif not sm.all_alive_and_valid():
events.append(
create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(
create_event('plannerError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid and os.getenv("NOSENSOR") is None:
events.append(
create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not sm['pathPlan'].posenetValid:
events.append(
create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING]))
if not sm['plan'].radarValid:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(
create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
if dragon_toyota_stock_dsu:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
events.append(
create_event('canError',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sounds_available:
events.append(
create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
if internet_needed:
events.append(
create_event('internetConnectivityNeeded',
[ET.NO_ENTRY, ET.PERMANENT]))
if community_feature_disallowed:
events.append(
create_event('communityFeatureDisallowed', [ET.PERMANENT]))
if read_only and not passive:
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
if log.HealthData.FaultType.relayMalfunction in sm['health'].faults:
events.append(
create_event(
'relayMalfunction',
[ET.NO_ENTRY, ET.PERMANENT, ET.IMMEDIATE_DISABLE]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if not dragon_toyota_stock_dsu and CS.brakePressed and sm[
'plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(
create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# dp
if dragon_lead_car_moving_alert:
# when car has a lead and is standstill and lead is barely moving, we start counting
if not CP.radarOffCan and sm[
'plan'].hasLead and CS.vEgo <= 0.01 and 0.3 >= abs(
sm['plan'].vTarget) >= 0:
dragon_stopped_has_lead_count += 1
else:
dragon_stopped_has_lead_count = 0
# when we detect lead car over 3 secs and the lead car is started moving, we are ready to send alerts
# once the condition is triggered, we want to keep the trigger
if dragon_stopped_has_lead_count >= 300:
if abs(sm['plan'].vTargetFuture) >= 0.1:
events.append(create_event('leadCarMoving', [ET.WARNING]))
else:
events.append(create_event('leadCarDetected',
[ET.WARNING]))
# we remove alert once our car is moving
if CS.vEgo > 0.:
dragon_stopped_has_lead_count = 0
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame, saturated_count = \
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
LaC, LoC, hyundai_lkas, is_metric, cal_perc, last_blinker_frame, saturated_count, dragon_lat_control, dragon_display_steering_limit_alert, dragon_lead_car_moving_alert)
prof.checkpoint("State Control")
# Publish data
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, AM, LaC, LoC,
hyundai_lkas, start_time, v_acc, a_acc,
lac_log, events_prev, last_blinker_frame,
is_ldw_enabled, can_error_counter,
dp_camera_offset)
prof.checkpoint("Sent")
rk.monitor_time()
prof.display()
if not CS.cruiseState.enabled:
# if state == State.disabled:
hyundai_lkas = True
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
#2018.09.05 add generic toggle for testing steering max#
#ret.genericToggle = self.CS.generic_toggle
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
#2018.09.04 change to vehicle yaw rate values
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
## ret.yawRate = self.CS.yaw_rate 2018.09.10 use yaw calculation from angle
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
#ret.gas = self.CS.car_gas / 256.0
ret.gas = self.CS.car_gas #2018.09.13 12:39AM change gas to car_gas without deviding
#TODO 2018.09.05 check Throttle report operator override match this
if not self.CP.enableGasInterceptor:
ret.gasPressed = self.CS.pedal_gas > 0
else:
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.SOUL else 0.1 #2018.09.03 change CIVIC to soul
ret.brakeLights = bool(self.CS.brake_switch or
c.actuators.brake > brakelights_threshold)
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# ret.steeringRate = self.CS.angle_steers_rate #2018.09.11 TODO some can duplication error? or not reading
# gear shifter lever (#2018.09.04 define in carstate.py
ret.gearShifter = self.CS.gear_shifter
#2018.09.05 #TODO find steering torque value from steering wheel
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override #2018.09.02 this come values py when steering operator override =1
#2018.09.02 TODO need to check is matter, we not using kia cruise control
# cruise state
#ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.enabled = False #2018.09.11 if use Openpilot cruise, no need, this should set to 0, need set to False
#ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS #2018.09.11 check in controlsd.py if not enable cruise, this not needed
ret.cruiseState.speed = 0 # 2018.09.11 check in controlsd.py if not enable cruise, this not needed, set to 0
ret.cruiseState.available = bool(self.CS.main_on) #2018.09.11 not use anywhere else beside interface.py
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset #2018.09.11 finally not use in controlsd.py if we use interceptor and enablecruise=false
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = '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 = 'accelCruise'
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = '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 = 'altButton1'
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
# TODO: I don't like the way capnp does enums
# These strings aren't checked at compile time
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
elif self.CS.steer_warning:
events.append(create_event('steerTempUnavailable', [ET.WARNING]))
if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == "reverse":
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
#remove brake hold and #electric parking brake
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# 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.:
# non loud alert if cruise disbales below 25mph as expected (+ a little margin)
if ret.vEgo < self.CP.minEnableSpeed + 2.:
events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
else:
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.append(create_event('manualRestart', [ET.WARNING]))
cur_time = sec_since_boot()
enable_pressed = False
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "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.append(create_event('buttonCancel', [ET.USER_DISABLE]))
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 enble
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 get_events(events, [ET.NO_ENTRY])):
events.append(create_event('buttonEnable', [ET.ENABLE]))
self.last_enable_sent = cur_time
elif enable_pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, c):
self.pt_cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.pt_cp)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal information.
ret.gas = self.CS.pedal_gas / 254.0
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake / 0xd0
ret.brakePressed = self.CS.brake_pressed
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.gearShifter = self.CS.gear_shifter
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = '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:
be.type = 'accelCruise'
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if self.CS.steer_error:
events.append(
create_event(
'steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.car_fingerprint == CAR.VOLT:
if self.CS.brake_error:
events.append(
create_event(
'brakeUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.gear_shifter == 3:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if self.CS.park_brake:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if ret.cruiseState.standstill:
events.append(create_event('resumeRequired', [ET.WARNING]))
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == "cancel" and b.pressed:
events.append(
create_event('buttonCancel', [ET.USER_DISABLE]))
if self.CS.car_fingerprint == CAR.CADILLAC_CT6:
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
ret.events = events
# update previous brake/gas pressed
self.acc_active_prev = self.CS.acc_active
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
return ret.as_reader()
def data_sample(CI, CC, thermal, calibration, health, driver_monitor,
gps_location, poller, cal_status, overtemp, free_space,
driver_status, geofence, state, mismatch_counter, params):
# *** read can and compute car states ***
CS = CI.update(CC)
events = list(CS.events)
enabled = isEnabled(state)
td = None
cal = None
hh = None
dm = None
gps = None
for socket, event in poller.poll(0):
if socket is thermal:
td = messaging.recv_one(socket)
elif socket is calibration:
cal = messaging.recv_one(socket)
elif socket is health:
hh = messaging.recv_one(socket)
elif socket is driver_monitor:
dm = messaging.recv_one(socket)
elif socket is gps_location:
gps = messaging.recv_one(socket)
# *** thermal checking logic ***
# thermal data, checked every second
if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
# under 15% of space free no enable allowed
free_space = td.thermal.freeSpace < 0.15
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# *** read calibration status ***
if cal is not None:
cal_status = cal.liveCalibration.calStatus
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationInProgress',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not enabled:
mismatch_counter = 0
# *** health checking logic ***
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 2:
events.append(
create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
if dm is not None:
driver_status.get_pose(dm.driverMonitoring, params)
if geofence is not None and gps is not None:
geofence.update_geofence_status(gps.gpsLocationExternal, params)
if geofence is not None and not geofence.in_geofence:
events.append(create_event('geofence', [ET.NO_ENTRY, ET.WARNING]))
return CS, events, cal_status, overtemp, free_space, mismatch_counter
def update(self, c, can_strings):
self.dp_load_params('gm')
self.cp.update_strings(can_strings)
ret = self.CS.update(self.cp)
ret.canValid = self.cp.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.cruiseState.available = self.CS.main_on
#ret.cruiseState.enabled = self.CS.main_on if not self.CS.regen_pressed else False
ret.cruiseState.enabled = self.CS.main_on
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)
if not ret.cruiseState.available:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.regen_pressed:
events.append(create_event('manualSteeringRequired', [ET.WARNING]))
if ret.cruiseState.enabled and not self.cruise_enable_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if self.CS.park_brake:
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
# handle button presses
for b in ret.buttonEvents:
if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
self.cruise_enable_prev = ret.cruiseState.enabled
# copy back carState packet to CS
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, c, can_strings):
self.pt_cp.update_strings(can_strings)
self.cam_cp.update_strings(can_strings)
self.CS.update(self.pt_cp, self.cam_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
ret.gas = self.CS.pedal_gas / 255.
ret.gasPressed = self.CS.user_gas_pressed
# cruise state
ret.cruiseState.enabled = bool(self.CS.acc_active)
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
ret.doorOpen = self.CS.door_open
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.accelCruise
buttonEvents.append(be)
events = []
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on gas pedal rising edge
if (ret.gasPressed and not self.gas_pressed_prev):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
ret.events = events
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.acc_active_prev = self.CS.acc_active
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
ret.gas = self.CS.car_gas / 256.0
if not self.CP.enableGas:
ret.gasPressed = self.CS.pedal_gas > 0
else:
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.civic else 0.1
ret.brakeLights = bool(self.CS.brake_switch
or c.actuators.brake > brakelights_threshold)
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
# gear shifter lever
ret.gearShifter = self.CS.gear_shifter
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = '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 = 'accelCruise'
elif but == CruiseButtons.DECEL_SET:
be.type = 'decelCruise'
elif but == CruiseButtons.CANCEL:
be.type = 'cancel'
elif but == CruiseButtons.MAIN:
be.type = 'altButton3'
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = '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 = 'altButton1'
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
# TODO: I don't like the way capnp does enums
# These strings aren't checked at compile time
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if self.CS.steer_error:
events.append(
create_event('steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
elif self.CS.steer_not_allowed:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if self.CS.brake_error:
events.append(
create_event('brakeUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not ret.gearShifter == 'drive':
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.door_all_closed:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.seatbelt:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.brake_hold:
events.append(
create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.park_brake:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# 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
# TODO: for the Acura, cancellation below 25mph is normal. Issue a non loud alert
if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.:
events.append(
create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
if not self.CS.civic and ret.vEgo < 0.001:
events.append(create_event('manualRestart', [ET.WARNING]))
cur_time = sec_since_boot()
enable_pressed = False
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
print "enabled pressed at", cur_time
self.last_enable_pressed = cur_time
enable_pressed = True
# do disable on button down
if b.type == "cancel" and b.pressed:
events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
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 enble
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 get_events(events, [ET.NO_ENTRY])):
events.append(create_event('buttonEnable', [ET.ENABLE]))
self.last_enable_sent = cur_time
elif enable_pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
ret.canMonoTimes = canMonoTimes
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, c, can_strings):
self.cp.update_strings(can_strings)
self.cp2.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp2, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp2.can_valid and self.cp_cam.can_valid
# most HKG cars has no long control, it is safer and easier to engage by main on
ret.cruiseState.enabled = ret.cruiseState.available if not self.CC.longcontrol else ret.cruiseState.enabled
# some Optima only has blinker flash signal
if self.CP.carFingerprint == CAR.KIA_OPTIMA:
ret.leftBlinker = self.CS.left_blinker_flash or self.CS.prev_left_blinker and self.CC.turning_signal_timer
ret.rightBlinker = self.CS.right_blinker_flash or self.CS.prev_right_blinker and self.CC.turning_signal_timer
# turning indicator alert logic
if (ret.leftBlinker or ret.rightBlinker
or self.CC.turning_signal_timer):
self.turning_indicator_alert = True
else:
self.turning_indicator_alert = False
# LKAS button alert logic: reverse on/off
#if not self.CS.lkas_error and self.CS.lkas_button_on != self.CS.prev_lkas_button_on:
#self.CC.lkas_button_on = not self.CC.lkas_button_on
#self.lkas_button_alert = not self.CC.lkas_button_on
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed +
0.2) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 0.7):
self.low_speed_alert = False
ret.buttonEvents = []
events = []
if not ret.gearShifter == GearShifter.drive:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.espDisabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not ret.cruiseState.available:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(
create_event('reverseGear', [ET.NO_ENTRY, ET.USER_DISABLE]))
#if ret.steerWarning or abs(ret.steeringAngle) > 120.:
# events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING]))
if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if ((ret.gasPressed and not self.CS.out.gasPressed) or \
(ret.brakePressed and (not self.CS.out.brakePressed or ret.vEgoRaw > 0.1))) and self.CC.longcontrol:
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed and self.CC.longcontrol:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.low_speed_alert and not self.CS.mdps_bus:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
if self.turning_indicator_alert:
events.append(create_event('turningIndicatorOn', [ET.WARNING]))
if self.lkas_button_alert:
events.append(create_event('lkasButtonOff', [ET.WARNING]))
#TODO Varible for min Speed for LCA
if ret.rightBlinker and ret.rightBlindspot and ret.vEgo > (
45 * CV.MPH_TO_MS):
events.append(create_event('rightLCAbsm', [ET.WARNING]))
if ret.leftBlinker and ret.leftBlindspot and ret.vEgo > (45 *
CV.MPH_TO_MS):
events.append(create_event('leftLCAbsm', [ET.WARNING]))
ret.events = events
self.CS.out = ret.as_reader()
return self.CS.out
def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
"""Gets called when new live20 is available"""
cur_time = live20.logMonoTime / 1e9
v_ego = CS.carState.vEgo
long_control_state = live100.live100.longControlState
v_cruise_kph = live100.live100.vCruise
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
self.last_md_ts = md.logMonoTime
self.radar_errors = list(live20.live20.radarErrors)
self.lead_1 = live20.live20.leadOne
self.lead_2 = live20.live20.leadTwo
enabled = (long_control_state
== LongCtrlState.pid) or (long_control_state
== LongCtrlState.stopping)
following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
self.v_speedlimit = NO_CURVATURE_SPEED
self.v_curvature = NO_CURVATURE_SPEED
self.map_valid = live_map_data.liveMapData.mapValid
# Speed limit and curvature
set_speed_limit_active = self.params.get(
"LimitSetSpeed") == "1" and self.params.get(
"SpeedLimitOffset") is not None
if set_speed_limit_active:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
self.v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
self.decel_for_turn = bool(self.v_curvature < min(
[v_cruise_setpoint, self.v_speedlimit, v_ego + 1.]))
v_cruise_setpoint = min(
[v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
accel_limits = map(float,
calc_cruise_accel_limits(v_ego, following))
jerk_limits = [
min(-0.1, accel_limits[0]),
max(0.1, accel_limits[1])
] # TODO: make a separate lookup for jerk tuning
accel_limits = limit_accel_in_turns(v_ego,
CS.carState.steeringAngle,
accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration
accel_limits[1] = min(accel_limits[1], AWARENESS_DECEL)
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
if self.decel_for_turn:
time_to_turn = max(
1.0, live_map_data.liveMapData.distToTurn /
max(self.v_cruise, 1.))
required_decel = min(0, (self.v_curvature - self.v_cruise) /
time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(
self.v_acc_start, self.a_acc_start, v_cruise_setpoint,
accel_limits[1], accel_limits[0], jerk_limits[1],
jerk_limits[0], _DT_MPC)
# cruise speed can't be negative even is user is distracted
self.v_cruise = max(self.v_cruise, 0.)
else:
starting = long_control_state == LongCtrlState.starting
a_ego = min(CS.carState.aEgo, 0.0)
reset_speed = MIN_CAN_SPEED if starting else v_ego
reset_accel = self.CP.startAccel if starting else a_ego
self.v_acc = reset_speed
self.a_acc = reset_accel
self.v_acc_start = reset_speed
self.a_acc_start = reset_accel
self.v_cruise = reset_speed
self.a_cruise = reset_accel
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
# determine fcw
if self.mpc1.new_lead:
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
self.fcw = self.fcw_checker.update(
self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
self.lead_1.yRel, self.lead_1.vLat, self.lead_1.fcw,
blinkers) and not CS.carState.brakePressed
if self.fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = []
if model_dead:
events.append(
create_event('modelCommIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if 'fault' in self.radar_errors:
events.append(
create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
plan_send.plan.events = events
plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = live20.logMonoTime
# longitudal plan
plan_send.plan.vCruise = self.v_cruise
plan_send.plan.aCruise = self.a_cruise
plan_send.plan.vStart = self.v_acc_start
plan_send.plan.aStart = self.a_acc_start
plan_send.plan.vTarget = self.v_acc
plan_send.plan.aTarget = self.a_acc
plan_send.plan.vTargetFuture = self.v_acc_future
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
plan_send.plan.vCurvature = self.v_curvature
plan_send.plan.decelForTurn = self.decel_for_turn
plan_send.plan.mapValid = self.map_valid
# Send out fcw
fcw = self.fcw and (self.fcw_enabled
or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
# Interpolate 0.05 seconds and save as starting point for next iteration
dt = 0.05 # s
a_acc_sol = self.a_acc_start + (dt / _DT_MPC) * (self.a_acc -
self.a_acc_start)
v_acc_sol = self.v_acc_start + dt * (a_acc_sol +
self.a_acc_start) / 2.0
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.aEgo = self.CS.a_ego
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gear shifter
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed
ret.brakeLights = self.CS.brake_lights
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status # same as main_on
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = self.CS.main_on
ret.cruiseState.speedOffset = 0.
# ignore standstill in hybrid rav4, since pcm allows to restart without
# receiving any special command
ret.cruiseState.standstill = False
ret.readdistancelines = 1
ret.genericToggle = False
ret.laneDepartureToggle = False
ret.distanceToggle = 1
ret.accSlowToggle = False
ret.blindspot = False
# TODO: button presses
buttonEvents = []
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed)
ret.genericToggle = self.CS.generic_toggle
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
disengage_event = True
else:
disengage_event = False
ret.gasbuttonstatus = self.CS.gasMode
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(
create_event('commIssue',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if not (ret.gearShifter in ('drive', 'low')):
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen and disengage_event:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched and disengage_event:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(
create_event(
'steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC
# from a 3+ second stop.
if self.CS.cstm_btns.get_button_status("mad") == 0:
if (ret.gasPressed and (not self.gas_pressed_prev)
and ret.vEgo > 2.0):
events.append(
create_event('pedalPressed',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if self.low_speed_alert:
events.append(create_event('belowSteerSpeed', [ET.WARNING]))
ret.events = events
ret.canMonoTimes = canMonoTimes
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal
ret.gas = self.CS.car_gas / 256.0
if not self.CP.enableGasInterceptor:
ret.gasPressed = self.CS.pedal_gas > 0
else:
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake
ret.brakePressed = self.CS.brake_pressed != 0
# 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
c.actuators.brake > brakelights_threshold)
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringRate = self.CS.angle_steers_rate
# gear shifter lever
ret.gearShifter = self.CS.gear_shifter
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringTorqueEps = self.CS.steer_torque_motor
ret.steeringPressed = self.CS.steer_override
# cruise state
ret.cruiseState.enabled = self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode)
ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
ret.leftBlinker = bool(self.CS.left_blinker_on)
ret.rightBlinker = bool(self.CS.right_blinker_on)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.stockAeb = self.CS.stock_aeb
ret.stockFcw = self.CS.stock_fcw
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on != 0
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on != 0
buttonEvents.append(be)
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 = []
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
elif self.CS.steer_warning:
events.append(create_event('steerTempUnavailable', [ET.WARNING]))
if self.CS.brake_error:
events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not ret.gearShifter == GearShifter.drive:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on or self.CS.cruise_mode:
events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gearShifter == GearShifter.reverse:
events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH:
events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.park_brake:
events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
# 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 disbales below 25mph as expected (+ a little margin)
if ret.vEgo < self.CP.minEnableSpeed + 2.:
events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE]))
else:
events.append(create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.append(create_event('manualRestart', [ET.WARNING]))
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.append(create_event('buttonCancel', [ET.USER_DISABLE]))
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 get_events(events, [ET.NO_ENTRY])):
events.append(create_event('buttonEnable', [ET.ENABLE]))
self.last_enable_sent = cur_time
elif enable_pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
ret.events = events
# update previous brake/gas pressed
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# steering wheel
ret.steeringAngle = self.CS.angle_steers
ret.steeringPressed = self.CS.steer_override
# gas pedal
ret.gas = self.CS.user_gas / 100.
ret.gasPressed = self.CS.user_gas > 0.0001
ret.brakePressed = self.CS.brake_pressed
ret.brakeLights = self.CS.brake_lights
ret.cruiseState.enabled = not (self.CS.pcm_acc_status in [0, 3])
ret.cruiseState.speed = self.CS.v_cruise_pcm
ret.cruiseState.available = self.CS.pcm_acc_status != 0
ret.genericToggle = self.CS.generic_toggle
# events
events = []
if self.CS.steer_error:
events.append(
create_event(
'steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
# enable request in prius is simple, as we activate when Toyota is active (rising edge)
if ret.cruiseState.enabled and not self.cruise_enabled_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
elif not ret.cruiseState.enabled:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if self.CS.lkas_state not in [
2, 3
] and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled:
events.append(
create_event('steerTempUnavailableMute', [ET.WARNING]))
ret.events = events
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled
return ret.as_reader()
def update(self, c, can_strings):
self.pt_cp.update_strings(can_strings)
self.ch_cp.update_strings(can_strings)
self.CS.update(self.pt_cp, self.ch_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = self.pt_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD,
self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# gas pedal information.
ret.gas = self.CS.pedal_gas / 254.0
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brake = self.CS.user_brake / 0xd0
ret.brakePressed = self.CS.brake_pressed
ret.brakeLights = self.CS.frictionBrakesActive
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF
ret.cruiseState.enabled = cruiseEnabled
ret.cruiseState.standstill = False
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
ret.gearShifter = self.CS.gear_shifter
ret.readdistancelines = self.CS.follow_level
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.leftBlinker
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.rightBlinker
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
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 (cruiseEnabled and self.CS.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:
if not cruiseEnabled and not self.CS.lkMode:
self.lkMode = True
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
if cruiseEnabled and self.CS.lka_button and self.CS.lka_button != self.CS.prev_lka_button:
self.CS.lkMode = not self.CS.lkMode
if self.CS.distance_button and self.CS.distance_button != self.CS.prev_distance_button:
self.CS.follow_level -= 1
if self.CS.follow_level < 1:
self.CS.follow_level = 3
events = []
if not self.CS.lkMode:
events.append(create_event('manualSteeringRequired', [ET.WARNING]))
#if cruiseEnabled and (self.CS.left_blinker_on or self.CS.right_blinker_on):
# events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING]))
if self.CS.steer_error:
events.append(
create_event(
'steerUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.car_fingerprint in SUPERCRUISE_CARS:
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
else:
if self.CS.brake_error:
events.append(
create_event(
'brakeUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if not self.CS.gear_shifter_valid:
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.esp_disabled:
events.append(
create_event('espDisabled',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not self.CS.main_on:
events.append(
create_event('wrongCarMode',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CS.gear_shifter == 3:
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
if self.CS.park_brake:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
# disable on pedals rising edge or when brake is pressed and speed isn't zero
if (ret.gasPressed and not self.gas_pressed_prev) or \
(ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
events.append(
create_event('pedalPressed',
[ET.NO_ENTRY, ET.USER_DISABLE]))
if ret.gasPressed:
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if ret.cruiseState.standstill:
events.append(create_event('resumeRequired', [ET.WARNING]))
if self.CS.pcm_acc_status == AccState.FAULTED:
events.append(
create_event('controlsFailed',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# handle button presses
for b in ret.buttonEvents:
# do enable on both accel and decel buttons
# The ECM will fault if resume triggers an enable while speed is set to 0
if b.type == ButtonType.accelCruise and c.hudControl.setSpeed > 0 and c.hudControl.setSpeed < 70 and not b.pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
if b.type == ButtonType.decelCruise and not b.pressed:
events.append(create_event('buttonEnable', [ET.ENABLE]))
# do disable on button down
if b.type == ButtonType.cancel and b.pressed:
events.append(
create_event('buttonCancel', [ET.USER_DISABLE]))
# The ECM independently tracks a ‘speed is set’ state that is reset on main off.
# To keep controlsd in sync with the ECM state, generate a RESET_V_CRUISE event on main cruise presses.
if b.type == ButtonType.altButton3 and b.pressed:
events.append(
create_event('buttonCancel',
[ET.RESET_V_CRUISE, ET.USER_DISABLE]))
ret.events = events
# update previous brake/gas pressed
self.acc_active_prev = self.CS.acc_active
self.gas_pressed_prev = ret.gasPressed
self.brake_pressed_prev = ret.brakePressed
# cast to reader so it can't be modified
return ret.as_reader()
def data_sample(CI, CC, sm, can_sock, state, mismatch_counter,
can_error_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
CS = CI.update(CC, can_strs)
sm.update(0)
events = list(CS.events)
events += list(sm['dMonitoringState'].events)
add_lane_change_event(events, sm['pathPlan'])
enabled = isEnabled(state)
lane_change_bsm = sm['pathPlan'].laneChangeBSM
# Check for CAN timeout
if not can_strs:
can_error_counter += 1
events.append(
create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
free_space = sm[
'thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = sm['thermal'].batteryPercent < 1 and sm[
'thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed
mem_low = sm['thermal'].memUsedPercent > 90
#bsm alerts
if lane_change_bsm == LaneChangeBSM.left:
events.append(create_event('preventLCA', [ET.WARNING]))
if lane_change_bsm == LaneChangeBSM.right:
events.append(create_event('preventLCA', [ET.WARNING]))
# Create events for battery, temperature and disk space
if low_battery:
events.append(
create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
if mem_low:
events.append(
create_event('lowMemory',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
if CS.stockAeb:
events.append(create_event('stockAeb', []))
# Handle calibration
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
events.append(
create_event('calibrationIncomplete',
[ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
else:
events.append(
create_event('calibrationInvalid',
[ET.NO_ENTRY, ET.SOFT_DISABLE]))
if CS.vEgo > 92 * CV.MPH_TO_MS:
events.append(
create_event('speedTooHigh', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
controls_allowed = sm['health'].controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 200:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
return CS, events, cal_perc, mismatch_counter, can_error_counter
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev,
last_blinker_frame, is_ldw_enabled):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
CC = car.CarControl.new_message()
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# Some override values for Honda
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = sm['plan'].hasLead
right_lane_visible = sm['pathPlan'].rProb > 0.5
left_lane_visible = sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown
ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state)
md = sm['model']
if len(md.meta.desirePrediction):
l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1]
r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1]
l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
if ldw_allowed:
CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart:
AM.add(sm.frame, 'ldwPermanent', False)
events.append(create_event('ldw', [ET.PERMANENT]))
AM.process_alerts(sm.frame)
CC.hudControl.visualAlert = AM.visual_alert
if not read_only:
# send car controls over can
can_sends = CI.apply(CC)
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = driver_status.awareness < 0.
# controlsState
dat = messaging.new_message()
dat.init('controlsState')
dat.valid = CS.canValid
dat.controlsState = {
"alertText1": AM.alert_text_1,
"alertText2": AM.alert_text_2,
"alertSize": AM.alert_size,
"alertStatus": AM.alert_status,
"alertBlinkingRate": AM.alert_rate,
"alertType": AM.alert_type,
"alertSound": AM.audible_alert,
"awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0,
"driverMonitoringOn": bool(driver_status.face_detected),
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": sm.logMonoTime['plan'],
"pathPlanMonoTime": sm.logMonoTime['pathPlan'],
"enabled": isEnabled(state),
"active": isActive(state),
"vEgo": CS.vEgo,
"vEgoRaw": CS.vEgoRaw,
"angleSteers": CS.steeringAngle,
"curvature": VM.calc_curvature((CS.steeringAngle - sm['pathPlan'].angleOffset) * CV.DEG_TO_RAD, CS.vEgo),
"steerOverride": CS.steeringPressed,
"state": state,
"engageable": not bool(get_events(events, [ET.NO_ENTRY])),
"longControlState": LoC.long_control_state,
"vPid": float(LoC.v_pid),
"vCruise": float(v_cruise_kph),
"upAccelCmd": float(LoC.pid.p),
"uiAccelCmd": float(LoC.pid.i),
"ufAccelCmd": float(LoC.pid.f),
"angleSteersDes": float(LaC.angle_steers_des),
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(sm['plan'].jerkFactor),
"gpsPlannerActive": sm['plan'].gpsPlannerActive,
"vCurvature": sm['plan'].vCurvature,
"decelForModel": sm['plan'].longitudinalPlanSource == log.Plan.LongitudinalPlanSource.model,
"cumLagMs": -rk.remaining * 1000.,
"startMonoTime": int(start_time * 1e9),
"mapValid": sm['plan'].mapValid,
"forceDecel": bool(force_decel),
}
if CP.lateralTuning.which() == 'pid':
dat.controlsState.lateralControlState.pidState = lac_log
elif CP.lateralTuning.which() == 'lqr':
dat.controlsState.lateralControlState.lqrState = lac_log
elif CP.lateralTuning.which() == 'indi':
dat.controlsState.lateralControlState.indiState = lac_log
pm.send('controlsState', dat)
# carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = events
pm.send('carState', cs_send)
# carEvents - logged every second or on change
events_bytes = events_to_bytes(events)
if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev):
ce_send = messaging.new_message()
ce_send.init('carEvents', len(events))
ce_send.carEvents = events
pm.send('carEvents', ce_send)
# carParams - logged every 50 seconds (> 1 per segment)
if (sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message()
cp_send.init('carParams')
cp_send.carParams = CP
pm.send('carParams', cp_send)
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
pm.send('carControl', cc_send)
return CC, events_bytes
def update(self, c):
self.pt_cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.pt_cp)
# create message
ret = car.CarState.new_message()
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
ret.vEgoRaw = self.CS.v_ego_raw
ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
ret.standstill = self.CS.standstill
ret.wheelSpeeds.fl = self.CS.v_wheel_fl
ret.wheelSpeeds.fr = self.CS.v_wheel_fr
ret.wheelSpeeds.rl = self.CS.v_wheel_rl
ret.wheelSpeeds.rr = self.CS.v_wheel_rr
# steering wheel
ret.steeringAngle = self.CS.angle_steers
# torque and user override. Driver awareness
# timer resets when the user uses the steering wheel.
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
buttonEvents = []
# blinkers
if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.left_blinker_on
buttonEvents.append(be)
if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.right_blinker_on
buttonEvents.append(be)
be = car.CarState.ButtonEvent.new_message()
be.type = 'accelCruise'
buttonEvents.append(be)
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
if self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
else:
self.can_invalid_count = 0
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
## handle button presses
#for b in ret.buttonEvents:
# # do enable on both accel and decel buttons
# if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
# events.append(create_event('buttonEnable', [ET.ENABLE]))
# # do disable on button down
# if b.type == "cancel" and b.pressed:
# events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
ret.events = events
# update previous brake/gas pressed
self.acc_active_prev = self.CS.acc_active
# cast to reader so it can't be modified
return ret.as_reader()
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params):
"""Receive data from sockets and create events for battery, temperature and disk space"""
# Update carstate from CAN and create events
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
CS = CI.update(CC, can_strs)
sm.update(0)
events = list(CS.events)
add_lane_change_event(events, sm['pathPlan'])
enabled = isEnabled(state)
# Check for CAN timeout
if not can_strs:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
free_space = sm['thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed
mem_low = sm['thermal'].memUsedPercent > 90
# Create events for battery, temperature and disk space
if low_battery:
events.append(create_event('lowBattery', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if overtemp:
events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if free_space:
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
if mem_low:
events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
if CS.stockAeb:
events.append(create_event('stockAeb', []))
# GPS coords RHD parsing, once every restart
if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked:
is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
driver_status.is_rhd_region = is_rhd
driver_status.is_rhd_region_checked = True
put_nonblocking("IsRHD", "1" if is_rhd else "0")
# Handle calibration
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
cal_rpy = [0,0,0]
#if cal_status != Calibration.CALIBRATED:
# if cal_status == Calibration.UNCALIBRATED:
# events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
# else:
# events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
#else:
# rpy = sm['liveCalibration'].rpyCalib
# if len(rpy) == 3:
# cal_rpy = rpy
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
controls_allowed = sm['health'].controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 200:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if sm.updated['model']:
driver_status.set_policy(sm['model'])
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled)
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
return CS, events, cal_perc, mismatch_counter
def controlsd_thread(gctx=None, rate=100):
gc.disable()
# start the loop
set_realtime_priority(3)
context = zmq.Context()
params = Params()
# Pub Sockets
live100 = messaging.pub_sock(context, service_list['live100'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
# No sendcan if passive
if not passive:
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
else:
sendcan = None
# Sub sockets
poller = zmq.Poller()
thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller)
driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller)
plan_sock = messaging.sub_sock(context, service_list['plan'].port, conflate=True, poller=poller)
path_plan_sock = messaging.sub_sock(context, service_list['pathPlan'].port, conflate=True, poller=poller)
logcan = messaging.sub_sock(context, service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan, 1.0 if passive else None)
if CI is None:
raise Exception("unsupported car")
# if stock camera is connected, then force passive behavior
if not CP.enableCamera:
passive = True
sendcan = None
if passive:
CP.safetyModel = car.CarParams.SafetyModels.noOutput
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(CP)
AM = AlertManager()
driver_status = DriverStatus()
if not passive:
AM.add("startup", False)
# Write CarParams for radard and boardd safety mode
params.put("CarParams", CP.to_bytes())
params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0")
state = State.disabled
soft_disable_timer = 0
v_cruise_kph = 255
v_cruise_kph_last = 0
overtemp = False
free_space = False
cal_status = Calibration.INVALID
cal_perc = 0
mismatch_counter = 0
low_battery = False
plan = messaging.new_message()
plan.init('plan')
path_plan = messaging.new_message()
path_plan.init('pathPlan')
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
controls_params = params.get("ControlsParams")
# Read angle offset from previous drive
if controls_params is not None:
controls_params = json.loads(controls_params)
angle_offset = controls_params['angle_offset']
else:
angle_offset = 0.
prof = Profiler(False) # off by default
while True:
start_time = int(sec_since_boot() * 1e9)
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\
data_sample(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status,
state, mismatch_counter, params, plan, path_plan)
prof.checkpoint("Sample")
path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
plan_age = (start_time - plan.logMonoTime) / 1e9
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
events += list(plan.plan.events)
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not passive:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \
state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status,
LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
prof.display()
