|
|
|
@ -6,7 +6,7 @@ from cereal import car, log |
|
|
|
|
from common.numpy_fast import clip |
|
|
|
|
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL |
|
|
|
|
from common.profiler import Profiler |
|
|
|
|
from common.params import Params, put_nonblocking |
|
|
|
|
from common.params import Params |
|
|
|
|
import cereal.messaging as messaging |
|
|
|
|
from selfdrive.config import Conversions as CV |
|
|
|
|
from selfdrive.boardd.boardd import can_list_to_can_capnp |
|
|
|
@ -23,9 +23,7 @@ from selfdrive.controls.lib.latcontrol_indi import LatControlINDI |
|
|
|
|
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR |
|
|
|
|
from selfdrive.controls.lib.alertmanager import AlertManager |
|
|
|
|
from selfdrive.controls.lib.vehicle_model import VehicleModel |
|
|
|
|
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION |
|
|
|
|
from selfdrive.controls.lib.planner import LON_MPC_STEP |
|
|
|
|
from selfdrive.controls.lib.gps_helpers import is_rhd_region |
|
|
|
|
from selfdrive.locationd.calibration_helpers import Calibration, Filter |
|
|
|
|
|
|
|
|
|
LANE_DEPARTURE_THRESHOLD = 0.1 |
|
|
|
@ -67,7 +65,7 @@ def events_to_bytes(events): |
|
|
|
|
return ret |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params): |
|
|
|
|
def data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params): |
|
|
|
|
"""Receive data from sockets and create events for battery, temperature and disk space""" |
|
|
|
|
|
|
|
|
|
# Update carstate from CAN and create events |
|
|
|
@ -77,6 +75,7 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, ca |
|
|
|
|
sm.update(0) |
|
|
|
|
|
|
|
|
|
events = list(CS.events) |
|
|
|
|
events += list(sm['dMonitoringState'].events) |
|
|
|
|
add_lane_change_event(events, sm['pathPlan']) |
|
|
|
|
enabled = isEnabled(state) |
|
|
|
|
|
|
|
|
@ -103,27 +102,15 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, ca |
|
|
|
|
if CS.stockAeb: |
|
|
|
|
events.append(create_event('stockAeb', [])) |
|
|
|
|
|
|
|
|
|
# GPS coords RHD parsing, once every restart |
|
|
|
|
if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked: |
|
|
|
|
is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude) |
|
|
|
|
driver_status.is_rhd_region = is_rhd |
|
|
|
|
driver_status.is_rhd_region_checked = True |
|
|
|
|
put_nonblocking("IsRHD", "1" if is_rhd else "0") |
|
|
|
|
|
|
|
|
|
# Handle calibration |
|
|
|
|
cal_status = sm['liveCalibration'].calStatus |
|
|
|
|
cal_perc = sm['liveCalibration'].calPerc |
|
|
|
|
|
|
|
|
|
cal_rpy = [0,0,0] |
|
|
|
|
if cal_status != Calibration.CALIBRATED: |
|
|
|
|
if cal_status == Calibration.UNCALIBRATED: |
|
|
|
|
events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) |
|
|
|
|
else: |
|
|
|
|
events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) |
|
|
|
|
else: |
|
|
|
|
rpy = sm['liveCalibration'].rpyCalib |
|
|
|
|
if len(rpy) == 3: |
|
|
|
|
cal_rpy = rpy |
|
|
|
|
|
|
|
|
|
# When the panda and controlsd do not agree on controls_allowed |
|
|
|
|
# we want to disengage openpilot. However the status from the panda goes through |
|
|
|
@ -138,16 +125,6 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, ca |
|
|
|
|
if mismatch_counter >= 200: |
|
|
|
|
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) |
|
|
|
|
|
|
|
|
|
# Driver monitoring |
|
|
|
|
if sm.updated['model']: |
|
|
|
|
driver_status.set_policy(sm['model']) |
|
|
|
|
|
|
|
|
|
if sm.updated['driverMonitoring']: |
|
|
|
|
driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled) |
|
|
|
|
|
|
|
|
|
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: |
|
|
|
|
events.append(create_event("tooDistracted", [ET.NO_ENTRY])) |
|
|
|
|
|
|
|
|
|
return CS, events, cal_perc, mismatch_counter, can_error_counter |
|
|
|
|
|
|
|
|
|
|
|
|
|
@ -239,7 +216,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, |
|
|
|
|
AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): |
|
|
|
|
AM, rk, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): |
|
|
|
|
"""Given the state, this function returns an actuators packet""" |
|
|
|
|
|
|
|
|
|
actuators = car.CarControl.Actuators.new_message() |
|
|
|
@ -247,17 +224,9 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr |
|
|
|
|
enabled = isEnabled(state) |
|
|
|
|
active = isActive(state) |
|
|
|
|
|
|
|
|
|
# check if user has interacted with the car |
|
|
|
|
driver_engaged = len(CS.buttonEvents) > 0 or \ |
|
|
|
|
v_cruise_kph != v_cruise_kph_last or \ |
|
|
|
|
CS.steeringPressed |
|
|
|
|
|
|
|
|
|
if CS.leftBlinker or CS.rightBlinker: |
|
|
|
|
last_blinker_frame = frame |
|
|
|
|
|
|
|
|
|
# add eventual driver distracted events |
|
|
|
|
events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) |
|
|
|
|
|
|
|
|
|
if plan.fcw: |
|
|
|
|
# send FCW alert if triggered by planner |
|
|
|
|
AM.add(frame, "fcw", enabled) |
|
|
|
@ -315,11 +284,11 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr |
|
|
|
|
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" |
|
|
|
|
AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) |
|
|
|
|
|
|
|
|
|
return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame |
|
|
|
|
return actuators, v_cruise_kph, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, |
|
|
|
|
driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, |
|
|
|
|
LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, |
|
|
|
|
last_blinker_frame, is_ldw_enabled, can_error_counter): |
|
|
|
|
"""Send actuators and hud commands to the car, send controlsstate and MPC logging""" |
|
|
|
|
|
|
|
|
@ -373,7 +342,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk |
|
|
|
|
can_sends = CI.apply(CC) |
|
|
|
|
pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) |
|
|
|
|
|
|
|
|
|
force_decel = driver_status.awareness < 0. |
|
|
|
|
force_decel = sm['dMonitoringState'].awarenessStatus < 0. |
|
|
|
|
|
|
|
|
|
# controlsState |
|
|
|
|
dat = messaging.new_message() |
|
|
|
@ -387,8 +356,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk |
|
|
|
|
"alertBlinkingRate": AM.alert_rate, |
|
|
|
|
"alertType": AM.alert_type, |
|
|
|
|
"alertSound": AM.audible_alert, |
|
|
|
|
"awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0, |
|
|
|
|
"driverMonitoringOn": bool(driver_status.face_detected), |
|
|
|
|
"driverMonitoringOn": sm['dMonitoringState'].faceDetected, |
|
|
|
|
"canMonoTimes": list(CS.canMonoTimes), |
|
|
|
|
"planMonoTime": sm.logMonoTime['plan'], |
|
|
|
|
"pathPlanMonoTime": sm.logMonoTime['pathPlan'], |
|
|
|
@ -483,8 +451,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): |
|
|
|
|
pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', 'carControl', 'carEvents', 'carParams']) |
|
|
|
|
|
|
|
|
|
if sm is None: |
|
|
|
|
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ |
|
|
|
|
'model', 'gpsLocation'], ignore_alive=['gpsLocation']) |
|
|
|
|
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'dMonitoringState', 'plan', 'pathPlan', \ |
|
|
|
|
'model']) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if can_sock is None: |
|
|
|
@ -529,11 +497,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): |
|
|
|
|
elif CP.lateralTuning.which() == 'lqr': |
|
|
|
|
LaC = LatControlLQR(CP) |
|
|
|
|
|
|
|
|
|
driver_status = DriverStatus() |
|
|
|
|
is_rhd = params.get("IsRHD") |
|
|
|
|
if is_rhd is not None: |
|
|
|
|
driver_status.is_rhd = bool(int(is_rhd)) |
|
|
|
|
|
|
|
|
|
state = State.disabled |
|
|
|
|
soft_disable_timer = 0 |
|
|
|
|
v_cruise_kph = 255 |
|
|
|
@ -547,6 +510,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): |
|
|
|
|
sm['pathPlan'].sensorValid = True |
|
|
|
|
sm['pathPlan'].posenetValid = True |
|
|
|
|
sm['thermal'].freeSpace = 1. |
|
|
|
|
sm['dMonitoringState'].events = [] |
|
|
|
|
sm['dMonitoringState'].awarenessStatus = 1. |
|
|
|
|
sm['dMonitoringState'].faceDetected = False |
|
|
|
|
|
|
|
|
|
# detect sound card presence |
|
|
|
|
sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') |
|
|
|
@ -563,7 +529,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): |
|
|
|
|
prof.checkpoint("Ratekeeper", ignore=True) |
|
|
|
|
|
|
|
|
|
# Sample data and compute car events |
|
|
|
|
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params) |
|
|
|
|
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, state, mismatch_counter, can_error_counter, params) |
|
|
|
|
prof.checkpoint("Sample") |
|
|
|
|
|
|
|
|
|
# Create alerts |
|
|
|
@ -605,14 +571,14 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): |
|
|
|
|
prof.checkpoint("State transition") |
|
|
|
|
|
|
|
|
|
# Compute actuators (runs PID loops and lateral MPC) |
|
|
|
|
actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ |
|
|
|
|
actuators, v_cruise_kph, v_acc, a_acc, lac_log, last_blinker_frame = \ |
|
|
|
|
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, |
|
|
|
|
driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) |
|
|
|
|
LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) |
|
|
|
|
|
|
|
|
|
prof.checkpoint("State Control") |
|
|
|
|
|
|
|
|
|
# Publish data |
|
|
|
|
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, |
|
|
|
|
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, LaC, |
|
|
|
|
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, |
|
|
|
|
is_ldw_enabled, can_error_counter) |
|
|
|
|
prof.checkpoint("Sent") |
|
|
|
|