def state_transition(frame, CS, CP, state, events, soft_disable_timer,
v_cruise_kph, AM, sm_smiskol, df_alert_manager):
"""Compute conditional state transitions and execute actions on state transitions"""
enabled = isEnabled(state)
v_cruise_kph_last = v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not CP.enableCruise:
v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled)
elif CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
df_alert = df_alert_manager.update(sm_smiskol)
if df_alert is not None:
AM.add(
frame,
'dfButtonAlert',
enabled,
extra_text_1=df_alert,
extra_text_2='Dynamic follow: {} profile active'.format(df_alert))
# DISABLED
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(frame, str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add(frame, "enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents,
v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.enabled
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif soft_disable_timer <= 0:
state = State.disabled
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(frame, e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.enabled
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset,
passive, is_metric):
# Given the state, this function returns the actuators
# reset actuators to zero
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
# check if user has interacted with the car
driver_engaged = len(CS.buttonEvents) > 0 or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed
# add eventual driver distracted events
events = driver_status.update(events, driver_engaged, isActive(state),
CS.standstill)
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# ***** state specific actions *****
# DISABLED
if state in [State.preEnabled, State.disabled]:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING
elif state in [State.enabled, State.softDisabling]:
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
extra_text = ''
if e == "belowSteerSpeed":
if is_metric:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(e, enabled, extra_text=extra_text)
# *** angle offset learning ***
if rk.frame % 5 == 2 and plan.lateralValid:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob,
CS.steeringAngle, CS.steeringPressed)
# *** gas/brake PID loop ***
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo,
CS.brakePressed, CS.standstill,
CS.cruiseState.standstill,
v_cruise_kph, plan.vTarget,
plan.vTargetFuture,
plan.aTarget, CP, PL.lead_1)
# *** steering PID loop ***
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo,
CS.steeringAngle,
CS.steeringPressed,
plan.dPoly,
angle_offset, VM, PL)
# send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
AM.add("steerSaturated", enabled)
# parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
AM.add(str(e) + "Permanent", enabled)
# *** process alerts ***
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, driver_status, angle_offset
def data_send(perception_state, plan, plan_ts, CS, CI, CP, VM, state, events,
actuators, v_cruise_kph, rk, carstate, carcontrol, live100,
livempc, AM, driver_status, LaC, LoC, angle_offset, passive):
# ***** control the car *****
CC = car.CarControl.new_message()
if not passive:
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
# always cancel if we have an interceptor
CC.cruiseControl.cancel = not CP.enableCruise or (
not isEnabled(state) and CS.cruiseState.enabled)
# brake discount removes a sharp nonlinearity
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0))
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, plan.aTarget, CS.vEgo, 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 = plan.hasLead
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC, perception_state)
# ***** publish state to logger *****
# publish controls state at 100Hz
dat = messaging.new_message()
dat.init('live100')
dat.live100 = {
"alertText1":
AM.alert_text_1,
"alertText2":
AM.alert_text_2,
"alertSize":
AM.alert_size,
"alertStatus":
AM.alert_status,
"alertBlinkingRate":
AM.alert_rate,
"awarenessStatus":
max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn":
bool(driver_status.monitor_on),
"canMonoTimes":
list(CS.canMonoTimes),
"planMonoTime":
plan_ts,
"enabled":
isEnabled(state),
"active":
isActive(state),
"vEgo":
CS.vEgo,
"vEgoRaw":
CS.vEgoRaw,
"angleSteers":
CS.steeringAngle,
"curvature":
VM.calc_curvature(CS.steeringAngle * 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),
"upSteer":
float(LaC.pid.p),
"uiSteer":
float(LaC.pid.i),
"ufSteer":
float(LaC.pid.f),
"vTargetLead":
float(plan.vTarget),
"aTarget":
float(plan.aTarget),
"jerkFactor":
float(plan.jerkFactor),
"angleOffset":
float(angle_offset),
"gpsPlannerActive":
plan.gpsPlannerActive,
"cumLagMs":
-rk.remaining * 1000.,
}
live100.send(dat.to_bytes())
# broadcast carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
# broadcast carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
# publish mpc state at 20Hz
if hasattr(LaC, 'mpc_updated') and LaC.mpc_updated:
dat = messaging.new_message()
dat.init('liveMpc')
dat.liveMpc.x = list(LaC.mpc_solution[0].x)
dat.liveMpc.y = list(LaC.mpc_solution[0].y)
dat.liveMpc.psi = list(LaC.mpc_solution[0].psi)
dat.liveMpc.delta = list(LaC.mpc_solution[0].delta)
dat.liveMpc.cost = LaC.mpc_solution[0].cost
livempc.send(dat.to_bytes())
return CC
def update(self, c, can_strings):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update_strings(can_strings)
ch_can_valid = self.cp.can_valid
self.epas_cp.update_strings(can_strings)
epas_can_valid = self.epas_cp.can_valid
self.pedal_cp.update_strings(can_strings)
pedal_can_valid = self.pedal_cp.can_valid
can_rcv_error = not (ch_can_valid and epas_can_valid
and pedal_can_valid)
self.CS.update(self.cp, self.epas_cp, self.pedal_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = ch_can_valid #and epas_can_valid #and pedal_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, we don't use with with interceptor so it's always 0/False
ret.gas = self.CS.user_gas
if not self.CP.enableGasInterceptor:
ret.gasPressed = self.CS.user_gas_pressed
else:
ret.gasPressed = self.CS.user_gas_pressed
# brake pedal
ret.brakePressed = False # (self.CS.brake_pressed != 0) and (self.CS.cstm_btns.get_button_status("brake") == 0)
# FIXME: read sendcan for brakelights
brakelights_threshold = 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 = True #self.CS.pcm_acc_status != 0
ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS * (
CV.MPH_TO_KPH if self.CS.imperial_speed_units else 1.)
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
ret.cruiseState.standstill = False
# TODO: button presses
buttonEvents = []
ret.leftBlinker = bool(self.CS.turn_signal_state_left == 1)
ret.rightBlinker = bool(self.CS.turn_signal_state_right == 1)
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
if self.CS.prev_turn_signal_stalk_state != self.CS.turn_signal_stalk_state:
if self.CS.turn_signal_stalk_state == 1 or self.CS.prev_turn_signal_stalk_state == 1:
be = car.CarState.ButtonEvent.new_message()
be.type = 'leftBlinker'
be.pressed = self.CS.turn_signal_stalk_state == 1
buttonEvents.append(be)
if self.CS.turn_signal_stalk_state == 2 or self.CS.prev_turn_signal_stalk_state == 2:
be = car.CarState.ButtonEvent.new_message()
be.type = 'rightBlinker'
be.pressed = self.CS.turn_signal_stalk_state == 2
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_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = 'unknown'
be.pressed = bool(self.CS.cruise_buttons)
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
events = []
#notification messages for DAS
if (not c.enabled) and (self.CC.opState == 2):
self.CC.opState = 0
if c.enabled and (self.CC.opState == 0):
self.CC.opState = 1
if can_rcv_error:
self.can_invalid_count += 1
if self.can_invalid_count >= 100: #BB increased to 100 to see if we still get the can error messages
events.append(
create_event('invalidGiraffeHonda',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
self.CS.DAS_canErrors = 1
if self.CC.opState == 1:
self.CC.opState = 2
else:
self.can_invalid_count = 0
if self.CS.steer_error:
if not self.CS.enableHSO:
events.append(
create_event('steerUnavailable',
[ET.NO_ENTRY, ET.WARNING]))
elif self.CS.steer_warning:
if not self.CS.enableHSO:
events.append(
create_event('steerTempUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CC.opState == 1:
self.CC.opState = 2
if self.CS.brake_error:
events.append(
create_event(
'brakeUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CC.opState == 1:
self.CC.opState = 2
if not ret.gearShifter == 'drive':
events.append(
create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if c.enabled:
self.CC.DAS_222_accCameraBlind = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
if ret.doorOpen:
events.append(
create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
self.CS.DAS_doorOpen = 1
if self.CC.opState == 1:
self.CC.opState = 0
if ret.seatbeltUnlatched:
events.append(
create_event('seatbeltNotLatched',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if c.enabled:
self.CC.DAS_211_accNoSeatBelt = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
if self.CC.opState == 1:
self.CC.opState = 2
if self.CS.esp_disabled:
events.append(
create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if self.CC.opState == 1:
self.CC.opState = 2
if not self.CS.main_on:
events.append(
create_event('wrongCarMode',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CC.opState == 1:
self.CC.opState = 0
if ret.gearShifter == 'reverse':
events.append(
create_event('reverseGear',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
self.CS.DAS_notInDrive = 1
if self.CC.opState == 1:
self.CC.opState = 0
if self.CS.brake_hold:
events.append(
create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CC.opState == 1:
self.CC.opState = 0
if self.CS.park_brake:
events.append(
create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE]))
if self.CC.opState == 1:
self.CC.opState = 0
if (not c.enabled) and (self.CC.opState == 1):
self.CC.opState = 0
if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed:
events.append(create_event('speedTooLow', [ET.NO_ENTRY]))
# Standard OP method to disengage:
# 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('steerTempUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
#if (self.CS.cstm_btns.get_button_status("brake")>0):
# if ((self.CS.brake_pressed !=0) != self.brake_pressed_prev): #break not canceling when pressed
# self.CS.cstm_btns.set_button_status("brake", 2 if self.CS.brake_pressed != 0 else 1)
#else:
# if ret.brakePressed:
# 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 = 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 == "altButton3" 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 # pylint: disable=chained-comparison
(cur_time - self.last_enable_sent) > 0.2 and
ret.cruiseState.enabled) or \
(enable_pressed and get_events(events, [ET.NO_ENTRY])):
if ret.seatbeltUnlatched:
self.CC.DAS_211_accNoSeatBelt = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
elif not ret.gearShifter == 'drive':
self.CC.DAS_222_accCameraBlind = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
elif not self.CS.apEnabled:
self.CC.DAS_206_apUnavailable = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
else:
events.append(create_event('buttonEnable', [ET.ENABLE]))
self.last_enable_sent = cur_time
elif enable_pressed:
if ret.seatbeltUnlatched:
self.CC.DAS_211_accNoSeatBelt = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
elif not ret.gearShifter == 'drive':
self.CC.DAS_222_accCameraBlind = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
elif not self.CS.apEnabled:
self.CC.DAS_206_apUnavailable = 1
self.CC.warningCounter = 300
self.CC.warningNeeded = 1
else:
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 = self.CS.brake_pressed != 0
# cast to reader so it can't be modified
return ret.as_reader()
def update(self, c):
# ******************* do can recv *******************
can_pub_main = []
canMonoTimes = []
self.CS.update(can_pub_main)
# 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
# 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.cp.vl[0x18F]['STEER_TORQUE_SENSOR']
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
# 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)
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.brakePressed and ret.vEgo < 0.001) or ret.gasPressed:
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. or ret.vEgo < 0.3):
events.append(
create_event("cruiseDisabled", [ET.IMMEDIATE_DISABLE]))
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
#print ret
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.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.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
ret.readdistancelines = self.CS.read_distance_lines
# 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
ret.gasbuttonstatus = self.CS.cstm_btns.get_button_status("gas")
# 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]))
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.:
# 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 state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status, LaC, LoC, VM,
angle_model_bias, passive, is_metric, cal_perc):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
# check if user has interacted with the car
driver_engaged = len(CS.buttonEvents) > 0 or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed
# add eventual driver distracted events
events = driver_status.update(events, driver_engaged, isActive(state),
CS.standstill)
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# State specific actions
if state in [State.preEnabled, State.disabled]:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
elif state in [State.enabled, State.softDisabling]:
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
extra_text = ""
if e == "belowSteerSpeed":
if is_metric:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(e, enabled, extra_text_2=extra_text)
# Run angle offset learner at 20 Hz
if rk.frame % 5 == 2:
angle_model_bias = learn_angle_model_bias(
active, CS.vEgo, angle_model_bias, path_plan.cPoly,
path_plan.cProb, CS.steeringAngle, CS.steeringPressed)
cur_time = sec_since_boot() # TODO: This won't work in replay
mpc_time = plan.l20MonoTime / 1e9
_DT = 0.01 # 100Hz
dt = min(
cur_time - mpc_time, _DT_MPC +
_DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
a_acc_sol = plan.aStart + (dt / _DT_MPC) * (plan.aTarget - plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
# Gas/Brake PID loop
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo,
CS.brakePressed, CS.standstill,
CS.cruiseState.standstill,
v_cruise_kph, v_acc_sol,
plan.vTargetFuture, a_acc_sol,
CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo,
CS.steeringAngle,
CS.steeringPressed, CP,
VM, path_plan)
# Send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
AM.add("steerSaturated", enabled)
# Parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
extra_text_1, extra_text_2 = "", ""
if e == "calibrationIncomplete":
extra_text_1 = str(cal_perc) + "%"
if is_metric:
extra_text_2 = str(int(round(
Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(
Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(str(e) + "Permanent",
enabled,
extra_text_1=extra_text_1,
extra_text_2=extra_text_2)
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
AM, rk, awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle):
# Given the state, this function returns the actuators
# reset actuators to zero
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
for b in CS.buttonEvents:
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed:
rear_view_toggle = True
else:
rear_view_toggle = False
if b.type == "altButton1" and b.pressed:
rear_view_toggle = not rear_view_toggle
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# ***** state specific actions *****
# DISABLED
if state in [State.preEnabled, State.disabled]:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING
elif state in [State.enabled, State.softDisabling]:
# decrease awareness status
awareness_status -= 0.01 / (AWARENESS_TIME)
if awareness_status <= 0.:
AM.add("driverDistracted", enabled)
elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
awareness_status >= (AWARENESS_PRE_TIME - 4.) / AWARENESS_TIME:
AM.add("preDriverDistracted", enabled)
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
AM.add(e, enabled)
# *** angle offset learning ***
if rk.frame % 5 == 2 and plan.lateralValid:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob,
CS.steeringAngle, CS.steeringPressed)
# *** gas/brake PID loop ***
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo,
CS.brakePressed, CS.standstill,
CS.cruiseState.standstill,
v_cruise_kph, plan.vTarget,
plan.vTargetFuture,
plan.aTarget, CP, PL.lead_1)
# *** steering PID loop ***
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, plan.dPoly, angle_offset,
VM, PL)
# send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
AM.add("steerSaturated", enabled)
if CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# reset conditions for the 6 minutes timout
if CS.buttonEvents or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed or \
state in [State.preEnabled, State.disabled]:
awareness_status = 1.
# parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
AM.add(str(e) + "Permanent", enabled)
# *** process alerts ***
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def update(self, c, can_strings):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD,
ret.vEgo)
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
ret.brakeLights = bool(self.CS.brake_switch
or c.actuators.brake > brakelights_threshold)
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_buttons != 0:
be.pressed = True
but = self.CS.cruise_buttons
else:
be.pressed = False
but = self.CS.prev_cruise_buttons
if but == CruiseButtons.RES_ACCEL:
be.type = ButtonType.accelCruise
elif but == CruiseButtons.DECEL_SET:
be.type = ButtonType.decelCruise
elif but == CruiseButtons.CANCEL:
be.type = ButtonType.cancel
elif but == CruiseButtons.MAIN:
be.type = ButtonType.altButton3
buttonEvents.append(be)
if self.CS.cruise_setting != self.CS.prev_cruise_setting:
be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown
if self.CS.cruise_setting != 0:
be.pressed = True
but = self.CS.cruise_setting
else:
be.pressed = False
but = self.CS.prev_cruise_setting
if but == 1:
be.type = ButtonType.altButton1
# TODO: more buttons?
buttonEvents.append(be)
ret.buttonEvents = buttonEvents
# events
events = self.create_common_events(ret)
if self.CS.brake_error:
events.append(
create_event(
'brakeUnavailable',
[ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
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]))
# 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
self.CS.out = ret.as_reader()
return self.CS.out
def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
AM, rk, driver_status, PL, LaC, LoC, VM, angle_offset,
passive, is_metric, cal_perc):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
# check if user has interacted with the car
driver_engaged = len(CS.buttonEvents) > 0 or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed
# add eventual driver distracted events
events = driver_status.update(events, driver_engaged, isActive(state),
CS.standstill)
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# State specific actions
if state in [State.preEnabled, State.disabled]:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
elif state in [State.enabled, State.softDisabling]:
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
extra_text = ""
if e == "belowSteerSpeed":
if is_metric:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(e, enabled, extra_text_2=extra_text)
# Run angle offset learner at 20 Hz
if rk.frame % 5 == 2 and plan.lateralValid:
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob,
CS.steeringAngle, CS.steeringPressed)
if CS.gasbuttonstatus == 0:
CP.gasMaxV = [0.2, 0.2, 0.2]
else:
CP.gasMaxV = [0.3, 0.7, 0.9]
# Gas/Brake PID loop
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo,
CS.brakePressed, CS.standstill,
CS.cruiseState.standstill,
v_cruise_kph, plan.vTarget,
plan.vTargetFuture,
plan.aTarget, CP, PL.lead_1)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle = LaC.update(
active, CS.vEgo, CS.steeringAngle, CS.steeringPressed, plan.dPoly,
angle_offset, CP, VM, PL, CS.blindspot, CS.leftBlinker,
CS.rightBlinker)
#BB added for ALCA support
#CS.pid = LaC.pid
# Send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
AM.add("steerSaturated", enabled)
# Parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
extra_text_1, extra_text_2 = "", ""
if e == "calibrationIncomplete":
extra_text_1 = str(cal_perc) + "%"
if is_metric:
extra_text_2 = str(int(round(
Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(
Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(str(e) + "Permanent",
enabled,
extra_text_1=extra_text_1,
extra_text_2=extra_text_2)
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, driver_status, angle_offset
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph,
AM):
# compute conditional state transitions and execute actions on state transitions
enabled = isEnabled(state)
# handle button presses. TODO: this should be in state_control, but a decelCruise press
# would have the effect of both enabling and changing speed is checked after the state transition
v_cruise_kph_last = v_cruise_kph
for b in CS.buttonEvents:
if not CP.enableCruise and enabled and not b.pressed:
if b.type == "accelCruise":
v_cruise_kph -= (v_cruise_kph %
V_CRUISE_DELTA) - V_CRUISE_DELTA
elif b.type == "decelCruise":
v_cruise_kph -= (v_cruise_kph %
V_CRUISE_DELTA) + V_CRUISE_DELTA
v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
# ***** handle state transitions *****
# DISABLED
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add("enable", enabled)
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
v_cruise_kph = int(
round(max(CS.vEgo * CV.MS_TO_KPH, V_CRUISE_ENABLE_MIN)))
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s TODO: use rate
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.enabled
elif soft_disable_timer <= 0:
state = State.disabled
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.enabled
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph,
rk, AM, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log,
events_prev, last_blinker_frame, is_ldw_enabled,
can_error_counter):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
global trace1
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
left_lane_visible = sm['pathPlan'].lProb
#lane_dPoly = sm['pathPlan'].dPoly
#lane_width = sm['pathPlan'].laneWidth
#lane_lPoly = sm['pathPlan'].lPoly
#lane_rPloy = sm['pathPlan'].rPoly
#str_dPoly = 'P='
#for x in lane_dPoly:
# str_dPoly += '{:.5f},'.format( x )
#str_log = 'L:{:.1f}/{:.1f} w:{:.1f} {}'.format( left_lane_visible, right_lane_visible, lane_width, str_dPoly )
#trace1.printf( str_log )
#traceCS.add( str_log )
lane_visible = right_lane_visible + left_lane_visible
if lane_visible > 1:
CC.hudControl.rightLaneVisible = True
CC.hudControl.leftLaneVisible = True
else:
CC.hudControl.rightLaneVisible = bool(right_lane_visible > 0.4)
CC.hudControl.leftLaneVisible = bool(left_lane_visible > 0.4)
AM.process_alerts(sm.frame)
CC.hudControl.visualAlert = AM.visual_alert # steer hand check.
if not read_only:
# send car controls over can
can_sends = CI.apply(CC, sm, LaC)
pm.send(
'sendcan',
can_list_to_can_capnp(can_sends,
msgtype='sendcan',
valid=CS.canValid))
force_decel = sm['dMonitoringState'].awarenessStatus < 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,
"driverMonitoringOn":
sm['dMonitoringState'].faceDetected,
"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(LaC.pid.p),
"uiAccelCmd":
float(LaC.pid.i),
"ufAccelCmd":
float(LaC.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":
float(LaC.model_sum), # 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),
"canErrorCounter":
trace1.cruise_set_mode,
"alertTextMsg1":
str(trace1.global_alertTextMsg1),
"alertTextMsg2":
str(trace1.global_alertTextMsg2),
}
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 state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, sm, poller, arne182Status):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
# check if user has interacted with the car
driver_engaged = len(CS.buttonEvents) > 0 or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed
# add eventual driver distracted events
events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill)
# send FCW alert if triggered by planner
if plan.fcw:
AM.add(frame, "fcw", enabled)
# State specific actions
if state in [State.preEnabled, State.disabled]:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
elif state in [State.enabled, State.softDisabling]:
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
extra_text = ""
if e == "belowSteerSpeed":
if is_metric:
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(frame, e, enabled, extra_text_2=extra_text)
plan_age = DT_CTRL * (frame - rcv_frame['plan'])
dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps
a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
try:
gasinterceptor = CP.enableGasInterceptor
except AttributeError:
gasinterceptor = False
# Gas/Brake PID loop
try:
leadOne = sm['radarState'].leadOne
except KeyError:
leadOne = None
gasstatus = None
for socket, _ in poller.poll(0):
if socket is arne182Status:
gasstatus = arne182.Arne182Status.from_bytes(socket.recv())
if gasstatus is None:
gasbuttonstatus = 0
else:
gasbuttonstatus = gasstatus.gasbuttonstatus
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, gasinterceptor, gasbuttonstatus, plan.decelForTurn, plan.longitudinalPlanSource,
leadOne, CS.gasPressed)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, path_plan)
# Send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
AM.add(frame, "steerSaturated", enabled)
# Parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
extra_text_1, extra_text_2 = "", ""
if e == "calibrationIncomplete":
extra_text_1 = str(cal_perc) + "%"
if is_metric:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
AM.process_alerts(frame)
return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log
def state_control(plan, CS, CP, state, events, v_cruise_kph, AM, rk,
awareness_status, PL, LaC, LoC, VM, angle_offset,
rear_view_allowed, rear_view_toggle):
# Given the state, this function returns the actuators
# reset actuators to zero
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
for b in CS.buttonEvents:
# any button event resets awarenesss_status
awareness_status = 1.
# button presses for rear view
if b.type == "leftBlinker" or b.type == "rightBlinker":
if b.pressed and rear_view_allowed:
rear_view_toggle = True
else:
rear_view_toggle = False
if b.type == "altButton1" and b.pressed:
rear_view_toggle = not rear_view_toggle
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
# ***** state specific actions *****
# DISABLED
if state in [State.PRE_ENABLED, State.DISABLED]:
awareness_status = 1.
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
# ENABLED or SOFT_DISABLING
elif state in [State.ENABLED, State.SOFT_DISABLING]:
if CS.steeringPressed:
# reset awareness status on steering
awareness_status = 1.0
# 6 minutes driver you're on
awareness_status -= 0.01 / (AWARENESS_TIME)
if awareness_status <= 0.:
AM.add("driverDistracted", enabled)
elif awareness_status <= AWARENESS_PRE_TIME / AWARENESS_TIME and \
awareness_status >= (AWARENESS_PRE_TIME - 0.1) / AWARENESS_TIME:
AM.add("preDriverDistracted", enabled)
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
AM.add(e, enabled)
# if user is not responsive to awareness alerts, then start a smooth deceleration
if awareness_status < -0.:
plan.aTargetMax = min(plan.aTargetMax, AWARENESS_DECEL)
plan.aTargetMin = min(plan.aTargetMin, plan.aTargetMax)
# *** angle offset learning ***
if rk.frame % 5 == 2 and plan.lateralValid:
# *** run this at 20hz again ***
angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
PL.PP.c_poly, PL.PP.c_prob,
LaC.y_des, CS.steeringPressed)
# *** gas/brake PID loop ***
actuators.gas, actuators.brake = LoC.update(
active, CS.vEgo, CS.brakePressed, v_cruise_kph, plan.vTarget,
[plan.aTargetMin, plan.aTargetMax], plan.jerkFactor, CP)
# *** steering PID loop ***
actuators.steer = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, plan.dPoly, angle_offset,
VM, PL)
# send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag:
AM.add("steerSaturated", enabled)
if CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# *** process alerts ***
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, awareness_status, angle_offset, rear_view_toggle
def state_control(frame, rcv_frame, plan, path_plan, 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):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
# check if user has interacted with the car
driver_engaged = len(CS.buttonEvents) > 0 or \
v_cruise_kph != v_cruise_kph_last or \
CS.steeringPressed
if CS.leftBlinker or CS.rightBlinker:
last_blinker_frame = frame
# add eventual driver distracted events
events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill)
if plan.fcw:
# send FCW alert if triggered by planner
AM.add(frame, "fcw", enabled)
elif CS.stockFcw:
# send a silent alert when stock fcw triggers, since the car is already beeping
AM.add(frame, "fcwStock", enabled)
# State specific actions
if state in [State.preEnabled, State.disabled]:
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
elif state in [State.enabled, State.softDisabling]:
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
extra_text = ""
if e == "belowSteerSpeed":
if is_metric:
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(frame, e, enabled, extra_text_2=extra_text)
plan_age = DT_CTRL * (frame - rcv_frame['plan'])
dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps
a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
# Gas/Brake PID loop
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan)
# Send a "steering required alert" if saturation count has reached the limit
if lac_log.saturated and not CS.steeringPressed:
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
if left_deviation or right_deviation:
AM.add(frame, "steerSaturated", enabled)
# Parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
extra_text_1, extra_text_2 = "", ""
if e == "calibrationIncomplete":
extra_text_1 = str(cal_perc) + "%"
if is_metric:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame
def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk,
carstate, carcontrol, carevents, carparams, controlsstate,
sendcan, AM, driver_status, LaC, LoC, read_only, start_time,
v_acc, a_acc, lac_log, events_prev):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
try:
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)
blinker = CS.leftBlinker or CS.rightBlinker
ldw_allowed = CS.vEgo > 12.5 and not blinker
if len(list(sm['pathPlan'].rPoly)) == 4:
CC.hudControl.rightLaneDepart = bool(
ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET)
and right_lane_visible)
if len(list(sm['pathPlan'].lPoly)) == 4:
CC.hudControl.leftLaneDepart = bool(ldw_allowed
and sm['pathPlan'].lPoly[3] <
(1 - CAMERA_OFFSET)
and left_lane_visible)
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
if not read_only:
# send car controls over can
can_sends = CI.apply(CC)
sendcan.send(
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":
"", # no EON sounds yet
"awarenessStatus":
max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn":
bool(driver_status.monitor_on and 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),
"angleModelBias":
0.,
"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
else:
dat.controlsState.lateralControlState.indiState = lac_log
controlsstate.send(dat.to_bytes())
# carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
# 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
carevents.send(ce_send.to_bytes())
# 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
carparams.send(cp_send.to_bytes())
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
return CC, events_bytes
except Exception as e:
print e
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):
# ******************* 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 state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph,
AM):
"""Compute conditional state transitions and execute actions on state transitions"""
enabled = isEnabled(state)
v_cruise_kph_last = v_cruise_kph
# if stock cruise is completely disabled, then we can use our own set speed logic
if not CP.enableCruise:
v_cruise_kph = update_v_cruise(v_cruise_kph, CS.buttonEvents, enabled)
elif CP.enableCruise and CS.cruiseState.enabled:
v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
# decrease the soft disable timer at every step, as it's reset on
# entrance in SOFT_DISABLING state
soft_disable_timer = max(0, soft_disable_timer - 1)
# DISABLED
if state == State.disabled:
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add("enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents,
v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
state = State.enabled
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
elif soft_disable_timer <= 0:
state = State.disabled
# PRE ENABLING
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events,
[ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.enabled
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events,
v_cruise_kph, v_cruise_kph_last, AM, rk, LaC, LoC, read_only,
is_metric, cal_perc, last_blinker_frame, saturated_count,
dragon_lat_control, dragon_display_steering_limit_alert,
dragon_lead_car_moving_alert):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
enabled = isEnabled(state)
active = isActive(state)
if CS.leftBlinker or CS.rightBlinker:
last_blinker_frame = frame
if plan.fcw:
# send FCW alert if triggered by planner
AM.add(frame, "fcw", enabled)
elif CS.stockFcw:
# send a silent alert when stock fcw triggers, since the car is already beeping
AM.add(frame, "fcwStock", enabled)
# State specific actions
if state in [State.preEnabled, State.disabled]:
if dragon_lead_car_moving_alert:
for e in get_events(events, [ET.WARNING]):
extra_text = ""
if e in ["leadCarDetected", "leadCarMoving"]:
AM.add(frame, e, enabled, extra_text_2=extra_text)
LaC.reset()
LoC.reset(v_pid=CS.vEgo)
elif state in [State.enabled, State.softDisabling]:
# parse warnings from car specific interface
for e in get_events(events, [ET.WARNING]):
extra_text = ""
if e == "belowSteerSpeed":
if is_metric:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(
int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(frame, e, enabled, extra_text_2=extra_text)
plan_age = DT_CTRL * (frame - rcv_frame['plan'])
dt = min(
plan_age, LON_MPC_STEP + DT_CTRL
) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps
a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget -
plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
# Gas/Brake PID loop
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.gasPressed,
CS.brakePressed, CS.standstill,
CS.cruiseState.standstill,
v_cruise_kph, v_acc_sol,
plan.vTargetFuture, a_acc_sol,
CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle, lac_log = LaC.update(
active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP,
path_plan)
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD
saturated_count = saturated_count + 1 if angle_control_saturated and not CS.steeringPressed and active else 0
if dragon_display_steering_limit_alert and dragon_lat_control:
# Send a "steering required alert" if saturation count has reached the limit
if (lac_log.saturated and not CS.steeringPressed) or (
saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
if left_deviation or right_deviation:
AM.add(frame, "steerSaturated", enabled)
# Parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
extra_text_1, extra_text_2 = "", ""
if e == "calibrationIncomplete":
extra_text_1 = str(cal_perc) + "%"
if is_metric:
extra_text_2 = str(int(round(
Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(
Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(frame,
str(e) + "Permanent",
enabled,
extra_text_1=extra_text_1,
extra_text_2=extra_text_2)
return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame, saturated_count
def 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_model_bias, passive, start_time,
params, v_acc, a_acc):
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
CC = car.CarControl.new_message()
if not passive:
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, plan.aTarget, CS.vEgo, 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 = plan.hasLead
CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5)
CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5)
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
CI.apply(CC)
force_decel = driver_status.awareness < 0.
# live100
dat = messaging.new_message()
dat.init('live100')
dat.live100 = {
"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":
"", # no EON sounds yet
"awarenessStatus":
max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn":
bool(driver_status.monitor_on),
"canMonoTimes":
list(CS.canMonoTimes),
"planMonoTime":
plan_ts,
"pathPlanMonoTime":
path_plan.logMonoTime,
"enabled":
isEnabled(state),
"active":
isActive(state),
"vEgo":
CS.vEgo,
"vEgoRaw":
CS.vEgoRaw,
"angleSteers":
CS.steeringAngle,
"curvature":
VM.calc_curvature(CS.steeringAngle * 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),
"upSteer":
float(LaC.pid.p),
"uiSteer":
float(LaC.pid.i),
"ufSteer":
float(LaC.pid.f),
"vTargetLead":
float(v_acc),
"aTarget":
float(a_acc),
"jerkFactor":
float(plan.jerkFactor),
"angleModelBias":
float(angle_model_bias),
"gpsPlannerActive":
plan.gpsPlannerActive,
"vCurvature":
plan.vCurvature,
"decelForTurn":
plan.decelForTurn,
"cumLagMs":
-rk.remaining * 1000.,
"startMonoTime":
start_time,
"mapValid":
plan.mapValid,
"forceDecel":
bool(force_decel),
}
live100.send(dat.to_bytes())
# carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
if (rk.frame % 36000) == 0: # update angle offset every 6 minutes
params.put("ControlsParams",
json.dumps({'angle_model_bias': angle_model_bias}))
return CC
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
# Bosch Lane Polynomials
ret.lPoly.c3 = self.CS.l_poly[3]
ret.lPoly.c2 = self.CS.l_poly[2]
ret.lPoly.c1 = self.CS.l_poly[1]
ret.lPoly.c0 = self.CS.l_poly[0]
ret.lPoly.prob = self.CS.l_prob
ret.lPoly.distVis = self.CS.l_distVis
ret.lPoly.isSolid = bool(self.CS.l_isSolid)
ret.lPoly.isDashed = bool(self.CS.l_isDashed)
ret.rPoly.c3 = self.CS.r_poly[3]
ret.rPoly.c2 = self.CS.r_poly[2]
ret.rPoly.c1 = self.CS.r_poly[1]
ret.rPoly.c0 = self.CS.r_poly[0]
ret.rPoly.prob = self.CS.r_prob
ret.rPoly.distVis = self.CS.r_distVis
ret.rPoly.isSolid = bool(self.CS.r_isSolid)
ret.rPoly.isDashed = bool(self.CS.r_isDashed)
ret.lAdjPoly.c3 = self.CS.lAdj_poly[3]
ret.lAdjPoly.c2 = self.CS.lAdj_poly[2]
ret.lAdjPoly.c1 = self.CS.lAdj_poly[1]
ret.lAdjPoly.c0 = self.CS.lAdj_poly[0]
ret.lAdjPoly.prob = self.CS.lAdj_prob
ret.lAdjPoly.distVis = self.CS.lAdj_distVis
ret.lAdjPoly.isSolid = bool(self.CS.lAdj_isSolid)
ret.lAdjPoly.isDashed = bool(self.CS.lAdj_isDashed)
ret.rAdjPoly.c3 = self.CS.rAdj_poly[3]
ret.rAdjPoly.c2 = self.CS.rAdj_poly[2]
ret.rAdjPoly.c1 = self.CS.rAdj_poly[1]
ret.rAdjPoly.c0 = self.CS.rAdj_poly[0]
ret.rAdjPoly.prob = self.CS.rAdj_prob
ret.rAdjPoly.distVis = self.CS.rAdj_distVis
ret.rAdjPoly.isSolid = bool(self.CS.rAdj_isSolid)
ret.rAdjPoly.isDashed = bool(self.CS.rAdj_isDashed)
# 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()
