mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-09-30 19:33:49 +08:00
openpilot release
This commit is contained in:
Vehicle Researcher
Executable
+358
@@ -0,0 +1,358 @@
|
||||
#!/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()
|
||||
@@ -0,0 +1,332 @@
|
||||
import selfdrive.messaging as messaging
|
||||
import numpy as np
|
||||
|
||||
# lookup tables VS speed to determine min and max accels in cruise
|
||||
_A_CRUISE_MIN_V = np.asarray([-1.0, -.8, -.67, -.5, -.30])
|
||||
_A_CRUISE_MIN_BP = np.asarray([ 0., 5., 10., 20., 40.])
|
||||
|
||||
# need fast accel at very low speed for stop and go
|
||||
_A_CRUISE_MAX_V = np.asarray([1., 1., .8, .5, .30])
|
||||
_A_CRUISE_MAX_BP = np.asarray([0., 5., 10., 20., 40.])
|
||||
|
||||
def calc_cruise_accel_limits(v_ego):
|
||||
a_cruise_min = np.interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||
a_cruise_max = np.interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
|
||||
|
||||
a_pcm = 1. # always 1 for now
|
||||
return np.vstack([a_cruise_min, a_cruise_max]), a_pcm
|
||||
|
||||
_A_TOTAL_MAX_V = np.asarray([1.5, 1.9, 3.2])
|
||||
_A_TOTAL_MAX_BP = np.asarray([0., 20., 40.])
|
||||
|
||||
def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP):
|
||||
#*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration
|
||||
# this should avoid accelerating when losing the target in turns
|
||||
deg_to_rad = np.pi / 180. # from can reading to rad
|
||||
|
||||
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_y = v_ego**2 * angle_steers * deg_to_rad / (VP.steer_ratio * VP.wheelbase)
|
||||
a_x_allowed = np.sqrt(np.maximum(a_total_max**2 - a_y**2, 0.))
|
||||
|
||||
a_target[1] = np.minimum(a_target[1], a_x_allowed)
|
||||
a_pcm = np.minimum(a_pcm, a_x_allowed)
|
||||
return a_target, a_pcm
|
||||
|
||||
def process_a_lead(a_lead):
|
||||
# soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead
|
||||
a_lead_threshold = 0.5
|
||||
a_lead = np.minimum(a_lead + a_lead_threshold, 0)
|
||||
return a_lead
|
||||
|
||||
def calc_desired_distance(v_lead):
|
||||
#*** compute desired distance ***
|
||||
t_gap = 1.7 # good to be far away
|
||||
d_offset = 4 # distance when at zero speed
|
||||
return d_offset + v_lead * t_gap
|
||||
|
||||
|
||||
#linear slope
|
||||
_L_SLOPE_V = np.asarray([0.40, 0.10])
|
||||
_L_SLOPE_BP = np.asarray([0., 40])
|
||||
|
||||
# parabola slope
|
||||
_P_SLOPE_V = np.asarray([1.0, 0.25])
|
||||
_P_SLOPE_BP = np.asarray([0., 40])
|
||||
|
||||
def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
|
||||
#*** compute desired speed ***
|
||||
# the desired speed curve is divided in 4 portions:
|
||||
# 1-constant
|
||||
# 2-linear to regain distance
|
||||
# 3-linear to shorten distance
|
||||
# 4-parabolic (constant decel)
|
||||
|
||||
max_runaway_speed = -2. # no slower than 2m/s over the lead
|
||||
|
||||
# interpolate the lookups to find the slopes for a give lead speed
|
||||
l_slope = np.interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
|
||||
p_slope = np.interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
|
||||
|
||||
# this is where parabola and linear curves are tangents
|
||||
x_linear_to_parabola = p_slope / l_slope**2
|
||||
|
||||
# parabola offset to have the parabola being tangent to the linear curve
|
||||
x_parabola_offset = p_slope / (2 * l_slope**2)
|
||||
|
||||
if d_lead < d_des:
|
||||
# calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des
|
||||
v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des)
|
||||
# calculate v_rel_des on one third of the linear slope
|
||||
v_rel_des_2 = (d_lead - d_des) * l_slope / 3.
|
||||
# take the min of the 2 above
|
||||
v_rel_des = np.minimum(v_rel_des_1, v_rel_des_2)
|
||||
v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
|
||||
elif d_lead < d_des + x_linear_to_parabola:
|
||||
v_rel_des = (d_lead - d_des) * l_slope
|
||||
v_rel_des = np.maximum(v_rel_des, max_runaway_speed)
|
||||
else:
|
||||
v_rel_des = np.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
|
||||
|
||||
# compute desired speed
|
||||
v_target = v_rel_des + v_lead
|
||||
|
||||
# compute v_coast: above this speed we want to coast
|
||||
t_lookahead = 1. # how far in time we consider a_lead to anticipate the coast region
|
||||
v_coast_shift = np.maximum(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0
|
||||
v_coast = (v_lead + v_target)/2 + v_coast_shift # no accel allowed above this line
|
||||
v_coast = np.minimum(v_coast, v_target)
|
||||
|
||||
return v_target, v_coast
|
||||
|
||||
def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
|
||||
# this function computes the required decel to avoid crashing, given safety offsets
|
||||
a_critical = - np.maximum(0., v_rel + v_offset)**2/np.maximum(2*(d_lead - d_offset), 0.5)
|
||||
return a_critical
|
||||
|
||||
|
||||
# maximum acceleration adjustment
|
||||
_A_CORR_BY_SPEED_V = np.asarray([0.4, 0.4, 0])
|
||||
# speeds
|
||||
_A_CORR_BY_SPEED_BP = np.asarray([0., 5., 20.])
|
||||
|
||||
def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
|
||||
a_coast_min = -1.0 # never coast faster then -1m/s^2
|
||||
# coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
|
||||
# regardless v_target
|
||||
if v_ref > np.minimum(v_coast, v_target):
|
||||
# for smooth coast we can be agrressive and target a point where car would actually crash
|
||||
v_offset_coast = 0.
|
||||
d_offset_coast = d_des/2. - 4.
|
||||
|
||||
# acceleration value to smoothly coast until we hit v_target
|
||||
if d_lead > d_offset_coast + 0.1:
|
||||
a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast)
|
||||
# if lead is decelerating, then offset the coast decel
|
||||
a_coast += a_lead_contr
|
||||
a_max = np.maximum(a_coast, a_coast_min)
|
||||
else:
|
||||
a_max = a_coast_min
|
||||
else:
|
||||
# same as cruise accel, but add a small correction based on lead acceleration at low speeds
|
||||
# when lead car accelerates faster, we can do the same, and vice versa
|
||||
|
||||
a_max = a_max + np.interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
|
||||
* np.clip(-v_rel / 4., -.5, 1)
|
||||
return a_max
|
||||
|
||||
# arbitrary limits to avoid too high accel being computed
|
||||
_A_SAT = np.asarray([-10., 5.])
|
||||
|
||||
# do not consider a_lead at 0m/s, fully consider it at 10m/s
|
||||
_A_LEAD_LOW_SPEED_V = np.asarray([0., 1.])
|
||||
|
||||
# speed break points
|
||||
_A_LEAD_LOW_SPEED_BP = np.asarray([0., 10.])
|
||||
|
||||
# add a small offset to the desired decel, just for safety margin
|
||||
_DECEL_OFFSET_V = np.asarray([-0.3, -0.5, -0.5, -0.4, -0.3])
|
||||
|
||||
# speed bp: different offset based on the likelyhood that lead decels abruptly
|
||||
_DECEL_OFFSET_BP = np.asarray([0., 4., 15., 30, 40.])
|
||||
|
||||
|
||||
def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
|
||||
v_target, v_coast, a_target, a_pcm):
|
||||
#*** compute max accel ***
|
||||
# v_rel is now your velocity in lead car frame
|
||||
v_rel = -v_rel # this simplifiess things when thinking in d_rel-v_rel diagram
|
||||
|
||||
v_rel_pid = v_pid - v_lead
|
||||
|
||||
# this is how much lead accel we consider in assigning the desired decel
|
||||
a_lead_contr = a_lead * np.interp(v_lead, _A_LEAD_LOW_SPEED_BP,
|
||||
_A_LEAD_LOW_SPEED_V) * 0.8
|
||||
|
||||
# first call of calc_positive_accel_limit is used to shape v_pid
|
||||
a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid,
|
||||
v_rel_pid, v_coast, v_target,
|
||||
a_lead_contr, a_target[1])
|
||||
# second call of calc_positive_accel_limit is used to limit the pcm throttle
|
||||
# control (only useful when we don't control throttle directly)
|
||||
a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego, v_rel,
|
||||
v_coast, v_target, a_lead_contr, a_pcm)
|
||||
|
||||
#*** compute max decel ***
|
||||
v_offset = 1. # assume the car is 1m/s slower
|
||||
d_offset = 1. # assume the distance is 1m lower
|
||||
if v_target - v_ego > 0.5:
|
||||
pass # acc target speed is above vehicle speed, so we can use the cruise limits
|
||||
elif d_lead > d_offset + 0.01: # add small value to avoid by zero divisions
|
||||
# compute needed accel to get to 1m distance with -1m/s rel speed
|
||||
decel_offset = np.interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
|
||||
|
||||
critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset)
|
||||
a_target[0] = np.minimum(decel_offset + critical_decel + a_lead_contr,
|
||||
a_target[0])
|
||||
else:
|
||||
a_target[0] = _A_SAT[0]
|
||||
# a_min can't be higher than a_max
|
||||
a_target[0] = np.minimum(a_target[0], a_target[1])
|
||||
# final check on limits
|
||||
a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1])
|
||||
a_target = a_target.tolist()
|
||||
return a_target, a_pcm
|
||||
|
||||
def calc_jerk_factor(d_lead, v_rel):
|
||||
# we don't have an explicit jerk limit, so this function calculates a factor
|
||||
# that is used by the PID controller to scale the gains. Not the cleanest solution
|
||||
# but we need this for the demo.
|
||||
# TODO: Calculate Kp and Ki directly in this function.
|
||||
|
||||
# the higher is the decel required to avoid a crash, the higher is the PI factor scaling
|
||||
d_offset = 0.5
|
||||
v_offset = 2.
|
||||
a_offset = 1.
|
||||
jerk_factor_max = 1.0 # can't increase Kp and Ki more than double.
|
||||
if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions
|
||||
jerk_factor = jerk_factor_max
|
||||
else:
|
||||
a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset)
|
||||
# increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2
|
||||
jerk_factor = np.maximum(a_critical - a_offset, 0.)/5.
|
||||
jerk_factor = np.minimum(jerk_factor, jerk_factor_max)
|
||||
return jerk_factor
|
||||
|
||||
|
||||
def calc_ttc(d_rel, v_rel, a_rel, v_lead):
|
||||
# this function returns the time to collision (ttc), assuming that a_rel will stay constant
|
||||
# TODO: Review these assumptions.
|
||||
# change sign to rel quantities as it's going to be easier for calculations
|
||||
v_rel = -v_rel
|
||||
a_rel = -a_rel
|
||||
|
||||
# assuming that closing gap a_rel comes from lead vehicle decel, then limit a_rel so that v_lead will get to zero in no sooner than t_decel
|
||||
# this helps overweighting a_rel when v_lead is close to zero.
|
||||
t_decel = 2.
|
||||
a_rel = np.minimum(a_rel, v_lead/t_decel)
|
||||
|
||||
delta = v_rel**2 + 2 * d_rel * a_rel
|
||||
# assign an arbitrary high ttc value if there is no solution to ttc
|
||||
if delta < 0.1:
|
||||
ttc = 5.
|
||||
elif np.sqrt(delta) + v_rel < 0.1:
|
||||
ttc = 5.
|
||||
else:
|
||||
ttc = 2 * d_rel / (np.sqrt(delta) + v_rel)
|
||||
return ttc
|
||||
|
||||
|
||||
def limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status):
|
||||
decel_bp = [0. , 40.]
|
||||
decel_v = [-0.3, -0.2]
|
||||
decel = np.interp(v_ego, decel_bp, decel_v)
|
||||
# gives 18 seconds before decel begins (w 6 minute timeout)
|
||||
if awareness_status < -0.05:
|
||||
a_target[1] = np.minimum(a_target[1], decel)
|
||||
a_target[0] = np.minimum(a_target[1], a_target[0])
|
||||
a_pcm = 0.
|
||||
return a_target, a_pcm
|
||||
|
||||
MAX_SPEED_POSSIBLE = 55.
|
||||
|
||||
def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, awareness_status, VP):
|
||||
# drive limits
|
||||
# TODO: Make lims function of speed (more aggressive at low speed).
|
||||
a_lim = [-3., 1.5]
|
||||
|
||||
#*** set target speed pretty high, as lead hasn't been considered yet
|
||||
v_target_lead = MAX_SPEED_POSSIBLE
|
||||
|
||||
#*** set accel limits as cruise accel/decel limits ***
|
||||
a_target, a_pcm = calc_cruise_accel_limits(v_ego)
|
||||
#*** limit max accel in sharp turns
|
||||
a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP)
|
||||
jerk_factor = 0.
|
||||
|
||||
if l1 is not None and l1.status:
|
||||
#*** process noisy a_lead signal from radar processing ***
|
||||
a_lead_p = process_a_lead(l1.aLeadK)
|
||||
|
||||
#*** compute desired distance ***
|
||||
d_des = calc_desired_distance(l1.vLead)
|
||||
|
||||
#*** compute desired speed ***
|
||||
v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p)
|
||||
|
||||
if l2 is not None and l2.status:
|
||||
#*** process noisy a_lead signal from radar processing ***
|
||||
a_lead_p2 = process_a_lead(l2.aLeadK)
|
||||
|
||||
#*** compute desired distance ***
|
||||
d_des2 = calc_desired_distance(l2.vLead)
|
||||
|
||||
#*** compute desired speed ***
|
||||
v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2)
|
||||
|
||||
# listen to lead that makes you go slower
|
||||
if v_target_lead2 < v_target_lead:
|
||||
l1 = l2
|
||||
d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2
|
||||
|
||||
# l1 is the main lead now
|
||||
|
||||
#*** compute accel limits ***
|
||||
a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead,
|
||||
l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm)
|
||||
|
||||
# we can now limit a_target to a_lim
|
||||
a_target = np.clip(a_target1, a_lim[0], a_lim[1])
|
||||
a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist()
|
||||
|
||||
#*** compute max factor ***
|
||||
jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel)
|
||||
|
||||
# force coasting decel if driver hasn't been controlling car in a while
|
||||
a_target, a_pcm = limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status)
|
||||
|
||||
return v_target_lead, a_target, a_pcm, jerk_factor
|
||||
|
||||
|
||||
class AdaptiveCruise(object):
|
||||
def __init__(self, live20):
|
||||
self.live20 = live20
|
||||
self.last_cal = 0.
|
||||
self.l1, self.l2 = None, None
|
||||
self.logMonoTime = 0
|
||||
self.dead = True
|
||||
def update(self, cur_time, v_ego, angle_steers, v_pid, awareness_status, VP):
|
||||
l20 = messaging.recv_sock(self.live20)
|
||||
if l20 is not None:
|
||||
self.l1 = l20.live20.leadOne
|
||||
self.l2 = l20.live20.leadTwo
|
||||
self.logMonoTime = l20.logMonoTime
|
||||
|
||||
# TODO: no longer has anything to do with calibration
|
||||
self.last_cal = cur_time
|
||||
self.dead = False
|
||||
elif cur_time - self.last_cal > 0.5:
|
||||
self.dead = True
|
||||
|
||||
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
|
||||
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, awareness_status, VP)
|
||||
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE
|
||||
@@ -0,0 +1,178 @@
|
||||
alerts = []
|
||||
keys = ["id",
|
||||
"chime",
|
||||
"beep",
|
||||
"hud_alert",
|
||||
"screen_chime",
|
||||
"priority",
|
||||
"text_line_1",
|
||||
"text_line_2",
|
||||
"duration_sound",
|
||||
"duration_hud_alert",
|
||||
"duration_text"]
|
||||
|
||||
|
||||
#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
|
||||
class CM:
|
||||
MUTE = 0
|
||||
SINGLE = 3
|
||||
DOUBLE = 4
|
||||
REPEATED = 1
|
||||
CONTINUOUS = 2
|
||||
|
||||
|
||||
#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
|
||||
class BP:
|
||||
MUTE = 0
|
||||
SINGLE = 3
|
||||
TRIPLE = 2
|
||||
REPEATED = 1
|
||||
|
||||
|
||||
# lert ids
|
||||
class AI:
|
||||
ENABLE = 0
|
||||
DISABLE = 1
|
||||
SEATBELT = 2
|
||||
DOOR_OPEN = 3
|
||||
PEDAL_PRESSED = 4
|
||||
COMM_ISSUE = 5
|
||||
ESP_OFF = 6
|
||||
FCW = 7
|
||||
STEER_ERROR = 8
|
||||
BRAKE_ERROR = 9
|
||||
CALIB_INCOMPLETE = 10
|
||||
CALIB_INVALID = 11
|
||||
GEAR_NOT_D = 12
|
||||
MAIN_OFF = 13
|
||||
STEER_SATURATED = 14
|
||||
PCM_LOW_SPEED = 15
|
||||
THERMAL_DEAD = 16
|
||||
OVERHEAT = 17
|
||||
HIGH_SPEED = 18
|
||||
CONTROLSD_LAG = 19
|
||||
STEER_ERROR_ID = 100
|
||||
BRAKE_ERROR_ID = 101
|
||||
PCM_MISMATCH_ID = 102
|
||||
CTRL_MISMATCH_ID = 103
|
||||
SEATBELT_SD = 200
|
||||
DOOR_OPEN_SD = 201
|
||||
COMM_ISSUE_SD = 202
|
||||
ESP_OFF_SD = 203
|
||||
THERMAL_DEAD_SD = 204
|
||||
OVERHEAT_SD = 205
|
||||
CONTROLSD_LAG_SD = 206
|
||||
CALIB_INCOMPLETE_SD = 207
|
||||
CALIB_INVALID_SD = 208
|
||||
DRIVER_DISTRACTED = 300
|
||||
|
||||
class AH:
|
||||
#[alert_idx, value]
|
||||
# See dbc files for info on values"
|
||||
NONE = [0, 0]
|
||||
FCW = [1, 0x8]
|
||||
STEER = [2, 1]
|
||||
BRAKE_PRESSED = [3, 10]
|
||||
GEAR_NOT_D = [4, 6]
|
||||
SEATBELT = [5, 5]
|
||||
SPEED_TOO_HIGH = [6, 8]
|
||||
|
||||
class ET:
|
||||
ENABLE = 0
|
||||
NO_ENTRY = 1
|
||||
WARNING = 2
|
||||
SOFT_DISABLE = 3
|
||||
IMMEDIATE_DISABLE = 4
|
||||
USER_DISABLE = 5
|
||||
|
||||
def process_alert(alert_id, alert, cur_time, sound_exp, hud_exp, text_exp, alert_p):
|
||||
# INPUTS:
|
||||
# alert_id is mapped to the alert properties in alert_database
|
||||
# cur_time is current time
|
||||
# sound_exp is when the alert beep/chime is supposed to end
|
||||
# hud_exp is when the hud visual is supposed to end
|
||||
# text_exp is when the alert text is supposed to disappear
|
||||
# alert_p is the priority of the current alert
|
||||
# CM, BP, AH are classes defined in alert_database and they respresents chimes, beeps and hud_alerts
|
||||
if len(alert_id) > 0:
|
||||
# take the alert with higher priority
|
||||
alerts_present = filter(lambda a_id: a_id['id'] in alert_id, alerts)
|
||||
alert = sorted(alerts_present, key=lambda k: k['priority'])[-1]
|
||||
# check if we have a more important alert
|
||||
if alert['priority'] > alert_p:
|
||||
alert_p = alert['priority']
|
||||
sound_exp = cur_time + alert['duration_sound']
|
||||
hud_exp = cur_time + alert['duration_hud_alert']
|
||||
text_exp = cur_time + alert['duration_text']
|
||||
|
||||
chime = CM.MUTE
|
||||
beep = BP.MUTE
|
||||
if cur_time < sound_exp:
|
||||
chime = alert['chime']
|
||||
beep = alert['beep']
|
||||
|
||||
hud_alert = AH.NONE
|
||||
if cur_time < hud_exp:
|
||||
hud_alert = alert['hud_alert']
|
||||
|
||||
alert_text = ["", ""]
|
||||
if cur_time < text_exp:
|
||||
alert_text = [alert['text_line_1'], alert['text_line_2']]
|
||||
|
||||
if chime == CM.MUTE and beep == BP.MUTE and hud_alert == AH.NONE: #and alert_text[0] is None and alert_text[1] is None:
|
||||
alert_p = 0
|
||||
return alert, chime, beep, hud_alert, alert_text, sound_exp, hud_exp, text_exp, alert_p
|
||||
|
||||
def process_hud_alert(hud_alert):
|
||||
# initialize to no alert
|
||||
fcw_display = 0
|
||||
steer_required = 0
|
||||
acc_alert = 0
|
||||
if hud_alert == AH.NONE: # no alert
|
||||
pass
|
||||
elif hud_alert == AH.FCW: # FCW
|
||||
fcw_display = hud_alert[1]
|
||||
elif hud_alert == AH.STEER: # STEER
|
||||
steer_required = hud_alert[1]
|
||||
else: # any other ACC alert
|
||||
acc_alert = hud_alert[1]
|
||||
|
||||
return fcw_display, steer_required, acc_alert
|
||||
|
||||
def app_alert(alert_add):
|
||||
alerts.append(dict(zip(keys, alert_add)))
|
||||
|
||||
app_alert([AI.ENABLE, CM.MUTE, BP.SINGLE, AH.NONE, ET.ENABLE, 2, "", "", .2, 0., 0.])
|
||||
app_alert([AI.DISABLE, CM.MUTE, BP.SINGLE, AH.NONE, ET.USER_DISABLE, 2, "", "", .2, 0., 0.])
|
||||
app_alert([AI.SEATBELT, CM.DOUBLE, BP.MUTE, AH.SEATBELT, ET.NO_ENTRY, 1, "Comma Unavailable", "Seatbelt Unlatched", .4, 2., 3.])
|
||||
app_alert([AI.DOOR_OPEN, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Door Open", .4, 0., 3.])
|
||||
app_alert([AI.PEDAL_PRESSED, CM.DOUBLE, BP.MUTE, AH.BRAKE_PRESSED, ET.NO_ENTRY, 1, "Comma Unavailable", "Pedal Pressed", .4, 2., 3.])
|
||||
app_alert([AI.COMM_ISSUE, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Communcation Issues", .4, 0., 3.])
|
||||
app_alert([AI.ESP_OFF, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "ESP Off", .4, 0., 3.])
|
||||
app_alert([AI.FCW, CM.REPEATED, BP.MUTE, AH.FCW, ET.WARNING, 3, "Risk of Collision", "", 1., 2., 3.])
|
||||
app_alert([AI.STEER_ERROR, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Steer Error", .4, 0., 3.])
|
||||
app_alert([AI.BRAKE_ERROR, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Brake Error", .4, 0., 3.])
|
||||
app_alert([AI.CALIB_INCOMPLETE, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Calibration in Progress", .4, 0., 3.])
|
||||
app_alert([AI.CALIB_INVALID, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Calibration Error", .4, 0., 3.])
|
||||
app_alert([AI.GEAR_NOT_D, CM.DOUBLE, BP.MUTE, AH.GEAR_NOT_D, ET.NO_ENTRY, 1, "Comma Unavailable", "Gear not in D", .4, 2., 3.])
|
||||
app_alert([AI.MAIN_OFF, CM.MUTE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Main Switch Off", .4, 0., 3.])
|
||||
app_alert([AI.STEER_SATURATED, CM.SINGLE, BP.MUTE, AH.STEER, ET.WARNING, 2, "Take Control", "Steer Control Saturated", 1., 2., 3.])
|
||||
app_alert([AI.PCM_LOW_SPEED, CM.MUTE, BP.SINGLE, AH.STEER, ET.WARNING, 2, "Comma disengaged", "Speed too low", .2, 2., 3.])
|
||||
app_alert([AI.THERMAL_DEAD, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Thermal Unavailable", .4, 0., 3.])
|
||||
app_alert([AI.OVERHEAT, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "System Overheated", .4, 0., 3.])
|
||||
app_alert([AI.HIGH_SPEED, CM.DOUBLE, BP.MUTE, AH.SPEED_TOO_HIGH, ET.NO_ENTRY, 1, "Comma Unavailable", "Speed Too High", .4, 2., 3.])
|
||||
app_alert([AI.CONTROLSD_LAG, CM.DOUBLE, BP.MUTE, AH.NONE, ET.NO_ENTRY, 1, "Comma Unavailable", "Controls Lagging", .4, 0., 3.])
|
||||
app_alert([AI.STEER_ERROR_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Steer Error", 1., 3., 3.])
|
||||
app_alert([AI.BRAKE_ERROR_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Brake Error", 1., 3., 3.])
|
||||
app_alert([AI.PCM_MISMATCH_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Pcm Mismatch", 1., 3., 3.])
|
||||
app_alert([AI.CTRL_MISMATCH_ID, CM.REPEATED, BP.MUTE, AH.STEER, ET.IMMEDIATE_DISABLE, 3, "Take Control Immediately", "Ctrl Mismatch", 1., 3., 3.])
|
||||
app_alert([AI.SEATBELT_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Seatbelt Unlatched", 1., 3., 3.])
|
||||
app_alert([AI.DOOR_OPEN_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Door Open", 1., 3., 3.])
|
||||
app_alert([AI.COMM_ISSUE_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Technical Issues", 1., 3., 3.])
|
||||
app_alert([AI.ESP_OFF_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "ESP Off", 1., 3., 3.])
|
||||
app_alert([AI.THERMAL_DEAD_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Thermal Unavailable", 1., 3., 3.])
|
||||
app_alert([AI.OVERHEAT_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "System Overheated", 1., 3., 3.])
|
||||
app_alert([AI.CONTROLSD_LAG_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Controls Lagging", 1., 3., 3.])
|
||||
app_alert([AI.CALIB_INCOMPLETE_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Calibration in Progress", 1., 3., 3.])
|
||||
app_alert([AI.CALIB_INVALID_SD, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 3, "Take Control Immediately", "Calibration Error", 1., 3., 3.])
|
||||
app_alert([AI.DRIVER_DISTRACTED, CM.REPEATED, BP.MUTE, AH.STEER, ET.SOFT_DISABLE, 2, "Take Control to Regain Speed", "User Distracted", 1., 1., 1.])
|
||||
@@ -0,0 +1,123 @@
|
||||
import os
|
||||
import dbcs
|
||||
from collections import defaultdict
|
||||
|
||||
from selfdrive.controls.lib.hondacan import fix
|
||||
from common.realtime import sec_since_boot
|
||||
from common.dbc import dbc
|
||||
|
||||
class CANParser(object):
|
||||
def __init__(self, dbc_f, signals, checks=[]):
|
||||
### input:
|
||||
# dbc_f : dbc file
|
||||
# signals : List of tuples (name, address, ival) where
|
||||
# - name is the signal name.
|
||||
# - address is the corresponding message address.
|
||||
# - ival is the initial value.
|
||||
# checks : List of pairs (address, frequency) where
|
||||
# - address is the message address of a message for which health should be
|
||||
# monitored.
|
||||
# - frequency is the frequency at which health should be monitored.
|
||||
|
||||
self.msgs_ck = [check[0] for check in checks]
|
||||
self.frqs = [check[1] for check in checks]
|
||||
self.can_valid = False # start with False CAN assumption
|
||||
self.msgs_upd = [] # list of updated messages
|
||||
# list of received msg we want to monitor counter and checksum for
|
||||
# read dbc file
|
||||
self.can_dbc = dbc(os.path.join(dbcs.DBC_PATH, dbc_f))
|
||||
# initialize variables to initial values
|
||||
self.vl = {} # signal values
|
||||
self.ts = {} # time stamp recorded in log
|
||||
self.ct = {} # current time stamp
|
||||
self.ok = {} # valid message?
|
||||
self.cn = {} # message counter
|
||||
self.cn_vl = {} # message counter mismatch value
|
||||
self.ck = {} # message checksum status
|
||||
|
||||
for _, addr, _ in signals:
|
||||
self.vl[addr] = {}
|
||||
self.ts[addr] = 0
|
||||
self.ct[addr] = sec_since_boot()
|
||||
self.ok[addr] = False
|
||||
self.cn[addr] = 0
|
||||
self.cn_vl[addr] = 0
|
||||
self.ck[addr] = False
|
||||
|
||||
for name, addr, ival in signals:
|
||||
self.vl[addr][name] = ival
|
||||
|
||||
self._msgs = [s[1] for s in signals]
|
||||
self._sgs = [s[0] for s in signals]
|
||||
|
||||
self._message_indices = defaultdict(list)
|
||||
for i, x in enumerate(self._msgs):
|
||||
self._message_indices[x].append(i)
|
||||
|
||||
def update_can(self, can_recv):
|
||||
self.msgs_upd = []
|
||||
cn_vl_max = 5 # no more than 5 wrong counter checks
|
||||
|
||||
# we are subscribing to PID_XXX, else data from USB
|
||||
for msg, ts, cdat in can_recv:
|
||||
idxs = self._message_indices[msg]
|
||||
if idxs:
|
||||
self.msgs_upd += [msg]
|
||||
# read the entire message
|
||||
out = self.can_dbc.decode([msg, 0, cdat])[1]
|
||||
# checksum check
|
||||
self.ck[msg] = True
|
||||
if "CHECKSUM" in out.keys() and msg in self.msgs_ck:
|
||||
# remove checksum (half byte)
|
||||
ck_portion = (''.join((cdat[:-1], '0'))).decode('hex')
|
||||
# recalculate checksum
|
||||
msg_vl = fix(ck_portion, msg)
|
||||
# compare recalculated vs received checksum
|
||||
if msg_vl != cdat.decode('hex'):
|
||||
print hex(msg), "CHECKSUM FAIL"
|
||||
self.ck[msg] = False
|
||||
self.ok[msg] = False
|
||||
# counter check
|
||||
cn = 0
|
||||
if "COUNTER" in out.keys():
|
||||
cn = out["COUNTER"]
|
||||
# check counter validity if it's a relevant message
|
||||
if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys():
|
||||
#print hex(msg), "FAILED COUNTER!"
|
||||
self.cn_vl[msg] += 1 # counter check failed
|
||||
else:
|
||||
self.cn_vl[msg] -= 1 # counter check passed
|
||||
# message status is invalid if we received too many wrong counter values
|
||||
if self.cn_vl[msg] >= cn_vl_max:
|
||||
self.ok[msg] = False
|
||||
|
||||
# update msg time stamps and counter value
|
||||
self.ts[msg] = ts
|
||||
self.ct[msg] = sec_since_boot()
|
||||
self.cn[msg] = cn
|
||||
self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
|
||||
|
||||
# set msg valid status if checksum is good and wrong counter counter is zero
|
||||
if self.ck[msg] and self.cn_vl[msg] == 0:
|
||||
self.ok[msg] = True
|
||||
|
||||
# update value of signals in the
|
||||
for ii in idxs:
|
||||
sg = self._sgs[ii]
|
||||
self.vl[msg][sg] = out[sg]
|
||||
|
||||
# for each message, check if it's too long since last time we received it
|
||||
self._check_dead_msgs()
|
||||
|
||||
# assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
|
||||
self.can_valid = True
|
||||
if False in self.ok.values():
|
||||
print "CAN INVALID!"
|
||||
self.can_valid = False
|
||||
|
||||
def _check_dead_msgs(self):
|
||||
### input:
|
||||
## simple stuff for now: msg is not valid if a message isn't received for 10 consecutive steps
|
||||
for msg in set(self._msgs):
|
||||
if msg in self.msgs_ck and sec_since_boot() - self.ct[msg] > 10./self.frqs[self.msgs_ck.index(msg)]:
|
||||
self.ok[msg] = False
|
||||
@@ -0,0 +1,185 @@
|
||||
from collections import namedtuple
|
||||
|
||||
import common.numpy_fast as np
|
||||
import selfdrive.controls.lib.hondacan as hondacan
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.config import CruiseButtons
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.controls.lib.alert_database import process_hud_alert
|
||||
from selfdrive.controls.lib.drive_helpers import actuator_hystereses, rate_limit
|
||||
|
||||
HUDData = namedtuple("HUDData",
|
||||
["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
|
||||
"lanes", "beep", "X8", "chime", "acc_alert"])
|
||||
|
||||
class CarController(object):
|
||||
def __init__(self):
|
||||
self.controls_allowed = False
|
||||
self.mismatch_start, self.pcm_mismatch_start = 0, 0
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.final_brake_last = 0.
|
||||
|
||||
def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
|
||||
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
|
||||
snd_beep, snd_chime):
|
||||
""" Controls thread """
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego, CS.civic)
|
||||
|
||||
# *** no output if not enabled ***
|
||||
if not enabled:
|
||||
final_gas = 0.
|
||||
final_brake = 0.
|
||||
final_steer = 0.
|
||||
# send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
|
||||
if CS.pcm_acc_status:
|
||||
pcm_cancel_cmd = True
|
||||
|
||||
# *** rate limit after the enable check ***
|
||||
final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
|
||||
self.final_brake_last = final_brake
|
||||
|
||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||
#TODO: use enum!!
|
||||
if hud_show_lanes:
|
||||
hud_lanes = 0x04
|
||||
else:
|
||||
hud_lanes = 0x00
|
||||
|
||||
# TODO: factor this out better
|
||||
if enabled:
|
||||
if hud_show_car:
|
||||
hud_car = 0xe0
|
||||
else:
|
||||
hud_car = 0xd0
|
||||
else:
|
||||
hud_car = 0xc0
|
||||
|
||||
#print chime, alert_id, hud_alert
|
||||
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
|
||||
|
||||
hud = HUDData(pcm_accel, hud_v_cruise, 0x41, hud_car,
|
||||
0xc1, 0x41, hud_lanes + steer_required,
|
||||
snd_beep, 0x48, (snd_chime << 5) + fcw_display, acc_alert)
|
||||
|
||||
if not all(isinstance(x, int) and 0 <= x < 256 for x in hud):
|
||||
print "INVALID HUD", hud
|
||||
hud = HUDData(0xc6, 255, 64, 0xc0, 209, 0x41, 0x40, 0, 0x48, 0, 0)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
user_brake_ctrl = CS.user_brake/0.015625 # FIXME: factor needed to convert to old scale
|
||||
|
||||
# *** compute control surfaces ***
|
||||
tt = sec_since_boot()
|
||||
GAS_MAX = 1004
|
||||
BRAKE_MAX = 1024/4
|
||||
#STEER_MAX = 0xF00 if not CS.torque_mod else 0xF00/4 # ilx has 8x steering torque limit, used as a 2x
|
||||
STEER_MAX = 0xF00 # ilx has 8x steering torque limit, used as a 2x
|
||||
GAS_OFFSET = 328
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_gas = int(np.clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
|
||||
apply_brake = int(np.clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
|
||||
apply_steer = int(np.clip(-final_steer*STEER_MAX, -STEER_MAX, STEER_MAX))
|
||||
|
||||
# no gas if you are hitting the brake or the user is
|
||||
if apply_gas > 0 and (apply_brake != 0 or user_brake_ctrl > 10):
|
||||
print "CANCELLING GAS", apply_brake, user_brake_ctrl
|
||||
apply_gas = 0
|
||||
|
||||
# no computer brake if the user is hitting the gas
|
||||
# if the computer is trying to brake, it can't be hitting the gas
|
||||
# TODO: car_gas can override brakes without canceling... this is bad
|
||||
if CS.car_gas > 0 and apply_brake != 0:
|
||||
apply_brake = 0
|
||||
|
||||
if (CS.prev_cruise_buttons == CruiseButtons.DECEL_SET or CS.prev_cruise_buttons == CruiseButtons.RES_ACCEL) and \
|
||||
CS.cruise_buttons == 0 and not self.controls_allowed:
|
||||
print "CONTROLS ARE LIVE"
|
||||
self.controls_allowed = True
|
||||
|
||||
# to avoid race conditions, check if control has been disabled for at least 0.2s
|
||||
# keep resetting start timer if mismatch isn't true
|
||||
if not (self.controls_allowed and not enabled):
|
||||
self.mismatch_start = tt
|
||||
|
||||
# to avoid race conditions, check if control is disabled but pcm control is on for at least 0.2s
|
||||
if not (not self.controls_allowed and CS.pcm_acc_status):
|
||||
self.pcm_mismatch_start = tt
|
||||
|
||||
# something is very wrong, since pcm control is active but controls should not be allowed; TODO: send pcm fault cmd?
|
||||
if (tt - self.pcm_mismatch_start) > 0.2:
|
||||
pcm_cancel_cmd = True
|
||||
|
||||
# TODO: clean up gear condition, ideally only D (and P for debug) shall be valid gears
|
||||
if (CS.cruise_buttons == CruiseButtons.CANCEL or CS.brake_pressed or
|
||||
CS.user_gas_pressed or (tt - self.mismatch_start) > 0.2 or
|
||||
not CS.main_on or not CS.gear_shifter_valid or
|
||||
(CS.pedal_gas > 0 and CS.brake_only)) and self.controls_allowed:
|
||||
self.controls_allowed = False
|
||||
|
||||
# 5 is a permanent fault, no torque request will be fullfilled
|
||||
if CS.steer_error:
|
||||
print "STEER ERROR"
|
||||
self.controls_allowed = False
|
||||
|
||||
# any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
|
||||
elif CS.steer_not_allowed:
|
||||
print "STEER ALERT, TORQUE INHIBITED"
|
||||
apply_steer = 0
|
||||
|
||||
if CS.brake_error:
|
||||
print "BRAKE ERROR"
|
||||
self.controls_allowed = False
|
||||
|
||||
if not CS.can_valid and self.controls_allowed: # 200 ms
|
||||
print "CAN INVALID"
|
||||
self.controls_allowed = False
|
||||
|
||||
if not self.controls_allowed:
|
||||
apply_steer = 0
|
||||
apply_gas = 0
|
||||
apply_brake = 0
|
||||
pcm_speed = 0 # make sure you send 0 target speed to pcm
|
||||
#pcm_cancel_cmd = 1 # prevent pcm control from turning on. FIXME: we can't just do this
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
|
||||
# Send steering command.
|
||||
idx = frame % 4
|
||||
can_sends.append(hondacan.create_steering_control(apply_steer, idx))
|
||||
|
||||
# Send gas and brake commands.
|
||||
if (frame % 2) == 0:
|
||||
idx = (frame / 2) % 4
|
||||
can_sends.append(
|
||||
hondacan.create_brake_command(apply_brake, pcm_override,
|
||||
pcm_cancel_cmd, hud.chime, idx))
|
||||
|
||||
if not CS.brake_only:
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
gas_amount = (apply_gas + GAS_OFFSET) * (apply_gas > 0)
|
||||
can_sends.append(hondacan.create_gas_command(gas_amount, idx))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
idx = (frame/10) % 4
|
||||
can_sends.extend(hondacan.create_ui_commands(pcm_speed, hud, CS.civic, idx))
|
||||
|
||||
# radar at 20Hz, but these msgs need to be sent at 50Hz on ilx (seems like an Acura bug)
|
||||
if CS.civic:
|
||||
radar_send_step = 5
|
||||
else:
|
||||
radar_send_step = 2
|
||||
|
||||
if (frame % radar_send_step) == 0:
|
||||
idx = (frame/radar_send_step) % 4
|
||||
can_sends.extend(hondacan.create_radar_commands(CS.v_ego, CS.civic, idx))
|
||||
|
||||
sendcan.send(can_list_to_can_capnp(can_sends).to_bytes())
|
||||
@@ -0,0 +1,275 @@
|
||||
import numpy as np
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.boardd.boardd import can_capnp_to_can_list_old, can_capnp_to_can_list
|
||||
from selfdrive.controls.lib.can_parser import CANParser
|
||||
from selfdrive.controls.lib.fingerprints import fingerprints
|
||||
from selfdrive.config import VehicleParams
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
|
||||
def get_can_parser(civic, brake_only):
|
||||
# this function generates lists for signal, messages and initial values
|
||||
if civic:
|
||||
dbc_f = 'honda_civic_touring_2016_can.dbc'
|
||||
signals = [
|
||||
("XMISSION_SPEED", 0x158, 0),
|
||||
("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
("STEER_ANGLE", 0x14a, 0),
|
||||
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
("GEAR", 0x191, 0),
|
||||
("WHEELS_MOVING", 0x1b0, 1),
|
||||
("DOOR_OPEN_FL", 0x405, 1),
|
||||
("DOOR_OPEN_FR", 0x405, 1),
|
||||
("DOOR_OPEN_RL", 0x405, 1),
|
||||
("DOOR_OPEN_RR", 0x405, 1),
|
||||
("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
("BRAKE_PRESSED", 0x17c, 0),
|
||||
("CAR_GAS", 0x130, 0),
|
||||
("CRUISE_BUTTONS", 0x296, 0),
|
||||
("ESP_DISABLED", 0x1a4, 1),
|
||||
("HUD_LEAD", 0x30c, 0),
|
||||
("USER_BRAKE", 0x1a4, 0),
|
||||
("STEER_STATUS", 0x18f, 5),
|
||||
("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
("GEAR_SHIFTER", 0x191, 0),
|
||||
("MAIN_ON", 0x326, 0),
|
||||
("ACC_STATUS", 0x17c, 0),
|
||||
("PEDAL_GAS", 0x17c, 0),
|
||||
("CRUISE_SETTING", 0x296, 0),
|
||||
("LEFT_BLINKER", 0x326, 0),
|
||||
("RIGHT_BLINKER", 0x326, 0),
|
||||
("COUNTER", 0x324, 0),
|
||||
]
|
||||
checks = [
|
||||
(0x14a, 100),
|
||||
(0x158, 100),
|
||||
(0x17c, 100),
|
||||
(0x191, 100),
|
||||
(0x1a4, 50),
|
||||
(0x326, 10),
|
||||
(0x1b0, 50),
|
||||
(0x1d0, 50),
|
||||
(0x305, 10),
|
||||
(0x324, 10),
|
||||
(0x405, 3),
|
||||
]
|
||||
|
||||
else:
|
||||
dbc_f = 'acura_ilx_2016_can.dbc'
|
||||
signals = [
|
||||
("XMISSION_SPEED", 0x158, 0),
|
||||
("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
("STEER_ANGLE", 0x156, 0),
|
||||
("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
("GEAR", 0x1a3, 0),
|
||||
("WHEELS_MOVING", 0x1b0, 1),
|
||||
("DOOR_OPEN_FL", 0x405, 1),
|
||||
("DOOR_OPEN_FR", 0x405, 1),
|
||||
("DOOR_OPEN_RL", 0x405, 1),
|
||||
("DOOR_OPEN_RR", 0x405, 1),
|
||||
("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
("BRAKE_PRESSED", 0x17c, 0),
|
||||
("CAR_GAS", 0x130, 0),
|
||||
("CRUISE_BUTTONS", 0x1a6, 0),
|
||||
("ESP_DISABLED", 0x1a4, 1),
|
||||
("HUD_LEAD", 0x30c, 0),
|
||||
("USER_BRAKE", 0x1a4, 0),
|
||||
("STEER_STATUS", 0x18f, 5),
|
||||
("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
("GEAR_SHIFTER", 0x1a3, 0),
|
||||
("MAIN_ON", 0x1a6, 0),
|
||||
("ACC_STATUS", 0x17c, 0),
|
||||
("PEDAL_GAS", 0x17c, 0),
|
||||
("CRUISE_SETTING", 0x1a6, 0),
|
||||
("LEFT_BLINKER", 0x294, 0),
|
||||
("RIGHT_BLINKER", 0x294, 0),
|
||||
("COUNTER", 0x324, 0),
|
||||
]
|
||||
checks = [
|
||||
(0x156, 100),
|
||||
(0x158, 100),
|
||||
(0x17c, 100),
|
||||
(0x1a3, 50),
|
||||
(0x1a4, 50),
|
||||
(0x1a6, 50),
|
||||
(0x1b0, 50),
|
||||
(0x1d0, 50),
|
||||
(0x305, 10),
|
||||
(0x324, 10),
|
||||
(0x405, 3),
|
||||
]
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if not brake_only:
|
||||
signals.append(("INTERCEPTOR_GAS", 0x201, 0))
|
||||
checks.append((0x201, 50))
|
||||
|
||||
return CANParser(dbc_f, signals, checks)
|
||||
|
||||
def fingerprint(logcan):
|
||||
print "waiting for fingerprint..."
|
||||
brake_only = True
|
||||
|
||||
finger = {}
|
||||
st = None
|
||||
while 1:
|
||||
possible_cars = []
|
||||
for a in messaging.drain_sock(logcan, wait_for_one=True):
|
||||
if st is None:
|
||||
st = sec_since_boot()
|
||||
for adr, _, msg, idx in can_capnp_to_can_list(a):
|
||||
# pedal
|
||||
if adr == 0x201 and idx == 0:
|
||||
brake_only = False
|
||||
if idx == 0:
|
||||
finger[adr] = len(msg)
|
||||
|
||||
# check for a single match
|
||||
for f in fingerprints:
|
||||
is_possible = True
|
||||
for adr in finger:
|
||||
# confirm all messages we have seen match
|
||||
if adr not in fingerprints[f] or fingerprints[f][adr] != finger[adr]:
|
||||
#print "mismatch", f, adr
|
||||
is_possible = False
|
||||
break
|
||||
if is_possible:
|
||||
possible_cars.append(f)
|
||||
|
||||
# if we only have one car choice and it's been 100ms since we got our first message, exit
|
||||
if len(possible_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
|
||||
break
|
||||
elif len(possible_cars) == 0:
|
||||
raise Exception("car doesn't match any fingerprints")
|
||||
|
||||
print "fingerprinted", possible_cars[0]
|
||||
return brake_only, possible_cars[0]
|
||||
|
||||
class CarState(object):
|
||||
def __init__(self, logcan):
|
||||
self.torque_mod = False
|
||||
self.brake_only, self.car_type = fingerprint(logcan)
|
||||
|
||||
# assuming if you have a pedal interceptor you also have a torque mod
|
||||
if not self.brake_only:
|
||||
self.torque_mod = True
|
||||
|
||||
if self.car_type == "HONDA CIVIC 2016 TOURING":
|
||||
self.civic = True
|
||||
elif self.car_type == "ACURA ILX 2016 ACURAWATCH PLUS":
|
||||
self.civic = False
|
||||
else:
|
||||
raise ValueError("unsupported car %s" % self.car_type)
|
||||
|
||||
# initialize can parser
|
||||
self.cp = get_can_parser(self.civic, self.brake_only)
|
||||
|
||||
self.user_gas, self.user_gas_pressed = 0., 0
|
||||
|
||||
self.cruise_buttons = 0
|
||||
self.cruise_setting = 0
|
||||
self.blinker_on = 0
|
||||
|
||||
# TODO: actually make this work
|
||||
self.a_ego = 0.
|
||||
|
||||
# speed in UI is shown as few % higher
|
||||
self.ui_speed_fudge = 1.01 if self.civic else 1.025
|
||||
|
||||
# load vehicle params
|
||||
self.VP = VehicleParams(self.civic)
|
||||
|
||||
def update(self, logcan):
|
||||
# ******************* do can recv *******************
|
||||
can_pub_main = []
|
||||
canMonoTimes = []
|
||||
for a in messaging.drain_sock(logcan):
|
||||
canMonoTimes.append(a.logMonoTime)
|
||||
can_pub_main.extend(can_capnp_to_can_list_old(a, [0,2]))
|
||||
|
||||
cp = self.cp
|
||||
cp.update_can(can_pub_main)
|
||||
|
||||
# copy can_valid
|
||||
self.can_valid = cp.can_valid
|
||||
|
||||
# car params
|
||||
v_weight_v = [0., 1. ] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.prev_cruise_setting = self.cruise_setting
|
||||
self.prev_blinker_on = self.blinker_on
|
||||
|
||||
# ******************* parse out can *******************
|
||||
self.door_all_closed = not any([cp.vl[0x405]['DOOR_OPEN_FL'], cp.vl[0x405]['DOOR_OPEN_FR'],
|
||||
cp.vl[0x405]['DOOR_OPEN_RL'], cp.vl[0x405]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl[0x305]['SEATBELT_DRIVER_LAMP'] and cp.vl[0x305]['SEATBELT_DRIVER_LATCHED']
|
||||
# error 2 = temporary
|
||||
# error 4 = temporary, hit a bump
|
||||
# error 5 (permanent)
|
||||
# error 6 = temporary
|
||||
# error 7 (permanent)
|
||||
#self.steer_error = cp.vl[0x18F]['STEER_STATUS'] in [5,7]
|
||||
# whitelist instead of blacklist, safer at the expense of disengages
|
||||
self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6]
|
||||
self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0
|
||||
if cp.vl[0x18F]['STEER_STATUS'] != 0:
|
||||
print cp.vl[0x18F]['STEER_STATUS']
|
||||
self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2']
|
||||
self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED']
|
||||
# calc best v_ego estimate, by averaging two opposite corners
|
||||
self.v_wheel = (
|
||||
cp.vl[0x1D0]['WHEEL_SPEED_FL'] + cp.vl[0x1D0]['WHEEL_SPEED_FR'] +
|
||||
cp.vl[0x1D0]['WHEEL_SPEED_RL'] + cp.vl[0x1D0]['WHEEL_SPEED_RR']) / 4.
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
self.v_weight = np.interp(self.v_wheel, v_weight_bp, v_weight_v)
|
||||
self.v_ego = (1. - self.v_weight) * cp.vl[0x158]['XMISSION_SPEED'] + self.v_weight * self.v_wheel
|
||||
if not self.brake_only:
|
||||
self.user_gas = cp.vl[0x201]['INTERCEPTOR_GAS']
|
||||
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
||||
#print user_gas, user_gas_pressed
|
||||
if self.civic:
|
||||
self.gear_shifter = cp.vl[0x191]['GEAR_SHIFTER']
|
||||
self.angle_steers = cp.vl[0x14A]['STEER_ANGLE']
|
||||
self.gear = 0 # TODO: civic has CVT... needs rev engineering
|
||||
self.cruise_setting = cp.vl[0x296]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl[0x296]['CRUISE_BUTTONS']
|
||||
self.main_on = cp.vl[0x326]['MAIN_ON']
|
||||
self.gear_shifter_valid = self.gear_shifter in [1,8] # TODO: 1/P allowed for debug
|
||||
self.blinker_on = cp.vl[0x326]['LEFT_BLINKER'] or cp.vl[0x326]['RIGHT_BLINKER']
|
||||
else:
|
||||
self.gear_shifter = cp.vl[0x1A3]['GEAR_SHIFTER']
|
||||
self.angle_steers = cp.vl[0x156]['STEER_ANGLE']
|
||||
self.gear = cp.vl[0x1A3]['GEAR']
|
||||
self.cruise_setting = cp.vl[0x1A6]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl[0x1A6]['CRUISE_BUTTONS']
|
||||
self.main_on = cp.vl[0x1A6]['MAIN_ON']
|
||||
self.gear_shifter_valid = self.gear_shifter in [1,4] # TODO: 1/P allowed for debug
|
||||
self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER']
|
||||
self.car_gas = cp.vl[0x130]['CAR_GAS']
|
||||
self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED']
|
||||
self.user_brake = cp.vl[0x1A4]['USER_BRAKE']
|
||||
self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING']
|
||||
self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200
|
||||
self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM']
|
||||
self.pcm_acc_status = cp.vl[0x17C]['ACC_STATUS']
|
||||
self.pedal_gas = cp.vl[0x17C]['PEDAL_GAS']
|
||||
self.hud_lead = cp.vl[0x30C]['HUD_LEAD']
|
||||
self.counter_pcm = cp.vl[0x324]['COUNTER']
|
||||
|
||||
return canMonoTimes
|
||||
@@ -0,0 +1,52 @@
|
||||
import numpy as np
|
||||
|
||||
def rate_limit(new_value, last_value, dw_step, up_step):
|
||||
return np.clip(new_value, last_value + dw_step, last_value + up_step)
|
||||
|
||||
def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override):
|
||||
# simple integral controller that learns how much steering offset to put to have the car going straight
|
||||
min_offset = -1. # deg
|
||||
max_offset = 1. # deg
|
||||
alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz
|
||||
min_learn_speed = 1.
|
||||
|
||||
# learn less at low speed or when turning
|
||||
alpha_v = alpha*(np.maximum(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des))
|
||||
|
||||
# only learn if lateral control is active and if driver is not overriding:
|
||||
if lateral_control and not steer_override:
|
||||
angle_offset += d_poly[3] * alpha_v
|
||||
angle_offset = np.clip(angle_offset, min_offset, max_offset)
|
||||
|
||||
return angle_offset
|
||||
|
||||
def actuator_hystereses(final_brake, braking, brake_steady, v_ego, civic):
|
||||
# hyst params... TODO: move these to VehicleParams
|
||||
brake_hyst_on = 0.055 if civic else 0.1 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
|
||||
|
||||
#*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
|
||||
if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
|
||||
final_brake = 0.
|
||||
braking = final_brake > 0.
|
||||
|
||||
# for small brake oscillations within brake_hyst_gap, don't change the brake command
|
||||
if final_brake == 0.:
|
||||
brake_steady = 0.
|
||||
elif final_brake > brake_steady + brake_hyst_gap:
|
||||
brake_steady = final_brake - brake_hyst_gap
|
||||
elif final_brake < brake_steady - brake_hyst_gap:
|
||||
brake_steady = final_brake + brake_hyst_gap
|
||||
final_brake = brake_steady
|
||||
|
||||
if not civic:
|
||||
brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
|
||||
brake_on_offset_bp = [15., 30.] # offset changes VS speed to not have too abrupt decels at high speeds
|
||||
# offset the brake command for threshold in the brake system. no brake torque perceived below it
|
||||
brake_on_offset = np.interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
|
||||
brake_offset = brake_on_offset - brake_hyst_on
|
||||
if final_brake > 0.0:
|
||||
final_brake += brake_offset
|
||||
|
||||
return final_brake, braking, brake_steady
|
||||
@@ -0,0 +1,8 @@
|
||||
fingerprints = {
|
||||
"ACURA ILX 2016 ACURAWATCH PLUS": {
|
||||
1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 929L: 4, 1057L: 5, 777L: 8, 1034L: 5, 1036L: 8, 398L: 3, 399L: 7, 145L: 8, 660L: 8, 985L: 3, 923L: 2, 542L: 7, 773L: 7, 800L: 8, 432L: 7, 419L: 8, 420L: 8, 1030L: 5, 422L: 8, 808L: 8, 428L: 8, 304L: 8, 819L: 7, 821L: 5, 57L: 3, 316L: 8, 545L: 4, 464L: 8, 1108L: 8, 597L: 8, 342L: 6, 983L: 8, 344L: 8, 804L: 8, 1039L: 8, 476L: 4, 892L: 8, 490L: 8, 1064L: 7, 882L: 2, 884L: 7, 887L: 8, 888L: 8, 380L: 8, 1365L: 5
|
||||
},
|
||||
"HONDA CIVIC 2016 TOURING": {
|
||||
1024L: 5, 513L: 5, 1027L: 5, 1029L: 8, 777L: 8, 1036L: 8, 1039L: 8, 1424L: 5, 401L: 8, 148L: 8, 662L: 4, 985L: 3, 795L: 8, 773L: 7, 800L: 8, 545L: 6, 420L: 8, 806L: 8, 808L: 8, 1322L: 5, 427L: 3, 428L: 8, 304L: 8, 432L: 7, 57L: 3, 450L: 8, 929L: 8, 330L: 8, 1302L: 8, 464L: 8, 1361L: 5, 1108L: 8, 597L: 8, 470L: 2, 344L: 8, 804L: 8, 399L: 7, 476L: 7, 1633L: 8, 487L: 4, 892L: 8, 490L: 8, 493L: 5, 884L: 8, 891L: 8, 380L: 8, 1365L: 5
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
import struct
|
||||
|
||||
import common.numpy_fast as np
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
|
||||
# *** Honda specific ***
|
||||
def can_cksum(mm):
|
||||
s = 0
|
||||
for c in mm:
|
||||
c = ord(c)
|
||||
s += (c>>4)
|
||||
s += c & 0xF
|
||||
s = 8-s
|
||||
s %= 0x10
|
||||
return s
|
||||
|
||||
def fix(msg, addr):
|
||||
msg2 = msg[0:-1] + chr(ord(msg[-1]) | can_cksum(struct.pack("I", addr)+msg))
|
||||
return msg2
|
||||
|
||||
def make_can_msg(addr, dat, idx, alt):
|
||||
if idx is not None:
|
||||
dat += chr(idx << 4)
|
||||
dat = fix(dat, addr)
|
||||
return [addr, 0, dat, alt]
|
||||
|
||||
def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
|
||||
"""Creates a CAN message for the Honda DBC BRAKE_COMMAND."""
|
||||
pump_on = apply_brake > 0
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
|
||||
pcm_fault_cmd = False
|
||||
amount = struct.pack("!H", (apply_brake << 6) + pump_on)
|
||||
msg = amount + struct.pack("BBB", (pcm_override << 4) |
|
||||
(pcm_fault_cmd << 2) |
|
||||
(pcm_cancel_cmd << 1) | brake_rq, 0x80,
|
||||
brakelights << 7) + chr(chime) + "\x00"
|
||||
return make_can_msg(0x1fa, msg, idx, 0)
|
||||
|
||||
def create_gas_command(gas_amount, idx):
|
||||
"""Creates a CAN message for the Honda DBC GAS_COMMAND."""
|
||||
msg = struct.pack("!H", gas_amount)
|
||||
return make_can_msg(0x200, msg, idx, 0)
|
||||
|
||||
def create_steering_control(apply_steer, idx):
|
||||
"""Creates a CAN message for the Honda DBC STEERING_CONTROL."""
|
||||
msg = struct.pack("!h", apply_steer) + ("\x80\x00" if apply_steer != 0 else "\x00\x00")
|
||||
return make_can_msg(0xe4, msg, idx, 0)
|
||||
|
||||
def create_ui_commands(pcm_speed, hud, civic, idx):
|
||||
"""Creates an iterable of CAN messages for the UIs."""
|
||||
commands = []
|
||||
pcm_speed_real = np.clip(int(round(pcm_speed / 0.002763889)), 0,
|
||||
64000) # conversion factor from dbc file
|
||||
msg_0x30c = struct.pack("!HBBBBB", pcm_speed_real, hud.pcm_accel,
|
||||
hud.v_cruise, hud.X2, hud.car, hud.X4)
|
||||
commands.append(make_can_msg(0x30c, msg_0x30c, idx, 0))
|
||||
|
||||
msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8)
|
||||
commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0))
|
||||
if civic: # 2 more msgs
|
||||
msg_0x35e = chr(0) * 7
|
||||
commands.append(make_can_msg(0x35e, msg_0x35e, idx, 0))
|
||||
msg_0x39f = (
|
||||
chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0)
|
||||
)
|
||||
commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0))
|
||||
return commands
|
||||
|
||||
def create_radar_commands(v_ego, civic, idx):
|
||||
"""Creates an iterable of CAN messages for the radar system."""
|
||||
commands = []
|
||||
v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255)
|
||||
speed = struct.pack('!B', v_ego_kph)
|
||||
msg_0x300 = ("\xf9" + speed + "\x8a\xd0" +\
|
||||
("\x20" if idx == 0 or idx == 3 else "\x00") +\
|
||||
"\x00\x00")
|
||||
if civic:
|
||||
msg_0x301 = "\x02\x38\x44\x32\x4f\x00\x00"
|
||||
# add 8 on idx.
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1))
|
||||
else:
|
||||
msg_0x301 = "\x0f\x18\x51\x02\x5a\x00\x00"
|
||||
commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
|
||||
commands.append(make_can_msg(0x301, msg_0x301, idx, 1))
|
||||
return commands
|
||||
@@ -0,0 +1,120 @@
|
||||
import numpy as np
|
||||
|
||||
def calc_curvature(v_ego, angle_steers, VP, angle_offset=0):
|
||||
deg_to_rad = np.pi/180.
|
||||
angle_steers_rad = (angle_steers - angle_offset) * deg_to_rad
|
||||
curvature = angle_steers_rad/(VP.steer_ratio * VP.wheelbase * (1. + VP.slip_factor * v_ego**2))
|
||||
return curvature
|
||||
|
||||
def calc_d_lookahead(v_ego):
|
||||
#*** this function computes how far too look for lateral control
|
||||
# howfar we look ahead is function of speed
|
||||
offset_lookahead = 1.
|
||||
coeff_lookahead = 4.4
|
||||
# sqrt on speed is needed to keep, for a given curvature, the y_offset
|
||||
# proportional to speed. Indeed, y_offset is prop to d_lookahead^2
|
||||
# 26m at 25m/s
|
||||
d_lookahead = offset_lookahead + np.sqrt(np.maximum(v_ego, 0)) * coeff_lookahead
|
||||
return d_lookahead
|
||||
|
||||
def calc_lookahead_offset(v_ego, angle_steers, d_lookahead, VP, angle_offset):
|
||||
#*** this function return teh lateral offset given the steering angle, speed and the lookahead distance
|
||||
curvature = calc_curvature(v_ego, angle_steers, VP, angle_offset)
|
||||
|
||||
# clip is to avoid arcsin NaNs due to too sharp turns
|
||||
y_actual = d_lookahead * np.tan(np.arcsin(np.clip(d_lookahead * curvature, -0.999, 0.999))/2.)
|
||||
return y_actual, curvature
|
||||
|
||||
def pid_lateral_control(v_ego, y_actual, y_des, Ui_steer, steer_max,
|
||||
steer_override, sat_count, enabled, half_pid, rate):
|
||||
|
||||
sat_count_rate = 1./rate
|
||||
sat_count_limit = 0.8 # after 0.8s of continuous saturation, an alert will be sent
|
||||
|
||||
error_steer = y_des - y_actual
|
||||
Ui_unwind_speed = 0.3/rate #.3 per second
|
||||
if not half_pid:
|
||||
Kp, Ki = 12.0, 1.0
|
||||
else:
|
||||
Kp, Ki = 6.0, .5 # 2x limit in ILX
|
||||
Up_steer = error_steer*Kp
|
||||
Ui_steer_new = Ui_steer + error_steer*Ki * 1./rate
|
||||
output_steer_new = Ui_steer_new + Up_steer
|
||||
|
||||
# Anti-wind up for integrator: do not integrate if we are against the steer limits
|
||||
if (
|
||||
(error_steer >= 0. and (output_steer_new < steer_max or Ui_steer < 0)) or
|
||||
(error_steer <= 0. and
|
||||
(output_steer_new > -steer_max or Ui_steer > 0))) and not steer_override:
|
||||
#update integrator
|
||||
Ui_steer = Ui_steer_new
|
||||
# unwind integrator if driver is maneuvering the steering wheel
|
||||
elif steer_override:
|
||||
Ui_steer -= Ui_unwind_speed * np.sign(Ui_steer)
|
||||
|
||||
# still, intergral term should not be bigger then limits
|
||||
Ui_steer = np.clip(Ui_steer, -steer_max, steer_max)
|
||||
|
||||
output_steer = Up_steer + Ui_steer
|
||||
|
||||
# don't run steer control if at very low speed
|
||||
if v_ego < 0.3 or not enabled:
|
||||
output_steer = 0.
|
||||
Ui_steer = 0.
|
||||
|
||||
# useful to know if control is against the limit
|
||||
lateral_control_sat = False
|
||||
if abs(output_steer) > steer_max:
|
||||
lateral_control_sat = True
|
||||
|
||||
output_steer = np.clip(output_steer, -steer_max, steer_max)
|
||||
|
||||
# if lateral control is saturated for a certain period of time, send an alert for taking control of the car
|
||||
# wind
|
||||
if lateral_control_sat and not steer_override and v_ego > 10 and abs(error_steer) > 0.1:
|
||||
sat_count += sat_count_rate
|
||||
# unwind
|
||||
else:
|
||||
sat_count -= sat_count_rate
|
||||
|
||||
sat_flag = False
|
||||
if sat_count >= sat_count_limit:
|
||||
sat_flag = True
|
||||
|
||||
sat_count = np.clip(sat_count, 0, 1)
|
||||
|
||||
return output_steer, Up_steer, Ui_steer, lateral_control_sat, sat_count, sat_flag
|
||||
|
||||
class LatControl(object):
|
||||
def __init__(self):
|
||||
self.Up_steer = 0.
|
||||
self.sat_count = 0
|
||||
self.y_des = 0.0
|
||||
self.lateral_control_sat = False
|
||||
self.Ui_steer = 0.
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.Ui_steer = 0.
|
||||
|
||||
def update(self, enabled, CS, d_poly, angle_offset):
|
||||
rate = 100
|
||||
|
||||
steer_max = 1.0
|
||||
|
||||
# how far we look ahead is function of speed
|
||||
d_lookahead = calc_d_lookahead(CS.v_ego)
|
||||
|
||||
# calculate actual offset at the lookahead point
|
||||
self.y_actual, _ = calc_lookahead_offset(CS.v_ego, CS.angle_steers,
|
||||
d_lookahead, CS.VP, angle_offset)
|
||||
|
||||
# desired lookahead offset
|
||||
self.y_des = np.polyval(d_poly, d_lookahead)
|
||||
|
||||
output_steer, self.Up_steer, self.Ui_steer, self.lateral_control_sat, self.sat_count, sat_flag = pid_lateral_control(
|
||||
CS.v_ego, self.y_actual, self.y_des, self.Ui_steer, steer_max,
|
||||
CS.steer_override, self.sat_count, enabled, CS.torque_mod, rate)
|
||||
|
||||
final_steer = np.clip(output_steer, -steer_max, steer_max)
|
||||
return final_steer, sat_flag
|
||||
@@ -0,0 +1,232 @@
|
||||
import numpy as np
|
||||
from selfdrive.config import Conversions as CV
|
||||
|
||||
class LongCtrlState:
|
||||
#*** this function handles the long control state transitions
|
||||
# long_control_state labels:
|
||||
off = 0 # Off
|
||||
pid = 1 # moving and tracking targets, with PID control running
|
||||
stopping = 2 # stopping and changing controls to almost open loop as PID does not fit well at such a low speed
|
||||
starting = 3 # starting and releasing brakes in open loop before giving back to PID
|
||||
|
||||
def long_control_state_trans(enabled, long_control_state, v_ego, v_target, v_pid, output_gb):
|
||||
|
||||
stopping_speed = 0.5
|
||||
stopping_target_speed = 0.3
|
||||
starting_target_speed = 0.5
|
||||
brake_threshold_to_pid = 0.2
|
||||
|
||||
stopping_condition = ((v_ego < stopping_speed) and (v_pid < stopping_target_speed) and (v_target < stopping_target_speed))
|
||||
|
||||
if not enabled:
|
||||
long_control_state = LongCtrlState.off
|
||||
else:
|
||||
if long_control_state == LongCtrlState.off:
|
||||
if enabled:
|
||||
long_control_state = LongCtrlState.pid
|
||||
elif long_control_state == LongCtrlState.pid:
|
||||
if stopping_condition:
|
||||
long_control_state = LongCtrlState.stopping
|
||||
elif long_control_state == LongCtrlState.stopping:
|
||||
if (v_target > starting_target_speed):
|
||||
long_control_state = LongCtrlState.starting
|
||||
elif long_control_state == LongCtrlState.starting:
|
||||
if stopping_condition:
|
||||
long_control_state = LongCtrlState.stopping
|
||||
elif output_gb >= -brake_threshold_to_pid:
|
||||
long_control_state = LongCtrlState.pid
|
||||
|
||||
return long_control_state
|
||||
|
||||
def get_compute_gb():
|
||||
# see debug/dump_accel_from_fiber.py
|
||||
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
|
||||
[ 1.03691769, 0.78210306, -0.41343188]])
|
||||
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
|
||||
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
|
||||
[ 0.79973811, 0.13178682, 0.08550351],
|
||||
[-0.15651935, -0.44360259, 0.76910877]])
|
||||
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
|
||||
w4 = np.array([[-0.31521443],
|
||||
[-0.38626176],
|
||||
[ 0.52667892]])
|
||||
b4 = np.array([-0.02922216])
|
||||
|
||||
def compute_output(dat, w0, b0, w2, b2, w4, b4):
|
||||
m0 = np.dot(dat, w0) + b0
|
||||
m0 = leakyrelu(m0, 0.1)
|
||||
m2 = np.dot(m0, w2) + b2
|
||||
m2 = leakyrelu(m2, 0.1)
|
||||
m4 = np.dot(m2, w4) + b4
|
||||
return m4
|
||||
|
||||
def leakyrelu(x, alpha):
|
||||
return np.maximum(x, alpha * x)
|
||||
|
||||
def _compute_gb(dat):
|
||||
#linearly extrap below v1 using v1 and v2 data
|
||||
v1 = 5.
|
||||
v2 = 10.
|
||||
vx = dat[1]
|
||||
if vx > 5.:
|
||||
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
|
||||
else:
|
||||
dat[1] = v1
|
||||
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
|
||||
dat[1] = v2
|
||||
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
|
||||
m4 = (vx - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
|
||||
return m4
|
||||
return _compute_gb
|
||||
|
||||
# takes in [desired_accel, current_speed] -> [-1.0, 1.0] where -1.0 is max brake and 1.0 is max gas
|
||||
compute_gb = get_compute_gb()
|
||||
|
||||
def pid_long_control(v_ego, v_pid, Ui_accel_cmd, gas_max, brake_max, jerk_factor, gear, rate):
|
||||
#*** This function compute the gb pedal positions in order to track the desired speed
|
||||
# proportional and integral terms. More precision at low speed
|
||||
Kp_v = [1.2, 0.8, 0.5]
|
||||
Kp_bp = [0., 5., 35.]
|
||||
Kp = np.interp(v_ego, Kp_bp, Kp_v)
|
||||
Ki_v = [0.18, 0.12]
|
||||
Ki_bp = [0., 35.]
|
||||
Ki = np.interp(v_ego, Ki_bp, Ki_v)
|
||||
|
||||
# scle Kp and Ki by jerk factor drom drive_thread
|
||||
Kp = (1. + jerk_factor)*Kp
|
||||
Ki = (1. + jerk_factor)*Ki
|
||||
|
||||
# this is ugly but can speed reports 0 when speed<0.3m/s and we can't have that jump
|
||||
v_ego_min = 0.3
|
||||
v_ego = np.maximum(v_ego, v_ego_min)
|
||||
|
||||
v_error = v_pid - v_ego
|
||||
|
||||
Up_accel_cmd = v_error*Kp
|
||||
Ui_accel_cmd_new = Ui_accel_cmd + v_error*Ki*1.0/rate
|
||||
accel_cmd_new = Ui_accel_cmd_new + Up_accel_cmd
|
||||
output_gb_new = compute_gb([accel_cmd_new, v_ego])
|
||||
|
||||
# Anti-wind up for integrator: only update integrator if we not against the thottle and brake limits
|
||||
# do not wind up if we are changing gear and we are on the gas pedal
|
||||
if (((v_error >= 0. and (output_gb_new < gas_max or Ui_accel_cmd < 0)) or
|
||||
(v_error <= 0. and (output_gb_new > - brake_max or Ui_accel_cmd > 0))) and
|
||||
not (v_error >= 0. and gear == 11 and output_gb_new > 0)):
|
||||
#update integrator
|
||||
Ui_accel_cmd = Ui_accel_cmd_new
|
||||
|
||||
accel_cmd = Ui_accel_cmd + Up_accel_cmd
|
||||
|
||||
# go from accel to pedals
|
||||
output_gb = compute_gb([accel_cmd, v_ego])
|
||||
output_gb = output_gb[0]
|
||||
|
||||
# useful to know if control is against the limit
|
||||
long_control_sat = False
|
||||
if output_gb > gas_max or output_gb < -brake_max:
|
||||
long_control_sat = True
|
||||
|
||||
output_gb = np.clip(output_gb, -brake_max, gas_max)
|
||||
|
||||
return output_gb, Up_accel_cmd, Ui_accel_cmd, long_control_sat
|
||||
|
||||
|
||||
stopping_brake_rate = 0.2 # brake_travel/s while trying to stop
|
||||
starting_brake_rate = 0.6 # brake_travel/s while releasing on restart
|
||||
starting_Ui = 0.5 # Since we don't have much info about acceleration at this point, be conservative
|
||||
brake_stopping_target = 0.5 # apply at least this amount of brake to maintain the vehicle stationary
|
||||
|
||||
max_speed_error_v = [1.5, .8] # max positive v_pid error VS actual speed; this avoids controls windup due to slow pedal resp
|
||||
max_speed_error_bp = [0., 30.] # speed breakpoints
|
||||
|
||||
class LongControl(object):
|
||||
def __init__(self):
|
||||
self.long_control_state = LongCtrlState.off # initialized to off
|
||||
self.long_control_sat = False
|
||||
self.Up_accel_cmd = 0.
|
||||
self.last_output_gb = 0.
|
||||
self.reset(0.)
|
||||
|
||||
def reset(self, v_pid):
|
||||
self.Ui_accel_cmd = 0.
|
||||
self.v_pid = v_pid
|
||||
|
||||
def update(self, enabled, CS, v_cruise, v_target_lead, a_target, jerk_factor):
|
||||
# TODO: not every time
|
||||
if CS.brake_only:
|
||||
gas_max_v = [0, 0] # values
|
||||
else:
|
||||
gas_max_v = [0.6, 0.6] # values
|
||||
gas_max_bp = [0., 100.] # speeds
|
||||
brake_max_v = [1.0, 1.0, 0.8, 0.8] # values
|
||||
brake_max_bp = [0., 5., 20., 100.] # speeds
|
||||
|
||||
# brake and gas limits
|
||||
brake_max = np.interp(CS.v_ego, brake_max_bp, brake_max_v)
|
||||
gas_max = np.interp(CS.v_ego, gas_max_bp, gas_max_v)
|
||||
|
||||
overshoot_allowance = 2.0 # overshoot allowed when changing accel sign
|
||||
|
||||
output_gb = self.last_output_gb
|
||||
rate = 100
|
||||
|
||||
# limit max target speed based on cruise setting:
|
||||
v_cruise_mph = round(v_cruise * CV.KPH_TO_MPH) # what's displayed in mph on the IC
|
||||
v_target = np.minimum(v_target_lead, v_cruise_mph * CV.MPH_TO_MS / CS.ui_speed_fudge)
|
||||
|
||||
max_speed_delta_up = a_target[1]*1.0/rate
|
||||
max_speed_delta_down = a_target[0]*1.0/rate
|
||||
|
||||
# *** long control substate transitions
|
||||
self.long_control_state = long_control_state_trans(enabled, self.long_control_state, CS.v_ego, v_target, self.v_pid, output_gb)
|
||||
|
||||
# *** long control behavior based on state
|
||||
# TODO: move this to drive_helpers
|
||||
# disabled
|
||||
if self.long_control_state == LongCtrlState.off:
|
||||
self.v_pid = CS.v_ego # do nothing
|
||||
output_gb = 0.
|
||||
self.Ui_accel_cmd = 0.
|
||||
# tracking objects and driving
|
||||
elif self.long_control_state == LongCtrlState.pid:
|
||||
#reset v_pid close to v_ego if it was too far and new v_target is closer to v_ego
|
||||
if ((self.v_pid > CS.v_ego + overshoot_allowance) and
|
||||
(v_target < self.v_pid)):
|
||||
self.v_pid = np.maximum(v_target, CS.v_ego + overshoot_allowance)
|
||||
elif ((self.v_pid < CS.v_ego - overshoot_allowance) and
|
||||
(v_target > self.v_pid)):
|
||||
self.v_pid = np.minimum(v_target, CS.v_ego - overshoot_allowance)
|
||||
|
||||
# move v_pid no faster than allowed accel limits
|
||||
if (v_target > self.v_pid + max_speed_delta_up):
|
||||
self.v_pid += max_speed_delta_up
|
||||
elif (v_target < self.v_pid + max_speed_delta_down):
|
||||
self.v_pid += max_speed_delta_down
|
||||
else:
|
||||
self.v_pid = v_target
|
||||
|
||||
# to avoid too much wind up on acceleration, limit positive speed error
|
||||
if not CS.brake_only:
|
||||
max_speed_error = np.interp(CS.v_ego, max_speed_error_bp, max_speed_error_v)
|
||||
self.v_pid = np.minimum(self.v_pid, CS.v_ego + max_speed_error)
|
||||
|
||||
output_gb, self.Up_accel_cmd, self.Ui_accel_cmd, self.long_control_sat = pid_long_control(CS.v_ego, self.v_pid, \
|
||||
self.Ui_accel_cmd, gas_max, brake_max, jerk_factor, CS.gear, rate)
|
||||
# intention is to stop, switch to a different brake control until we stop
|
||||
elif self.long_control_state == LongCtrlState.stopping:
|
||||
if CS.v_ego > 0. or output_gb > -brake_stopping_target or not CS.standstill:
|
||||
output_gb -= stopping_brake_rate/rate
|
||||
output_gb = np.clip(output_gb, -brake_max, gas_max)
|
||||
self.v_pid = CS.v_ego
|
||||
self.Ui_accel_cmd = 0.
|
||||
# intention is to move again, release brake fast before handling control to PID
|
||||
elif self.long_control_state == LongCtrlState.starting:
|
||||
if output_gb < -0.2:
|
||||
output_gb += starting_brake_rate/rate
|
||||
self.v_pid = CS.v_ego
|
||||
self.Ui_accel_cmd = starting_Ui
|
||||
|
||||
self.last_output_gb = output_gb
|
||||
final_gas = np.clip(output_gb, 0., gas_max)
|
||||
final_brake = -np.clip(output_gb, -brake_max, 0.)
|
||||
return final_gas, final_brake
|
||||
@@ -0,0 +1,63 @@
|
||||
import selfdrive.messaging as messaging
|
||||
import numpy as np
|
||||
X_PATH = np.arange(0.0, 50.0)
|
||||
|
||||
def model_polyfit(points):
|
||||
return np.polyfit(X_PATH, map(float, points), 3)
|
||||
|
||||
# lane width http://safety.fhwa.dot.gov/geometric/pubs/mitigationstrategies/chapter3/3_lanewidth.cfm
|
||||
_LANE_WIDTH_V = np.asarray([3., 3.8])
|
||||
|
||||
# break points of speed
|
||||
_LANE_WIDTH_BP = np.asarray([0., 31.])
|
||||
|
||||
def calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, speed):
|
||||
#*** this function computes the poly for the center of the lane, averaging left and right polys
|
||||
lane_width = np.interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
|
||||
|
||||
# lanes in US are ~3.6m wide
|
||||
half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
|
||||
if l_prob + r_prob > 0.01:
|
||||
c_poly = ((l_poly - half_lane_poly) * l_prob +
|
||||
(r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
|
||||
c_prob = np.sqrt((l_prob**2 + r_prob**2) / 2.)
|
||||
else:
|
||||
c_poly = np.zeros(4)
|
||||
c_prob = 0.
|
||||
|
||||
p_weight = 1. # predicted path weight relatively to the center of the lane
|
||||
d_poly = list((c_poly*c_prob + p_poly*p_prob*p_weight ) / (c_prob + p_prob*p_weight))
|
||||
return d_poly, c_poly, c_prob
|
||||
|
||||
class PathPlanner(object):
|
||||
def __init__(self, model):
|
||||
self.model = model
|
||||
self.dead = True
|
||||
self.d_poly = [0., 0., 0., 0.]
|
||||
self.last_model = 0.
|
||||
self.logMonoTime = 0
|
||||
self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
|
||||
|
||||
def update(self, cur_time, v_ego):
|
||||
md = messaging.recv_sock(self.model)
|
||||
|
||||
if md is not None:
|
||||
self.logMonoTime = md.logMonoTime
|
||||
p_poly = model_polyfit(md.model.path.points) # predicted path
|
||||
p_prob = 1. # model does not tell this probability yet, so set to 1 for now
|
||||
l_poly = model_polyfit(md.model.leftLane.points) # left line
|
||||
l_prob = md.model.leftLane.prob # left line prob
|
||||
r_poly = model_polyfit(md.model.rightLane.points) # right line
|
||||
r_prob = md.model.rightLane.prob # right line prob
|
||||
|
||||
self.lead_dist = md.model.lead.dist
|
||||
self.lead_prob = md.model.lead.prob
|
||||
self.lead_var = md.model.lead.std**2
|
||||
|
||||
#*** compute target path ***
|
||||
self.d_poly, _, _ = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego)
|
||||
|
||||
self.last_model = cur_time
|
||||
self.dead = False
|
||||
elif cur_time - self.last_model > 0.5:
|
||||
self.dead = True
|
||||
@@ -0,0 +1,256 @@
|
||||
import numpy as np
|
||||
import platform
|
||||
import os
|
||||
import sys
|
||||
|
||||
from common.kalman.ekf import FastEKF1D, SimpleSensor
|
||||
|
||||
# radar tracks
|
||||
SPEED, ACCEL = 0, 1 # Kalman filter states enum
|
||||
|
||||
rate, ratev = 20., 20. # model and radar are both at 20Hz
|
||||
ts = 1./rate
|
||||
freq_v_lat = 0.2 # Hz
|
||||
k_v_lat = 2*np.pi*freq_v_lat*ts / (1 + 2*np.pi*freq_v_lat*ts)
|
||||
|
||||
freq_a_lead = .5 # Hz
|
||||
k_a_lead = 2*np.pi*freq_a_lead*ts / (1 + 2*np.pi*freq_a_lead*ts)
|
||||
|
||||
# stationary qualification parameters
|
||||
v_stationary_thr = 4. # objects moving below this speed are classified as stationary
|
||||
v_oncoming_thr = -3.9 # needs to be a bit lower in abs value than v_stationary_thr to not leave "holes"
|
||||
v_ego_stationary = 4. # no stationary object flag below this speed
|
||||
|
||||
class Track(object):
|
||||
def __init__(self):
|
||||
self.ekf = None
|
||||
self.stationary = True
|
||||
self.initted = False
|
||||
|
||||
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned):
|
||||
if self.initted:
|
||||
self.dPathPrev = self.dPath
|
||||
self.vLeadPrev = self.vLead
|
||||
self.vRelPrev = self.vRel
|
||||
|
||||
# relative values, copy
|
||||
self.dRel = d_rel # LONG_DIST
|
||||
self.yRel = y_rel # -LAT_DIST
|
||||
self.vRel = v_rel # REL_SPEED
|
||||
|
||||
# compute distance to path
|
||||
self.dPath = d_path
|
||||
|
||||
# computed velocity and accelerations
|
||||
self.vLead = self.vRel + v_ego_t_aligned
|
||||
|
||||
if not self.initted:
|
||||
self.aRel = 0. # nidec gives no information about this
|
||||
self.vLat = 0.
|
||||
self.aLead = 0.
|
||||
else:
|
||||
# estimate acceleration
|
||||
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
|
||||
a_rel_unfilt = np.clip(a_rel_unfilt, -10., 10.)
|
||||
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
|
||||
|
||||
v_lat_unfilt = (self.dPath - self.dPathPrev) / ts
|
||||
self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
|
||||
|
||||
a_lead_unfilt = (self.vLead - self.vLeadPrev) / ts
|
||||
a_lead_unfilt = np.clip(a_lead_unfilt, -10., 10.)
|
||||
self.aLead = k_a_lead * a_lead_unfilt + (1 - k_a_lead) * self.aLead
|
||||
|
||||
if self.stationary:
|
||||
# stationary objects can become non stationary, but not the other way around
|
||||
self.stationary = v_ego_t_aligned > v_ego_stationary and abs(self.vLead) < v_stationary_thr
|
||||
self.oncoming = self.vLead < v_oncoming_thr
|
||||
|
||||
if self.ekf is None:
|
||||
self.ekf = FastEKF1D(ts, 1e3, [0.1, 1])
|
||||
self.ekf.state[SPEED] = self.vLead
|
||||
self.ekf.state[ACCEL] = 0
|
||||
self.lead_sensor = SimpleSensor(SPEED, 1, 2)
|
||||
|
||||
self.vLeadK = self.vLead
|
||||
self.aLeadK = self.aLead
|
||||
else:
|
||||
self.ekf.update_scalar(self.lead_sensor.read(self.vLead))
|
||||
self.ekf.predict(ts)
|
||||
self.vLeadK = float(self.ekf.state[SPEED])
|
||||
self.aLeadK = float(self.ekf.state[ACCEL])
|
||||
|
||||
if not self.initted:
|
||||
self.cnt = 1
|
||||
self.vision_cnt = 0
|
||||
else:
|
||||
self.cnt += 1
|
||||
|
||||
self.initted = True
|
||||
self.vision = False
|
||||
|
||||
def mix_vision(self, dist_to_vision, rel_speed_diff):
|
||||
# rel speed is very hard to estimate from vision
|
||||
if dist_to_vision < 4.0 and rel_speed_diff < 10.:
|
||||
# vision point is never stationary
|
||||
self.stationary = False
|
||||
self.vision = True
|
||||
self.vision_cnt += 1
|
||||
|
||||
def get_key_for_cluster(self):
|
||||
# Weigh y higher since radar is inaccurate in this dimension
|
||||
return [self.dRel, self.dPath*2, self.vRel]
|
||||
|
||||
# ******************* Cluster *******************
|
||||
|
||||
if platform.machine() == 'aarch64':
|
||||
for x in sys.path:
|
||||
pp = os.path.join(x, "phonelibs/hierarchy/lib")
|
||||
if os.path.isfile(os.path.join(pp, "_hierarchy.so")):
|
||||
sys.path.append(pp)
|
||||
break
|
||||
import _hierarchy
|
||||
else:
|
||||
from scipy.cluster import _hierarchy
|
||||
|
||||
def fcluster(Z, t, criterion='inconsistent', depth=2, R=None, monocrit=None):
|
||||
# supersimplified function to get fast clustering. Got it from scipy
|
||||
Z = np.asarray(Z, order='c')
|
||||
n = Z.shape[0] + 1
|
||||
T = np.zeros((n,), dtype='i')
|
||||
_hierarchy.cluster_dist(Z, T, float(t), int(n))
|
||||
return T
|
||||
|
||||
RDR_TO_LDR = 2.7
|
||||
|
||||
def mean(l):
|
||||
return sum(l)/len(l)
|
||||
|
||||
class Cluster(object):
|
||||
def __init__(self):
|
||||
self.tracks = set()
|
||||
|
||||
def add(self, t):
|
||||
# add the first track
|
||||
self.tracks.add(t)
|
||||
|
||||
# TODO: make generic
|
||||
@property
|
||||
def dRel(self):
|
||||
return mean([t.dRel for t in self.tracks])
|
||||
|
||||
@property
|
||||
def yRel(self):
|
||||
return mean([t.yRel for t in self.tracks])
|
||||
|
||||
@property
|
||||
def vRel(self):
|
||||
return mean([t.vRel for t in self.tracks])
|
||||
|
||||
@property
|
||||
def aRel(self):
|
||||
return mean([t.aRel for t in self.tracks])
|
||||
|
||||
@property
|
||||
def vLead(self):
|
||||
return mean([t.vLead for t in self.tracks])
|
||||
|
||||
@property
|
||||
def aLead(self):
|
||||
return mean([t.aLead for t in self.tracks])
|
||||
|
||||
@property
|
||||
def dPath(self):
|
||||
return mean([t.dPath for t in self.tracks])
|
||||
|
||||
@property
|
||||
def vLat(self):
|
||||
return mean([t.vLat for t in self.tracks])
|
||||
|
||||
@property
|
||||
def vLeadK(self):
|
||||
return mean([t.vLeadK for t in self.tracks])
|
||||
|
||||
@property
|
||||
def aLeadK(self):
|
||||
return mean([t.aLeadK for t in self.tracks])
|
||||
|
||||
@property
|
||||
def vision(self):
|
||||
return any([t.vision for t in self.tracks])
|
||||
|
||||
@property
|
||||
def vision_cnt(self):
|
||||
return max([t.vision_cnt for t in self.tracks])
|
||||
|
||||
@property
|
||||
def stationary(self):
|
||||
return all([t.stationary for t in self.tracks])
|
||||
|
||||
@property
|
||||
def oncoming(self):
|
||||
return all([t.oncoming for t in self.tracks])
|
||||
|
||||
def toLive20(self, lead):
|
||||
lead.dRel = float(self.dRel) - RDR_TO_LDR
|
||||
lead.yRel = float(self.yRel)
|
||||
lead.vRel = float(self.vRel)
|
||||
lead.aRel = float(self.aRel)
|
||||
lead.vLead = float(self.vLead)
|
||||
lead.aLead = float(self.aLead)
|
||||
lead.dPath = float(self.dPath)
|
||||
lead.vLat = float(self.vLat)
|
||||
lead.vLeadK = float(self.vLeadK)
|
||||
lead.aLeadK = float(self.aLeadK)
|
||||
lead.status = True
|
||||
lead.fcw = False
|
||||
|
||||
def __str__(self):
|
||||
ret = "x: %7.2f y: %7.2f v: %7.2f a: %7.2f" % (self.dRel, self.yRel, self.vRel, self.aRel)
|
||||
if self.stationary:
|
||||
ret += " stationary"
|
||||
if self.vision:
|
||||
ret += " vision"
|
||||
if self.oncoming:
|
||||
ret += " oncoming"
|
||||
if self.vision_cnt > 0:
|
||||
ret += " vision_cnt: %6.0f" % self.vision_cnt
|
||||
return ret
|
||||
|
||||
def is_potential_lead(self, v_ego, enabled):
|
||||
# predict cut-ins by extrapolating lateral speed by a lookahead time
|
||||
# lookahead time depends on cut-in distance. more attentive for close cut-ins
|
||||
# also, above 50 meters the predicted path isn't very reliable
|
||||
|
||||
# the distance at which v_lat matters is higher at higher speed
|
||||
lookahead_dist = 40. + v_ego/1.2 #40m at 0mph, ~70m at 80mph
|
||||
|
||||
t_lookahead_v = [1., 0.]
|
||||
t_lookahead_bp = [10., lookahead_dist]
|
||||
|
||||
# average dist
|
||||
d_path = self.dPath
|
||||
|
||||
if enabled:
|
||||
t_lookahead = np.interp(self.dRel, t_lookahead_bp, t_lookahead_v)
|
||||
# correct d_path for lookahead time, considering only cut-ins and no more than 1m impact
|
||||
lat_corr = np.clip(t_lookahead * self.vLat, -1, 0)
|
||||
else:
|
||||
lat_corr = 0.
|
||||
d_path = np.maximum(d_path + lat_corr, 0)
|
||||
|
||||
if d_path < 1.5 and not self.stationary and not self.oncoming:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
|
||||
def is_potential_lead2(self, lead_clusters):
|
||||
if len(lead_clusters) > 0:
|
||||
lead_cluster = lead_clusters[0]
|
||||
# check if the new lead is too close and roughly at the same speed of the first lead: it might just be the second axle of the same vehicle
|
||||
if (self.dRel - lead_cluster.dRel) < 8. and abs(self.vRel - lead_cluster.vRel) < 1.:
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
Executable
+268
@@ -0,0 +1,268 @@
|
||||
#!/usr/bin/env python
|
||||
import zmq
|
||||
import numpy as np
|
||||
import numpy.matlib
|
||||
from collections import defaultdict
|
||||
|
||||
from fastcluster import linkage_vector
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.boardd.boardd import can_capnp_to_can_list_old
|
||||
from selfdrive.controls.lib.latcontrol import calc_lookahead_offset
|
||||
from selfdrive.controls.lib.can_parser import CANParser
|
||||
from selfdrive.controls.lib.pathplanner import PathPlanner
|
||||
from selfdrive.config import VehicleParams
|
||||
from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, RDR_TO_LDR
|
||||
|
||||
from common.services import service_list
|
||||
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
|
||||
from common.kalman.ekf import EKF, SimpleSensor
|
||||
|
||||
#vision point
|
||||
DIMSV = 2
|
||||
XV, SPEEDV = 0, 1
|
||||
VISION_POINT = 1
|
||||
|
||||
class EKFV1D(EKF):
|
||||
def __init__(self):
|
||||
super(EKFV1D, self).__init__(False)
|
||||
self.identity = np.matlib.identity(DIMSV)
|
||||
self.state = np.matlib.zeros((DIMSV, 1))
|
||||
self.var_init = 1e2 # ~ model variance when probability is 70%, so good starting point
|
||||
self.covar = self.identity * self.var_init
|
||||
|
||||
# self.process_noise = np.asmatrix(np.diag([100, 10]))
|
||||
self.process_noise = np.matlib.diag([0.5, 1])
|
||||
|
||||
def calc_transfer_fun(self, dt):
|
||||
tf = np.matlib.identity(DIMSV)
|
||||
tf[XV, SPEEDV] = dt
|
||||
tfj = tf
|
||||
return tf, tfj
|
||||
|
||||
|
||||
# nidec radar decoding
|
||||
def nidec_decode(cp, ar_pts):
|
||||
for ii in cp.msgs_upd:
|
||||
# filter points with very big distance, as fff (~255) is invalid. FIXME: use VAL tables from dbc
|
||||
if cp.vl[ii]['LONG_DIST'] < 255:
|
||||
ar_pts[ii] = [cp.vl[ii]['LONG_DIST'] + RDR_TO_LDR,
|
||||
-cp.vl[ii]['LAT_DIST'], cp.vl[ii]['REL_SPEED'], np.nan,
|
||||
cp.ts[ii], cp.vl[ii]['NEW_TRACK'], cp.ct[ii]]
|
||||
elif ii in ar_pts:
|
||||
del ar_pts[ii]
|
||||
return ar_pts
|
||||
|
||||
|
||||
def _create_radard_can_parser():
|
||||
dbc_f = 'acura_ilx_2016_nidec.dbc'
|
||||
radar_messages = range(0x430, 0x43A) + range(0x440, 0x446)
|
||||
signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
|
||||
['REL_SPEED'] * 16, radar_messages * 4,
|
||||
[255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
|
||||
checks = zip(radar_messages, [20]*16)
|
||||
|
||||
return CANParser(dbc_f, signals, checks)
|
||||
|
||||
|
||||
# fuses camera and radar data for best lead detection
|
||||
def radard_thread(gctx=None):
|
||||
set_realtime_priority(1)
|
||||
|
||||
context = zmq.Context()
|
||||
|
||||
# *** subscribe to features and model from visiond
|
||||
model = messaging.sub_sock(context, service_list['model'].port)
|
||||
logcan = messaging.sub_sock(context, service_list['can'].port)
|
||||
live100 = messaging.sub_sock(context, service_list['live100'].port)
|
||||
|
||||
PP = PathPlanner(model)
|
||||
|
||||
# *** publish live20 and liveTracks
|
||||
live20 = messaging.pub_sock(context, service_list['live20'].port)
|
||||
liveTracks = messaging.pub_sock(context, service_list['liveTracks'].port)
|
||||
|
||||
# subscribe to stats about the car
|
||||
# TODO: move this to new style packet
|
||||
VP = VehicleParams(False) # same for ILX and civic
|
||||
|
||||
ar_pts = {}
|
||||
path_x = np.arange(0.0, 140.0, 0.1) # 140 meters is max
|
||||
|
||||
# Time-alignment
|
||||
rate = 20. # model and radar are both at 20Hz
|
||||
tsv = 1./rate
|
||||
rdr_delay = 0.10 # radar data delay in s
|
||||
v_len = 20 # how many speed data points to remember for t alignment with rdr data
|
||||
|
||||
enabled = 0
|
||||
steer_angle = 0.
|
||||
|
||||
tracks = defaultdict(dict)
|
||||
|
||||
# Nidec
|
||||
cp = _create_radard_can_parser()
|
||||
|
||||
# Kalman filter stuff:
|
||||
ekfv = EKFV1D()
|
||||
speedSensorV = SimpleSensor(XV, 1, 2)
|
||||
|
||||
# v_ego
|
||||
v_ego = None
|
||||
v_ego_array = np.zeros([2, v_len])
|
||||
v_ego_t_aligned = 0.
|
||||
|
||||
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
|
||||
while 1:
|
||||
canMonoTimes = []
|
||||
can_pub_radar = []
|
||||
for a in messaging.drain_sock(logcan, wait_for_one=True):
|
||||
canMonoTimes.append(a.logMonoTime)
|
||||
can_pub_radar.extend(can_capnp_to_can_list_old(a, [1, 3]))
|
||||
|
||||
# only run on the 0x445 packets, used for timing
|
||||
if not any(x[0] == 0x445 for x in can_pub_radar):
|
||||
continue
|
||||
|
||||
cp.update_can(can_pub_radar)
|
||||
|
||||
if not cp.can_valid:
|
||||
# TODO: handle this
|
||||
pass
|
||||
|
||||
ar_pts = nidec_decode(cp, ar_pts)
|
||||
|
||||
# receive the live100s
|
||||
l100 = messaging.recv_sock(live100)
|
||||
if l100 is not None:
|
||||
enabled = l100.live100.enabled
|
||||
v_ego = l100.live100.vEgo
|
||||
steer_angle = l100.live100.angleSteers
|
||||
|
||||
v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
|
||||
v_ego_array = v_ego_array[:, 1:]
|
||||
|
||||
if v_ego is None:
|
||||
continue
|
||||
|
||||
# *** get path prediction from the model ***
|
||||
PP.update(sec_since_boot(), v_ego)
|
||||
|
||||
# run kalman filter only if prob is high enough
|
||||
if PP.lead_prob > 0.7:
|
||||
ekfv.update(speedSensorV.read(PP.lead_dist, covar=PP.lead_var))
|
||||
ekfv.predict(tsv)
|
||||
ar_pts[VISION_POINT] = (float(ekfv.state[XV]), np.polyval(PP.d_poly, float(ekfv.state[XV])),
|
||||
float(ekfv.state[SPEEDV]), np.nan, PP.logMonoTime, np.nan, sec_since_boot())
|
||||
else:
|
||||
ekfv.state[XV] = PP.lead_dist
|
||||
ekfv.covar = (np.diag([PP.lead_var, ekfv.var_init]))
|
||||
ekfv.state[SPEEDV] = 0.
|
||||
if VISION_POINT in ar_pts:
|
||||
del ar_pts[VISION_POINT]
|
||||
|
||||
# *** compute the likely path_y ***
|
||||
if enabled: # use path from model path_poly
|
||||
path_y = np.polyval(PP.d_poly, path_x)
|
||||
else: # use path from steer, set angle_offset to 0 since calibration does not exactly report the physical offset
|
||||
path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VP, angle_offset=0)[0]
|
||||
|
||||
# *** remove missing points from meta data ***
|
||||
for ids in tracks.keys():
|
||||
if ids not in ar_pts:
|
||||
tracks.pop(ids, None)
|
||||
|
||||
# *** compute the tracks ***
|
||||
for ids in ar_pts:
|
||||
# ignore the vision point for now
|
||||
if ids == VISION_POINT:
|
||||
continue
|
||||
rpt = ar_pts[ids]
|
||||
|
||||
# align v_ego by a fixed time to align it with the radar measurement
|
||||
cur_time = float(rk.frame)/rate
|
||||
v_ego_t_aligned = np.interp(cur_time - rdr_delay, v_ego_array[1], v_ego_array[0])
|
||||
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
|
||||
|
||||
# create the track
|
||||
if ids not in tracks or rpt[5] == 1:
|
||||
tracks[ids] = Track()
|
||||
tracks[ids].update(rpt[0], rpt[1], rpt[2], d_path, v_ego_t_aligned)
|
||||
|
||||
# allow the vision model to remove the stationary flag if distance and rel speed roughly match
|
||||
if VISION_POINT in ar_pts:
|
||||
dist_to_vision = np.sqrt((0.5*(ar_pts[VISION_POINT][0] - rpt[0])) ** 2 + (2*(ar_pts[VISION_POINT][1] - rpt[1])) ** 2)
|
||||
rel_speed_diff = abs(ar_pts[VISION_POINT][2] - rpt[2])
|
||||
tracks[ids].mix_vision(dist_to_vision, rel_speed_diff)
|
||||
|
||||
# publish tracks (debugging)
|
||||
dat = messaging.new_message()
|
||||
dat.init('liveTracks', len(tracks))
|
||||
for cnt, ids in enumerate(tracks.keys()):
|
||||
dat.liveTracks[cnt].trackId = ids
|
||||
dat.liveTracks[cnt].dRel = float(tracks[ids].dRel)
|
||||
dat.liveTracks[cnt].yRel = float(tracks[ids].yRel)
|
||||
dat.liveTracks[cnt].vRel = float(tracks[ids].vRel)
|
||||
dat.liveTracks[cnt].aRel = float(tracks[ids].aRel)
|
||||
dat.liveTracks[cnt].stationary = tracks[ids].stationary
|
||||
dat.liveTracks[cnt].oncoming = tracks[ids].oncoming
|
||||
liveTracks.send(dat.to_bytes())
|
||||
|
||||
idens = tracks.keys()
|
||||
track_pts = np.array([tracks[iden].get_key_for_cluster() for iden in idens])
|
||||
|
||||
# If we have multiple points, cluster them
|
||||
if len(track_pts) > 1:
|
||||
link = linkage_vector(track_pts, method='centroid')
|
||||
cluster_idxs = fcluster(link, 2.5, criterion='distance')
|
||||
clusters = [None]*max(cluster_idxs)
|
||||
|
||||
for idx in xrange(len(track_pts)):
|
||||
cluster_i = cluster_idxs[idx]-1
|
||||
|
||||
if clusters[cluster_i] == None:
|
||||
clusters[cluster_i] = Cluster()
|
||||
clusters[cluster_i].add(tracks[idens[idx]])
|
||||
elif len(track_pts) == 1:
|
||||
# TODO: why do we need this?
|
||||
clusters = [Cluster()]
|
||||
clusters[0].add(tracks[idens[0]])
|
||||
else:
|
||||
clusters = []
|
||||
|
||||
# *** extract the lead car ***
|
||||
lead_clusters = [c for c in clusters
|
||||
if c.is_potential_lead(v_ego, enabled)]
|
||||
lead_clusters.sort(key=lambda x: x.dRel)
|
||||
lead_len = len(lead_clusters)
|
||||
|
||||
# *** extract the second lead from the whole set of leads ***
|
||||
lead2_clusters = [c for c in lead_clusters
|
||||
if c.is_potential_lead2(lead_clusters)]
|
||||
lead2_clusters.sort(key=lambda x: x.dRel)
|
||||
lead2_len = len(lead2_clusters)
|
||||
|
||||
# *** publish live20 ***
|
||||
dat = messaging.new_message()
|
||||
dat.init('live20')
|
||||
dat.live20.mdMonoTime = PP.logMonoTime
|
||||
dat.live20.canMonoTimes = canMonoTimes
|
||||
if lead_len > 0:
|
||||
lead_clusters[0].toLive20(dat.live20.leadOne)
|
||||
if lead2_len > 0:
|
||||
lead2_clusters[0].toLive20(dat.live20.leadTwo)
|
||||
else:
|
||||
dat.live20.leadTwo.status = False
|
||||
else:
|
||||
dat.live20.leadOne.status = False
|
||||
|
||||
dat.live20.cumLagMs = -rk.remaining*1000.
|
||||
live20.send(dat.to_bytes())
|
||||
|
||||
rk.monitor_time()
|
||||
|
||||
def main(gctx=None):
|
||||
radard_thread(gctx)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user