You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
358 lines
12 KiB
358 lines
12 KiB
#!/usr/bin/env python
|
|
import zmq
|
|
import numpy as np
|
|
|
|
from common.services import service_list
|
|
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
|
|
|
|
from selfdrive.config import CruiseButtons
|
|
from selfdrive.config import Conversions as CV
|
|
|
|
from selfdrive.controls.lib.drive_helpers import learn_angle_offset
|
|
from selfdrive.controls.lib.alert_database import process_alert, AI
|
|
|
|
import selfdrive.messaging as messaging
|
|
|
|
from selfdrive.controls.lib.carstate import CarState
|
|
from selfdrive.controls.lib.carcontroller import CarController
|
|
from selfdrive.controls.lib.longcontrol import LongControl
|
|
from selfdrive.controls.lib.latcontrol import LatControl
|
|
|
|
from selfdrive.controls.lib.pathplanner import PathPlanner
|
|
from selfdrive.controls.lib.adaptivecruise import AdaptiveCruise
|
|
|
|
def controlsd_thread(gctx, rate=100): #rate in Hz
|
|
# *** log ***
|
|
context = zmq.Context()
|
|
live100 = messaging.pub_sock(context, service_list['live100'].port)
|
|
thermal = messaging.sub_sock(context, service_list['thermal'].port)
|
|
live20 = messaging.sub_sock(context, service_list['live20'].port)
|
|
model = messaging.sub_sock(context, service_list['model'].port)
|
|
|
|
logcan = messaging.sub_sock(context, service_list['can'].port)
|
|
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
|
|
|
|
# *** init the major players ***
|
|
CS = CarState(logcan)
|
|
CC = CarController()
|
|
|
|
PP = PathPlanner(model)
|
|
AC = AdaptiveCruise(live20)
|
|
|
|
LoC = LongControl()
|
|
LaC = LatControl()
|
|
|
|
# *** control initial values ***
|
|
apply_brake = 0
|
|
enabled = False
|
|
|
|
# *** time values ***
|
|
last_enable_pressed = 0
|
|
|
|
# *** controls initial values ***
|
|
# *** display stuff
|
|
soft_disable_start = 0
|
|
sounding = False
|
|
no_mismatch_pcm_last, no_mismatch_ctrl_last = 0, 0
|
|
|
|
# car state
|
|
alert, sound_exp, hud_exp, text_exp, alert_p = None, 0, 0, 0, 0
|
|
rear_view_cam, rear_view_toggle = False, False
|
|
|
|
v_cruise = 255 # this means no display
|
|
v_cruise_max = 144
|
|
v_cruise_min = 8
|
|
v_cruise_delta = 8
|
|
|
|
# on activation target at least 25mph. With 5mph you need too much tapping
|
|
v_cruise_enable_min = 40
|
|
|
|
hud_v_cruise = 255
|
|
|
|
angle_offset = 0
|
|
|
|
max_enable_speed = 57. # ~91 mph
|
|
|
|
pcm_threshold = 25.*CV.MPH_TO_MS # below this speed pcm cancels
|
|
|
|
overtemp = True
|
|
|
|
# 0.0 - 1.0
|
|
awareness_status = 0.0
|
|
|
|
# start the loop
|
|
set_realtime_priority(2)
|
|
|
|
rk = Ratekeeper(rate)
|
|
while 1:
|
|
cur_time = sec_since_boot()
|
|
|
|
# read CAN
|
|
canMonoTimes = CS.update(logcan)
|
|
|
|
# **** rearview mirror management ***
|
|
if CS.cruise_setting == 1 and CS.prev_cruise_setting == 0:
|
|
rear_view_toggle = not rear_view_toggle
|
|
|
|
# show rear view camera on phone if in reverse gear or when lkas button is pressed
|
|
rear_view_cam = (CS.gear_shifter == 2) or rear_view_toggle or CS.blinker_on
|
|
|
|
# *** thermal checking logic ***
|
|
|
|
# thermal data, checked every second
|
|
td = messaging.recv_sock(thermal)
|
|
if td is not None:
|
|
cpu_temps = [td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2,
|
|
td.thermal.cpu3, td.thermal.mem, td.thermal.gpu]
|
|
# check overtemp
|
|
overtemp = any(t > 950 for t in cpu_temps)
|
|
|
|
# *** getting model logic ***
|
|
PP.update(cur_time, CS.v_ego)
|
|
|
|
if rk.frame % 5 == 2:
|
|
# *** run this at 20hz again ***
|
|
angle_offset = learn_angle_offset(enabled, CS.v_ego, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steer_override)
|
|
|
|
# to avoid race conditions, check if control has been disabled for at least 0.2s
|
|
mismatch_ctrl = not CC.controls_allowed and enabled
|
|
mismatch_pcm = (not CS.pcm_acc_status and (not apply_brake or CS.v_ego < 0.1)) and enabled
|
|
|
|
# keep resetting start timer if mismatch isn't true
|
|
if not mismatch_ctrl:
|
|
no_mismatch_ctrl_last = cur_time
|
|
if not mismatch_pcm or not CS.brake_only:
|
|
no_mismatch_pcm_last = cur_time
|
|
|
|
#*** v_cruise logic ***
|
|
if CS.brake_only:
|
|
v_cruise = int(CS.v_cruise_pcm) # TODO: why sometimes v_cruise_pcm is long type?
|
|
else:
|
|
if CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.RES_ACCEL and enabled:
|
|
v_cruise = v_cruise - (v_cruise % v_cruise_delta) + v_cruise_delta
|
|
elif CS.prev_cruise_buttons == 0 and CS.cruise_buttons == CruiseButtons.DECEL_SET and enabled:
|
|
v_cruise = v_cruise + (v_cruise % v_cruise_delta) - v_cruise_delta
|
|
|
|
# *** enabling/disabling logic ***
|
|
enable_pressed = (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) \
|
|
and CS.cruise_buttons == 0
|
|
|
|
if enable_pressed:
|
|
print "enabled pressed at", cur_time
|
|
last_enable_pressed = cur_time
|
|
|
|
# if pcm does speed control than we need to wait on pcm to enable
|
|
if CS.brake_only:
|
|
enable_condition = (cur_time - last_enable_pressed) < 0.2 and CS.pcm_acc_status
|
|
else:
|
|
enable_condition = enable_pressed
|
|
|
|
# always clear the alert at every cycle
|
|
alert_id = []
|
|
|
|
# check for PCM not enabling
|
|
if CS.brake_only and (cur_time - last_enable_pressed) < 0.2 and not CS.pcm_acc_status:
|
|
print "waiting for PCM to enable"
|
|
|
|
# check for denied enabling
|
|
if enable_pressed and not enabled:
|
|
deny_enable = \
|
|
[(AI.SEATBELT, not CS.seatbelt),
|
|
(AI.DOOR_OPEN, not CS.door_all_closed),
|
|
(AI.ESP_OFF, CS.esp_disabled),
|
|
(AI.STEER_ERROR, CS.steer_error),
|
|
(AI.BRAKE_ERROR, CS.brake_error),
|
|
(AI.GEAR_NOT_D, not CS.gear_shifter_valid),
|
|
(AI.MAIN_OFF, not CS.main_on),
|
|
(AI.PEDAL_PRESSED, CS.user_gas_pressed or CS.brake_pressed or (CS.pedal_gas > 0 and CS.brake_only)),
|
|
(AI.HIGH_SPEED, CS.v_ego > max_enable_speed),
|
|
(AI.OVERHEAT, overtemp),
|
|
(AI.COMM_ISSUE, PP.dead or AC.dead),
|
|
(AI.CONTROLSD_LAG, rk.remaining < -0.2)]
|
|
for alertn, cond in deny_enable:
|
|
if cond:
|
|
alert_id += [alertn]
|
|
|
|
# check for soft disables
|
|
if enabled:
|
|
soft_disable = \
|
|
[(AI.SEATBELT_SD, not CS.seatbelt),
|
|
(AI.DOOR_OPEN_SD, not CS.door_all_closed),
|
|
(AI.ESP_OFF_SD, CS.esp_disabled),
|
|
(AI.OVERHEAT_SD, overtemp),
|
|
(AI.COMM_ISSUE_SD, PP.dead or AC.dead),
|
|
(AI.CONTROLSD_LAG_SD, rk.remaining < -0.2)]
|
|
sounding = False
|
|
for alertn, cond in soft_disable:
|
|
if cond:
|
|
alert_id += [alertn]
|
|
sounding = True
|
|
# soft disengagement expired, user need to take control
|
|
if (cur_time - soft_disable_start) > 3.:
|
|
enabled = False
|
|
v_cruise = 255
|
|
if not sounding:
|
|
soft_disable_start = cur_time
|
|
|
|
# check for immediate disables
|
|
if enabled:
|
|
immediate_disable = \
|
|
[(AI.PCM_LOW_SPEED, (cur_time > no_mismatch_pcm_last > 0.2) and CS.v_ego < pcm_threshold),
|
|
(AI.STEER_ERROR_ID, CS.steer_error),
|
|
(AI.BRAKE_ERROR_ID, CS.brake_error),
|
|
(AI.CTRL_MISMATCH_ID, (cur_time - no_mismatch_ctrl_last) > 0.2),
|
|
(AI.PCM_MISMATCH_ID, (cur_time - no_mismatch_pcm_last) > 0.2)]
|
|
for alertn, cond in immediate_disable:
|
|
if cond:
|
|
alert_id += [alertn]
|
|
# immediate turn off control
|
|
enabled = False
|
|
v_cruise = 255
|
|
|
|
# user disabling
|
|
if enabled and (CS.user_gas_pressed or CS.brake_pressed or not CS.gear_shifter_valid or \
|
|
(CS.cruise_buttons == CruiseButtons.CANCEL and CS.prev_cruise_buttons == 0) or \
|
|
not CS.main_on or (CS.pedal_gas > 0 and CS.brake_only)):
|
|
enabled = False
|
|
v_cruise = 255
|
|
alert_id += [AI.DISABLE]
|
|
|
|
# enabling
|
|
if enable_condition and not enabled and len(alert_id) == 0:
|
|
print "*** enabling controls"
|
|
|
|
#enable both lateral and longitudinal controls
|
|
enabled = True
|
|
counter_pcm_enabled = CS.counter_pcm
|
|
# on activation, let's always set v_cruise from where we are, even if PCM ACC is active
|
|
# what we want to be displayed in mph
|
|
v_cruise_mph = round(CS.v_ego * CV.MS_TO_MPH * CS.ui_speed_fudge)
|
|
# what we need to send to have that displayed
|
|
v_cruise = int(round(np.maximum(v_cruise_mph * CV.MPH_TO_KPH, v_cruise_enable_min)))
|
|
|
|
# 6 minutes driver you're on
|
|
awareness_status = 1.0
|
|
|
|
# reset the PID loops
|
|
LaC.reset()
|
|
# start long control at actual speed
|
|
LoC.reset(v_pid = CS.v_ego)
|
|
|
|
alert_id += [AI.ENABLE]
|
|
|
|
if v_cruise != 255 and not CS.brake_only:
|
|
v_cruise = np.clip(v_cruise, v_cruise_min, v_cruise_max)
|
|
|
|
# **** awareness status manager ****
|
|
if enabled:
|
|
# gives the user 6 minutes
|
|
awareness_status -= 1.0/(100*60*6)
|
|
# reset on steering, blinker, or cruise buttons
|
|
if CS.steer_override or CS.blinker_on or CS.cruise_buttons or CS.cruise_setting:
|
|
awareness_status = 1.0
|
|
if awareness_status <= 0.:
|
|
alert_id += [AI.DRIVER_DISTRACTED]
|
|
|
|
# ****** initial actuators commands ***
|
|
# *** gas/brake PID loop ***
|
|
AC.update(cur_time, CS.v_ego, CS.angle_steers, LoC.v_pid, awareness_status, CS.VP)
|
|
final_gas, final_brake = LoC.update(enabled, CS, v_cruise, AC.v_target_lead, AC.a_target, AC.jerk_factor)
|
|
pcm_accel = int(np.clip(AC.a_pcm/1.4,0,1)*0xc6) # TODO: perc of max accel in ACC?
|
|
|
|
# *** steering PID loop ***
|
|
final_steer, sat_flag = LaC.update(enabled, CS, PP.d_poly, angle_offset)
|
|
|
|
# this needs to stay before hysteresis logic to avoid pcm staying on control during brake hysteresis
|
|
pcm_override = True # this is always True
|
|
pcm_cancel_cmd = False
|
|
if CS.brake_only and final_brake == 0.:
|
|
pcm_speed = LoC.v_pid - .3 # FIXME: just for exp
|
|
else:
|
|
pcm_speed = 0
|
|
|
|
# ***** handle alerts ****
|
|
# send a "steering required alert" if saturation count has reached the limit
|
|
if sat_flag:
|
|
alert_id += [AI.STEER_SATURATED]
|
|
|
|
# process the alert, based on id
|
|
alert, chime, beep, hud_alert, alert_text, sound_exp, hud_exp, text_exp, alert_p = \
|
|
process_alert(alert_id, alert, cur_time, sound_exp, hud_exp, text_exp, alert_p)
|
|
|
|
# alerts pub
|
|
if len(alert_id) != 0:
|
|
print alert_id, alert_text
|
|
|
|
# *** process for hud display ***
|
|
if not enabled or (hud_v_cruise == 255 and CS.counter_pcm == counter_pcm_enabled):
|
|
hud_v_cruise = 255
|
|
else:
|
|
hud_v_cruise = v_cruise
|
|
|
|
# *** actually do can sends ***
|
|
CC.update(sendcan, enabled, CS, rk.frame, \
|
|
final_gas, final_brake, final_steer, \
|
|
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
|
hud_v_cruise, hud_show_lanes = enabled, \
|
|
hud_show_car = AC.has_lead, \
|
|
hud_alert = hud_alert, \
|
|
snd_beep = beep, snd_chime = chime)
|
|
|
|
# ***** publish state to logger *****
|
|
|
|
# publish controls state at 100Hz
|
|
dat = messaging.new_message()
|
|
dat.init('live100')
|
|
|
|
# move liveUI into live100
|
|
dat.live100.rearViewCam = bool(rear_view_cam)
|
|
dat.live100.alertText1 = alert_text[0]
|
|
dat.live100.alertText2 = alert_text[1]
|
|
dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0
|
|
|
|
# what packets were used to process
|
|
dat.live100.canMonoTimes = canMonoTimes
|
|
dat.live100.mdMonoTime = PP.logMonoTime
|
|
dat.live100.l20MonoTime = AC.logMonoTime
|
|
|
|
# if controls is enabled
|
|
dat.live100.enabled = enabled
|
|
|
|
# car state
|
|
dat.live100.vEgo = float(CS.v_ego)
|
|
dat.live100.aEgo = float(CS.a_ego)
|
|
dat.live100.angleSteers = float(CS.angle_steers)
|
|
dat.live100.hudLead = CS.hud_lead
|
|
dat.live100.steerOverride = CS.steer_override
|
|
|
|
# longitudinal control state
|
|
dat.live100.vPid = float(LoC.v_pid)
|
|
dat.live100.vCruise = float(v_cruise)
|
|
dat.live100.upAccelCmd = float(LoC.Up_accel_cmd)
|
|
dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd)
|
|
|
|
# lateral control state
|
|
dat.live100.yActual = float(LaC.y_actual)
|
|
dat.live100.yDes = float(LaC.y_des)
|
|
dat.live100.upSteer = float(LaC.Up_steer)
|
|
dat.live100.uiSteer = float(LaC.Ui_steer)
|
|
|
|
# processed radar state, should add a_pcm?
|
|
dat.live100.vTargetLead = float(AC.v_target_lead)
|
|
dat.live100.aTargetMin = float(AC.a_target[0])
|
|
dat.live100.aTargetMax = float(AC.a_target[1])
|
|
dat.live100.jerkFactor = float(AC.jerk_factor)
|
|
|
|
# lag
|
|
dat.live100.cumLagMs = -rk.remaining*1000.
|
|
|
|
live100.send(dat.to_bytes())
|
|
|
|
# *** run loop at fixed rate ***
|
|
rk.keep_time()
|
|
|
|
def main(gctx=None):
|
|
controlsd_thread(gctx, 100)
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|