RHD support for driver monitoring (#1299)
parent
a18832748c
commit
133b1a20b4
BIN
apk/ai.comma.plus.offroad.apk (Stored with Git LFS)
BIN
apk/ai.comma.plus.offroad.apk (Stored with Git LFS)
Binary file not shown.
2
apks
2
apks
|
@ -1 +1 @@
|
||||||
Subproject commit e4bf49bc65e8e680e5afc629240b4ebbcedc4ebd
|
Subproject commit f5d2c1715c9482d898062110ce4c612093aa5d4f
|
2
cereal
2
cereal
|
@ -1 +1 @@
|
||||||
Subproject commit c8e5db830d30e5ea0d0abef0479d9c48cfd24504
|
Subproject commit 442e914d73b163823fceb91c2b57062965fad045
|
|
@ -70,6 +70,7 @@ keys = {
|
||||||
"GithubSshKeys": [TxType.PERSISTENT],
|
"GithubSshKeys": [TxType.PERSISTENT],
|
||||||
"HasAcceptedTerms": [TxType.PERSISTENT],
|
"HasAcceptedTerms": [TxType.PERSISTENT],
|
||||||
"HasCompletedSetup": [TxType.PERSISTENT],
|
"HasCompletedSetup": [TxType.PERSISTENT],
|
||||||
|
"IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START],
|
||||||
"IsLdwEnabled": [TxType.PERSISTENT],
|
"IsLdwEnabled": [TxType.PERSISTENT],
|
||||||
"IsGeofenceEnabled": [TxType.PERSISTENT],
|
"IsGeofenceEnabled": [TxType.PERSISTENT],
|
||||||
"IsMetric": [TxType.PERSISTENT],
|
"IsMetric": [TxType.PERSISTENT],
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
2ad118ee-df4f-49ff-a374-6dd693cee37b
|
43221d85-46fd-40b9-bff0-2b1b18a86b07
|
BIN
models/dmonitoring_model.keras (Stored with Git LFS)
BIN
models/dmonitoring_model.keras (Stored with Git LFS)
Binary file not shown.
BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
Binary file not shown.
|
@ -217,6 +217,7 @@ selfdrive/controls/lib/fcw.py
|
||||||
selfdrive/controls/lib/long_mpc.py
|
selfdrive/controls/lib/long_mpc.py
|
||||||
selfdrive/controls/lib/long_mpc_model.py
|
selfdrive/controls/lib/long_mpc_model.py
|
||||||
selfdrive/controls/lib/gps_helpers.py
|
selfdrive/controls/lib/gps_helpers.py
|
||||||
|
selfdrive/controls/lib/driverview.py
|
||||||
|
|
||||||
selfdrive/controls/lib/cluster/*
|
selfdrive/controls/lib/cluster/*
|
||||||
|
|
||||||
|
|
|
@ -114,6 +114,8 @@ struct VisionState {
|
||||||
int rgb_front_width, rgb_front_height, rgb_front_stride;
|
int rgb_front_width, rgb_front_height, rgb_front_stride;
|
||||||
VisionBuf rgb_front_bufs[UI_BUF_COUNT];
|
VisionBuf rgb_front_bufs[UI_BUF_COUNT];
|
||||||
cl_mem rgb_front_bufs_cl[UI_BUF_COUNT];
|
cl_mem rgb_front_bufs_cl[UI_BUF_COUNT];
|
||||||
|
bool rhd_front;
|
||||||
|
bool rhd_front_checked;
|
||||||
int front_meteringbox_xmin, front_meteringbox_xmax;
|
int front_meteringbox_xmin, front_meteringbox_xmax;
|
||||||
int front_meteringbox_ymin, front_meteringbox_ymax;
|
int front_meteringbox_ymin, front_meteringbox_ymax;
|
||||||
|
|
||||||
|
@ -152,7 +154,9 @@ void* frontview_thread(void *arg) {
|
||||||
// TODO: the loop is bad, ideally models shouldn't affect sensors
|
// TODO: the loop is bad, ideally models shouldn't affect sensors
|
||||||
Context *msg_context = Context::create();
|
Context *msg_context = Context::create();
|
||||||
SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverState", "127.0.0.1", true);
|
SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverState", "127.0.0.1", true);
|
||||||
|
SubSocket *dmonstate_sock = SubSocket::create(msg_context, "dMonitoringState", "127.0.0.1", true);
|
||||||
assert(monitoring_sock != NULL);
|
assert(monitoring_sock != NULL);
|
||||||
|
assert(dmonstate_sock != NULL);
|
||||||
|
|
||||||
cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err);
|
cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
|
@ -195,7 +199,24 @@ void* frontview_thread(void *arg) {
|
||||||
clReleaseEvent(debayer_event);
|
clReleaseEvent(debayer_event);
|
||||||
tbuffer_release(&s->cameras.front.camera_tb, buf_idx);
|
tbuffer_release(&s->cameras.front.camera_tb, buf_idx);
|
||||||
visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE);
|
visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE);
|
||||||
// set front camera metering target
|
|
||||||
|
// no more check after gps check
|
||||||
|
if (!s->rhd_front_checked) {
|
||||||
|
Message *msg_dmon = dmonstate_sock->receive(true);
|
||||||
|
if (msg_dmon != NULL) {
|
||||||
|
auto amsg = kj::heapArray<capnp::word>((msg_dmon->getSize() / sizeof(capnp::word)) + 1);
|
||||||
|
memcpy(amsg.begin(), msg_dmon->getData(), msg_dmon->getSize());
|
||||||
|
|
||||||
|
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||||
|
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||||
|
|
||||||
|
s->rhd_front = event.getDMonitoringState().getIsRHD();
|
||||||
|
s->rhd_front_checked = event.getDMonitoringState().getRhdChecked();
|
||||||
|
|
||||||
|
delete msg_dmon;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
Message *msg = monitoring_sock->receive(true);
|
Message *msg = monitoring_sock->receive(true);
|
||||||
if (msg != NULL) {
|
if (msg != NULL) {
|
||||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||||
|
@ -212,7 +233,7 @@ void* frontview_thread(void *arg) {
|
||||||
// set front camera metering target
|
// set front camera metering target
|
||||||
if (face_prob > 0.4)
|
if (face_prob > 0.4)
|
||||||
{
|
{
|
||||||
int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height;
|
int x_offset = s->rhd_front ? 0:s->rgb_front_width - 0.5 * s->rgb_front_height;
|
||||||
s->front_meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72;
|
s->front_meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72;
|
||||||
s->front_meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72;
|
s->front_meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72;
|
||||||
s->front_meteringbox_ymin = (face_position[1] + 0.5) * (s->rgb_front_height) - 72;
|
s->front_meteringbox_ymin = (face_position[1] + 0.5) * (s->rgb_front_height) - 72;
|
||||||
|
@ -222,8 +243,8 @@ void* frontview_thread(void *arg) {
|
||||||
{
|
{
|
||||||
s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3;
|
s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3;
|
||||||
s->front_meteringbox_ymax = s->rgb_front_height * 1;
|
s->front_meteringbox_ymax = s->rgb_front_height * 1;
|
||||||
s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5;
|
s->front_meteringbox_xmin = s->rhd_front ? 0:s->rgb_front_width * 3 / 5;
|
||||||
s->front_meteringbox_xmax = s->rgb_front_width;
|
s->front_meteringbox_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
|
||||||
}
|
}
|
||||||
|
|
||||||
delete msg;
|
delete msg;
|
||||||
|
@ -252,8 +273,8 @@ void* frontview_thread(void *arg) {
|
||||||
{
|
{
|
||||||
y_start = s->rgb_front_height * 1 / 3;
|
y_start = s->rgb_front_height * 1 / 3;
|
||||||
y_end = s->rgb_front_height * 1;
|
y_end = s->rgb_front_height * 1;
|
||||||
x_start = s->rgb_front_width * 3 / 5;
|
x_start = s->rhd_front ? 0:s->rgb_front_width * 3 / 5;
|
||||||
x_end = s->rgb_front_width;
|
x_end = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint32_t lum_binning[256] = {0,};
|
uint32_t lum_binning[256] = {0,};
|
||||||
|
@ -336,6 +357,7 @@ void* frontview_thread(void *arg) {
|
||||||
}
|
}
|
||||||
|
|
||||||
delete monitoring_sock;
|
delete monitoring_sock;
|
||||||
|
delete dmonstate_sock;
|
||||||
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,12 +1,11 @@
|
||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
import gc
|
import gc
|
||||||
from common.realtime import set_realtime_priority
|
from common.realtime import set_realtime_priority
|
||||||
from common.params import Params, put_nonblocking
|
from common.params import Params
|
||||||
import cereal.messaging as messaging
|
import cereal.messaging as messaging
|
||||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||||
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
|
from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
|
||||||
from selfdrive.locationd.calibration_helpers import Calibration
|
from selfdrive.locationd.calibration_helpers import Calibration
|
||||||
from selfdrive.controls.lib.gps_helpers import is_rhd_region
|
|
||||||
|
|
||||||
def dmonitoringd_thread(sm=None, pm=None):
|
def dmonitoringd_thread(sm=None, pm=None):
|
||||||
gc.disable()
|
gc.disable()
|
||||||
|
@ -21,12 +20,13 @@ def dmonitoringd_thread(sm=None, pm=None):
|
||||||
pm = messaging.PubMaster(['dMonitoringState'])
|
pm = messaging.PubMaster(['dMonitoringState'])
|
||||||
|
|
||||||
if sm is None:
|
if sm is None:
|
||||||
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model', 'gpsLocation'], ignore_alive=['gpsLocation'])
|
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model'])
|
||||||
|
|
||||||
driver_status = DriverStatus()
|
driver_status = DriverStatus()
|
||||||
is_rhd = params.get("IsRHD")
|
is_rhd = params.get("IsRHD")
|
||||||
if is_rhd is not None:
|
if is_rhd is not None:
|
||||||
driver_status.is_rhd_region = bool(int(is_rhd))
|
driver_status.is_rhd_region = bool(int(is_rhd))
|
||||||
|
driver_status.is_rhd_region_checked = True
|
||||||
|
|
||||||
sm['liveCalibration'].calStatus = Calibration.INVALID
|
sm['liveCalibration'].calStatus = Calibration.INVALID
|
||||||
sm['carState'].vEgo = 0.
|
sm['carState'].vEgo = 0.
|
||||||
|
@ -44,13 +44,6 @@ def dmonitoringd_thread(sm=None, pm=None):
|
||||||
while True:
|
while True:
|
||||||
sm.update()
|
sm.update()
|
||||||
|
|
||||||
# GPS coords RHD parsing, once every restart
|
|
||||||
if not driver_status.is_rhd_region_checked and sm.updated['gpsLocation']:
|
|
||||||
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
|
# Handle calibration
|
||||||
if sm.updated['liveCalibration']:
|
if sm.updated['liveCalibration']:
|
||||||
if sm['liveCalibration'].calStatus == Calibration.CALIBRATED:
|
if sm['liveCalibration'].calStatus == Calibration.CALIBRATED:
|
||||||
|
@ -99,6 +92,7 @@ def dmonitoringd_thread(sm=None, pm=None):
|
||||||
"awarenessPassive": driver_status.awareness_passive,
|
"awarenessPassive": driver_status.awareness_passive,
|
||||||
"isLowStd": driver_status.pose.low_std,
|
"isLowStd": driver_status.pose.low_std,
|
||||||
"hiStdCount": driver_status.hi_stds,
|
"hiStdCount": driver_status.hi_stds,
|
||||||
|
"isPreview": False,
|
||||||
}
|
}
|
||||||
pm.send('dMonitoringState', dat)
|
pm.send('dMonitoringState', dat)
|
||||||
|
|
||||||
|
|
|
@ -31,8 +31,8 @@ _HI_STD_FALLBACK_TIME = 10 # fall back to wheel touch if model is uncertain for
|
||||||
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||||
|
|
||||||
_POSE_CALIB_MIN_SPEED = 13 # 30 mph
|
_POSE_CALIB_MIN_SPEED = 13 # 30 mph
|
||||||
_POSE_OFFSET_MIN_COUNT = 60 # valid data counts before calibration completes, 1 seg is 600 counts
|
_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts
|
||||||
_POSE_OFFSET_MAX_COUNT = 360 # stop deweighting new data after 6 min, aka "short term memory"
|
_POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "short term memory"
|
||||||
|
|
||||||
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
|
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
|
||||||
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
|
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
|
||||||
|
@ -49,7 +49,7 @@ class DistractedType():
|
||||||
BAD_POSE = 1
|
BAD_POSE = 1
|
||||||
BAD_BLINK = 2
|
BAD_BLINK = 2
|
||||||
|
|
||||||
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
|
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd):
|
||||||
# the output of these angles are in device frame
|
# the output of these angles are in device frame
|
||||||
# so from driver's perspective, pitch is up and yaw is right
|
# so from driver's perspective, pitch is up and yaw is right
|
||||||
|
|
||||||
|
@ -67,7 +67,7 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
|
||||||
|
|
||||||
# no calib for roll
|
# no calib for roll
|
||||||
pitch -= rpy_calib[1]
|
pitch -= rpy_calib[1]
|
||||||
yaw -= rpy_calib[2]
|
yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> +=
|
||||||
return roll, pitch, yaw
|
return roll, pitch, yaw
|
||||||
|
|
||||||
class DriverPose():
|
class DriverPose():
|
||||||
|
@ -174,7 +174,7 @@ class DriverStatus():
|
||||||
if len(driver_state.faceOrientation) == 0 or len(driver_state.facePosition) == 0 or len(driver_state.faceOrientationStd) == 0 or len(driver_state.facePositionStd) == 0:
|
if len(driver_state.faceOrientation) == 0 or len(driver_state.facePosition) == 0 or len(driver_state.faceOrientationStd) == 0 or len(driver_state.facePositionStd) == 0:
|
||||||
return
|
return
|
||||||
|
|
||||||
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy)
|
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region)
|
||||||
self.pose.pitch_std = driver_state.faceOrientationStd[0]
|
self.pose.pitch_std = driver_state.faceOrientationStd[0]
|
||||||
self.pose.yaw_std = driver_state.faceOrientationStd[1]
|
self.pose.yaw_std = driver_state.faceOrientationStd[1]
|
||||||
# self.pose.roll_std = driver_state.faceOrientationStd[2]
|
# self.pose.roll_std = driver_state.faceOrientationStd[2]
|
||||||
|
@ -183,8 +183,7 @@ class DriverStatus():
|
||||||
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb>_EYE_THRESHOLD)
|
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb>_EYE_THRESHOLD)
|
||||||
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb>_EYE_THRESHOLD)
|
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb>_EYE_THRESHOLD)
|
||||||
self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \
|
self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \
|
||||||
abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 and \
|
abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45
|
||||||
not self.is_rhd_region
|
|
||||||
|
|
||||||
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0
|
self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0
|
||||||
# first order filters
|
# first order filters
|
||||||
|
|
|
@ -0,0 +1,74 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
import os
|
||||||
|
import subprocess
|
||||||
|
import multiprocessing
|
||||||
|
import signal
|
||||||
|
import time
|
||||||
|
|
||||||
|
import cereal.messaging as messaging
|
||||||
|
from common.params import Params
|
||||||
|
|
||||||
|
from common.basedir import BASEDIR
|
||||||
|
|
||||||
|
KILL_TIMEOUT = 15
|
||||||
|
|
||||||
|
def send_controls_packet(pm):
|
||||||
|
while True:
|
||||||
|
dat = messaging.new_message('controlsState')
|
||||||
|
dat.controlsState = {
|
||||||
|
"rearViewCam": True,
|
||||||
|
}
|
||||||
|
pm.send('controlsState', dat)
|
||||||
|
|
||||||
|
def send_dmon_packet(pm, d):
|
||||||
|
dat = messaging.new_message('dMonitoringState')
|
||||||
|
dat.dMonitoringState = {
|
||||||
|
"isRHD": d[0],
|
||||||
|
"rhdChecked": d[1],
|
||||||
|
"isPreview": d[2],
|
||||||
|
}
|
||||||
|
pm.send('dMonitoringState', dat)
|
||||||
|
|
||||||
|
def main():
|
||||||
|
pm = messaging.PubMaster(['controlsState', 'dMonitoringState'])
|
||||||
|
controls_sender = multiprocessing.Process(target=send_controls_packet, args=[pm])
|
||||||
|
controls_sender.start()
|
||||||
|
|
||||||
|
# TODO: refactor with manager start/kill
|
||||||
|
proc_cam = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad"))
|
||||||
|
proc_mon = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/modeld/dmonitoringmodeld"), cwd=os.path.join(BASEDIR, "selfdrive/modeld"))
|
||||||
|
|
||||||
|
params = Params()
|
||||||
|
is_rhd = False
|
||||||
|
is_rhd_checked = False
|
||||||
|
should_exit = False
|
||||||
|
|
||||||
|
def terminate(signalNumber, frame):
|
||||||
|
print('got SIGTERM, exiting..')
|
||||||
|
should_exit = True
|
||||||
|
send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit])
|
||||||
|
proc_cam.send_signal(signal.SIGINT)
|
||||||
|
proc_mon.send_signal(signal.SIGINT)
|
||||||
|
kill_start = time.time()
|
||||||
|
while proc_cam.poll() is None:
|
||||||
|
if time.time() - kill_start > KILL_TIMEOUT:
|
||||||
|
from selfdrive.swaglog import cloudlog
|
||||||
|
cloudlog.critical("FORCE REBOOTING PHONE!")
|
||||||
|
os.system("date >> /sdcard/unkillable_reboot")
|
||||||
|
os.system("reboot")
|
||||||
|
raise RuntimeError
|
||||||
|
continue
|
||||||
|
controls_sender.terminate()
|
||||||
|
exit()
|
||||||
|
|
||||||
|
signal.signal(signal.SIGTERM, terminate)
|
||||||
|
|
||||||
|
while True:
|
||||||
|
send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit])
|
||||||
|
|
||||||
|
if not is_rhd_checked:
|
||||||
|
is_rhd = params.get("IsRHD") == b"1"
|
||||||
|
is_rhd_checked = True
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
|
@ -187,6 +187,7 @@ managed_processes = {
|
||||||
"updated": "selfdrive.updated",
|
"updated": "selfdrive.updated",
|
||||||
"dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]),
|
"dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]),
|
||||||
"modeld": ("selfdrive/modeld", ["./modeld"]),
|
"modeld": ("selfdrive/modeld", ["./modeld"]),
|
||||||
|
"driverview": "selfdrive.controls.lib.driverview",
|
||||||
}
|
}
|
||||||
|
|
||||||
daemon_processes = {
|
daemon_processes = {
|
||||||
|
@ -471,7 +472,7 @@ def manager_thread():
|
||||||
if msg.thermal.freeSpace < 0.05:
|
if msg.thermal.freeSpace < 0.05:
|
||||||
logger_dead = True
|
logger_dead = True
|
||||||
|
|
||||||
if msg.thermal.started:
|
if msg.thermal.started and "driverview" not in running:
|
||||||
for p in car_started_processes:
|
for p in car_started_processes:
|
||||||
if p == "loggerd" and logger_dead:
|
if p == "loggerd" and logger_dead:
|
||||||
kill_managed_process(p)
|
kill_managed_process(p)
|
||||||
|
@ -481,6 +482,11 @@ def manager_thread():
|
||||||
logger_dead = False
|
logger_dead = False
|
||||||
for p in reversed(car_started_processes):
|
for p in reversed(car_started_processes):
|
||||||
kill_managed_process(p)
|
kill_managed_process(p)
|
||||||
|
# this is ugly
|
||||||
|
if "driverview" not in running and params.get("IsDriverViewEnabled") == b"1":
|
||||||
|
start_managed_process("driverview")
|
||||||
|
elif "driverview" in running and params.get("IsDriverViewEnabled") == b"0":
|
||||||
|
kill_managed_process("driverview")
|
||||||
|
|
||||||
# check the status of all processes, did any of them die?
|
# check the status of all processes, did any of them die?
|
||||||
running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running]
|
running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running]
|
||||||
|
@ -538,6 +544,7 @@ def main():
|
||||||
default_params = [
|
default_params = [
|
||||||
("CommunityFeaturesToggle", "0"),
|
("CommunityFeaturesToggle", "0"),
|
||||||
("CompletedTrainingVersion", "0"),
|
("CompletedTrainingVersion", "0"),
|
||||||
|
("IsRHD", "0"),
|
||||||
("IsMetric", "0"),
|
("IsMetric", "0"),
|
||||||
("RecordFront", "0"),
|
("RecordFront", "0"),
|
||||||
("HasAcceptedTerms", "0"),
|
("HasAcceptedTerms", "0"),
|
||||||
|
@ -552,6 +559,7 @@ def main():
|
||||||
("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')),
|
("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')),
|
||||||
("OpenpilotEnabledToggle", "1"),
|
("OpenpilotEnabledToggle", "1"),
|
||||||
("LaneChangeEnabled", "1"),
|
("LaneChangeEnabled", "1"),
|
||||||
|
("IsDriverViewEnabled", "0"),
|
||||||
]
|
]
|
||||||
|
|
||||||
# set unset params
|
# set unset params
|
||||||
|
|
|
@ -28,6 +28,8 @@ int main(int argc, char **argv) {
|
||||||
// messaging
|
// messaging
|
||||||
Context *msg_context = Context::create();
|
Context *msg_context = Context::create();
|
||||||
PubSocket *dmonitoring_sock = PubSocket::create(msg_context, "driverState");
|
PubSocket *dmonitoring_sock = PubSocket::create(msg_context, "driverState");
|
||||||
|
SubSocket *dmonstate_sock = SubSocket::create(msg_context, "dMonitoringState", "127.0.0.1", true);
|
||||||
|
assert(dmonstate_sock != NULL);
|
||||||
|
|
||||||
// init the models
|
// init the models
|
||||||
DMonitoringModelState dmonitoringmodel;
|
DMonitoringModelState dmonitoringmodel;
|
||||||
|
@ -46,6 +48,7 @@ int main(int argc, char **argv) {
|
||||||
LOGW("connected with buffer size: %d", buf_info.buf_len);
|
LOGW("connected with buffer size: %d", buf_info.buf_len);
|
||||||
|
|
||||||
double last = 0;
|
double last = 0;
|
||||||
|
int chk_counter = 0;
|
||||||
while (!do_exit) {
|
while (!do_exit) {
|
||||||
VIPCBuf *buf;
|
VIPCBuf *buf;
|
||||||
VIPCBufExtra extra;
|
VIPCBufExtra extra;
|
||||||
|
@ -55,6 +58,23 @@ int main(int argc, char **argv) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
//printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height);
|
//printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height);
|
||||||
|
if (!dmonitoringmodel.is_rhd_checked) {
|
||||||
|
if (chk_counter >= RHD_CHECK_INTERVAL) {
|
||||||
|
Message *msg = dmonstate_sock->receive(true);
|
||||||
|
if (msg != NULL) {
|
||||||
|
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||||
|
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||||
|
|
||||||
|
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||||
|
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||||
|
|
||||||
|
dmonitoringmodel.is_rhd = event.getDMonitoringState().getIsRHD();
|
||||||
|
dmonitoringmodel.is_rhd_checked = event.getDMonitoringState().getRhdChecked();
|
||||||
|
}
|
||||||
|
chk_counter = 0;
|
||||||
|
}
|
||||||
|
chk_counter += 1;
|
||||||
|
}
|
||||||
|
|
||||||
double t1 = millis_since_boot();
|
double t1 = millis_since_boot();
|
||||||
|
|
||||||
|
@ -74,6 +94,7 @@ int main(int argc, char **argv) {
|
||||||
visionstream_destroy(&stream);
|
visionstream_destroy(&stream);
|
||||||
|
|
||||||
delete dmonitoring_sock;
|
delete dmonitoring_sock;
|
||||||
|
delete dmonstate_sock;
|
||||||
dmonitoring_free(&dmonitoringmodel);
|
dmonitoring_free(&dmonitoringmodel);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
|
|
|
@ -23,6 +23,8 @@ void dmonitoring_init(DMonitoringModelState* s) {
|
||||||
const char* model_path = "../../models/dmonitoring_model.dlc";
|
const char* model_path = "../../models/dmonitoring_model.dlc";
|
||||||
#endif
|
#endif
|
||||||
s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME);
|
s->m = new DefaultRunModel(model_path, (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME);
|
||||||
|
s->is_rhd = false;
|
||||||
|
s->is_rhd_checked = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) {
|
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) {
|
||||||
|
@ -43,7 +45,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
|
||||||
uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height);
|
uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height);
|
||||||
uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2));
|
uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2));
|
||||||
|
|
||||||
if (true) {
|
if (!s->is_rhd) {
|
||||||
for (int r = 0; r < height/2; r++) {
|
for (int r = 0; r < height/2; r++) {
|
||||||
memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width + (width - cropped_width), cropped_width);
|
memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width + (width - cropped_width), cropped_width);
|
||||||
memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width + (width - cropped_width), cropped_width);
|
memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width + (width - cropped_width), cropped_width);
|
||||||
|
|
|
@ -14,6 +14,7 @@ extern "C" {
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define OUTPUT_SIZE 33
|
#define OUTPUT_SIZE 33
|
||||||
|
#define RHD_CHECK_INTERVAL 10
|
||||||
|
|
||||||
typedef struct DMonitoringResult {
|
typedef struct DMonitoringResult {
|
||||||
float face_orientation[3];
|
float face_orientation[3];
|
||||||
|
@ -29,6 +30,8 @@ typedef struct DMonitoringResult {
|
||||||
|
|
||||||
typedef struct DMonitoringModelState {
|
typedef struct DMonitoringModelState {
|
||||||
RunModel *m;
|
RunModel *m;
|
||||||
|
bool is_rhd;
|
||||||
|
bool is_rhd_checked;
|
||||||
float output[OUTPUT_SIZE];
|
float output[OUTPUT_SIZE];
|
||||||
} DMonitoringModelState;
|
} DMonitoringModelState;
|
||||||
|
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
ea2820c0708a95e8f392d503c7b76090ade380b0
|
3523742130b9e0554bab4ac5bc5ab535f1342e90
|
|
@ -339,8 +339,9 @@ def thermald_thread():
|
||||||
|
|
||||||
# don't start while taking snapshot
|
# don't start while taking snapshot
|
||||||
if not should_start_prev:
|
if not should_start_prev:
|
||||||
|
is_viewing_driver = params.get("IsDriverViewEnabled") == b"1"
|
||||||
is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
|
is_taking_snapshot = params.get("IsTakingSnapshot") == b"1"
|
||||||
should_start = should_start and (not is_taking_snapshot)
|
should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver)
|
||||||
|
|
||||||
if fw_version_match and not fw_version_match_prev:
|
if fw_version_match and not fw_version_match_prev:
|
||||||
params.delete("Offroad_PandaFirmwareMismatch")
|
params.delete("Offroad_PandaFirmwareMismatch")
|
||||||
|
|
|
@ -279,7 +279,6 @@ static void draw_steering(UIState *s, float curvature) {
|
||||||
static void draw_frame(UIState *s) {
|
static void draw_frame(UIState *s) {
|
||||||
const UIScene *scene = &s->scene;
|
const UIScene *scene = &s->scene;
|
||||||
|
|
||||||
float x1, x2, y1, y2;
|
|
||||||
if (s->scene.frontview) {
|
if (s->scene.frontview) {
|
||||||
glBindVertexArray(s->frame_vao[1]);
|
glBindVertexArray(s->frame_vao[1]);
|
||||||
} else {
|
} else {
|
||||||
|
@ -703,6 +702,100 @@ static void ui_draw_vision_face(UIState *s) {
|
||||||
ui_draw_image(s->vg, face_img_x, face_img_y, face_img_size, face_img_size, s->img_face, face_img_alpha);
|
ui_draw_image(s->vg, face_img_x, face_img_y, face_img_size, face_img_size, s->img_face, face_img_alpha);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void ui_draw_driver_view(UIState *s) {
|
||||||
|
const UIScene *scene = &s->scene;
|
||||||
|
s->scene.uilayout_sidebarcollapsed = true;
|
||||||
|
const int frame_x = scene->ui_viz_rx;
|
||||||
|
const int frame_w = scene->ui_viz_rw;
|
||||||
|
const int valid_frame_w = 4 * box_h / 3;
|
||||||
|
const int valid_frame_x = frame_x + (frame_w - valid_frame_w) / 2 + ff_xoffset;
|
||||||
|
|
||||||
|
// blackout
|
||||||
|
if (!scene->is_rhd) {
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x + valid_frame_w,
|
||||||
|
box_y,
|
||||||
|
valid_frame_x + box_h / 2, box_y,
|
||||||
|
nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0));
|
||||||
|
nvgFillPaint(s->vg, gradient);
|
||||||
|
nvgRect(s->vg, valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h);
|
||||||
|
nvgFill(s->vg);
|
||||||
|
} else {
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x,
|
||||||
|
box_y,
|
||||||
|
valid_frame_w - box_h / 2, box_y,
|
||||||
|
nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0));
|
||||||
|
nvgFillPaint(s->vg, gradient);
|
||||||
|
nvgRect(s->vg, valid_frame_x, box_y, valid_frame_w - box_h / 2, box_h);
|
||||||
|
nvgFill(s->vg);
|
||||||
|
}
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
nvgRect(s->vg, scene->is_rhd ? valid_frame_x:valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h);
|
||||||
|
nvgFillColor(s->vg, COLOR_BLACK_ALPHA(144));
|
||||||
|
nvgFill(s->vg);
|
||||||
|
|
||||||
|
// borders
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
nvgRect(s->vg, frame_x, box_y, valid_frame_x - frame_x, box_h);
|
||||||
|
nvgFillColor(s->vg, nvgRGBA(23,51,73,255));
|
||||||
|
nvgFill(s->vg);
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
nvgRect(s->vg, valid_frame_x + valid_frame_w, box_y, frame_w - valid_frame_w - (valid_frame_x - frame_x), box_h);
|
||||||
|
nvgFillColor(s->vg, nvgRGBA(23,51,73,255));
|
||||||
|
nvgFill(s->vg);
|
||||||
|
|
||||||
|
// draw face box
|
||||||
|
if (scene->face_prob > 0.4) {
|
||||||
|
int fbox_x;
|
||||||
|
int fbox_y = box_y + (scene->face_y + 0.5) * box_h - 0.5 * 0.6 * box_h / 2;;
|
||||||
|
if (!scene->is_rhd) {
|
||||||
|
fbox_x = valid_frame_x + (1 - (scene->face_x + 0.5)) * (box_h / 2) - 0.5 * 0.6 * box_h / 2;
|
||||||
|
} else {
|
||||||
|
fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + (scene->face_x + 0.5) * (box_h / 2) - 0.5 * 0.6 * box_h / 2;
|
||||||
|
}
|
||||||
|
if (abs(scene->face_x) <= 0.35 && abs(scene->face_y) <= 0.4) {
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
nvgRoundedRect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, 35);
|
||||||
|
nvgStrokeColor(s->vg, nvgRGBAf(1.0, 1.0, 1.0, 0.8 - ((abs(scene->face_x) > abs(scene->face_y) ? abs(scene->face_x):abs(scene->face_y))) * 0.6 / 0.375));
|
||||||
|
nvgStrokeWidth(s->vg, 10);
|
||||||
|
nvgStroke(s->vg);
|
||||||
|
} else {
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
nvgRoundedRect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2, 35);
|
||||||
|
nvgStrokeColor(s->vg, nvgRGBAf(1.0, 1.0, 1.0, 0.2));
|
||||||
|
nvgStrokeWidth(s->vg, 10);
|
||||||
|
nvgStroke(s->vg);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
|
// draw face icon
|
||||||
|
const int face_size = 85;
|
||||||
|
const int face_x = (valid_frame_x + face_size + (bdr_s * 2)) + (scene->is_rhd ? valid_frame_w - box_h / 2:0);
|
||||||
|
const int face_y = (box_y + box_h - face_size - bdr_s - (bdr_s * 1.5));
|
||||||
|
const int face_img_size = (face_size * 1.5);
|
||||||
|
const int face_img_x = (face_x - (face_img_size / 2));
|
||||||
|
const int face_img_y = (face_y - (face_size / 4));
|
||||||
|
float face_img_alpha = scene->face_prob > 0.4 ? 1.0f : 0.15f;
|
||||||
|
float face_bg_alpha = scene->face_prob > 0.4 ? 0.3f : 0.1f;
|
||||||
|
NVGcolor face_bg = nvgRGBA(0, 0, 0, (255 * face_bg_alpha));
|
||||||
|
NVGpaint face_img = nvgImagePattern(s->vg, face_img_x, face_img_y,
|
||||||
|
face_img_size, face_img_size, 0, s->img_face, face_img_alpha);
|
||||||
|
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
nvgCircle(s->vg, face_x, (face_y + (bdr_s * 1.5)), face_size);
|
||||||
|
nvgFillColor(s->vg, face_bg);
|
||||||
|
nvgFill(s->vg);
|
||||||
|
|
||||||
|
nvgBeginPath(s->vg);
|
||||||
|
nvgRect(s->vg, face_img_x, face_img_y, face_img_size, face_img_size);
|
||||||
|
nvgFillPaint(s->vg, face_img);
|
||||||
|
nvgFill(s->vg);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
static void ui_draw_vision_header(UIState *s) {
|
static void ui_draw_vision_header(UIState *s) {
|
||||||
const UIScene *scene = &s->scene;
|
const UIScene *scene = &s->scene;
|
||||||
int ui_viz_rx = scene->ui_viz_rx;
|
int ui_viz_rx = scene->ui_viz_rx;
|
||||||
|
@ -828,14 +921,18 @@ static void ui_draw_vision(UIState *s) {
|
||||||
nvgRestore(s->vg);
|
nvgRestore(s->vg);
|
||||||
|
|
||||||
// Set Speed, Current Speed, Status/Events
|
// Set Speed, Current Speed, Status/Events
|
||||||
ui_draw_vision_header(s);
|
if (!scene->frontview) {
|
||||||
|
ui_draw_vision_header(s);
|
||||||
|
} else {
|
||||||
|
ui_draw_driver_view(s);
|
||||||
|
}
|
||||||
|
|
||||||
if (s->scene.alert_size != ALERTSIZE_NONE) {
|
if (s->scene.alert_size != ALERTSIZE_NONE) {
|
||||||
// Controls Alerts
|
// Controls Alerts
|
||||||
ui_draw_vision_alert(s, s->scene.alert_size, s->status,
|
ui_draw_vision_alert(s, s->scene.alert_size, s->status,
|
||||||
s->scene.alert_text1, s->scene.alert_text2);
|
s->scene.alert_text1, s->scene.alert_text2);
|
||||||
} else {
|
} else {
|
||||||
ui_draw_vision_footer(s);
|
if (!scene->frontview){ui_draw_vision_footer(s);}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -134,10 +134,18 @@ static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void handle_driver_view_touch(UIState *s, int touch_x, int touch_y) {
|
||||||
|
int err = write_db_value(NULL, "IsDriverViewEnabled", "0", 1);
|
||||||
|
}
|
||||||
|
|
||||||
static void handle_vision_touch(UIState *s, int touch_x, int touch_y) {
|
static void handle_vision_touch(UIState *s, int touch_x, int touch_y) {
|
||||||
if (s->started && (touch_x >= s->scene.ui_viz_rx - bdr_s)
|
if (s->started && (touch_x >= s->scene.ui_viz_rx - bdr_s)
|
||||||
&& (s->active_app != cereal_UiLayoutState_App_settings)) {
|
&& (s->active_app != cereal_UiLayoutState_App_settings)) {
|
||||||
s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed;
|
if (!s->scene.frontview) {
|
||||||
|
s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed;
|
||||||
|
} else {
|
||||||
|
handle_driver_view_touch(s, touch_x, touch_y);
|
||||||
|
}
|
||||||
update_offroad_layout_state(s);
|
update_offroad_layout_state(s);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -226,6 +234,8 @@ static void ui_init(UIState *s) {
|
||||||
s->thermal_sock = SubSocket::create(s->ctx, "thermal");
|
s->thermal_sock = SubSocket::create(s->ctx, "thermal");
|
||||||
s->health_sock = SubSocket::create(s->ctx, "health");
|
s->health_sock = SubSocket::create(s->ctx, "health");
|
||||||
s->ubloxgnss_sock = SubSocket::create(s->ctx, "ubloxGnss");
|
s->ubloxgnss_sock = SubSocket::create(s->ctx, "ubloxGnss");
|
||||||
|
s->driverstate_sock = SubSocket::create(s->ctx, "driverState");
|
||||||
|
s->dmonitoring_sock = SubSocket::create(s->ctx, "dMonitoringState");
|
||||||
s->offroad_sock = PubSocket::create(s->ctx, "offroadLayout");
|
s->offroad_sock = PubSocket::create(s->ctx, "offroadLayout");
|
||||||
|
|
||||||
assert(s->model_sock != NULL);
|
assert(s->model_sock != NULL);
|
||||||
|
@ -236,6 +246,8 @@ static void ui_init(UIState *s) {
|
||||||
assert(s->thermal_sock != NULL);
|
assert(s->thermal_sock != NULL);
|
||||||
assert(s->health_sock != NULL);
|
assert(s->health_sock != NULL);
|
||||||
assert(s->ubloxgnss_sock != NULL);
|
assert(s->ubloxgnss_sock != NULL);
|
||||||
|
assert(s->driverstate_sock != NULL);
|
||||||
|
assert(s->dmonitoring_sock != NULL);
|
||||||
assert(s->offroad_sock != NULL);
|
assert(s->offroad_sock != NULL);
|
||||||
|
|
||||||
s->poller = Poller::create({
|
s->poller = Poller::create({
|
||||||
|
@ -246,7 +258,9 @@ static void ui_init(UIState *s) {
|
||||||
s->radarstate_sock,
|
s->radarstate_sock,
|
||||||
s->thermal_sock,
|
s->thermal_sock,
|
||||||
s->health_sock,
|
s->health_sock,
|
||||||
s->ubloxgnss_sock
|
s->ubloxgnss_sock,
|
||||||
|
s->driverstate_sock,
|
||||||
|
s->dmonitoring_sock
|
||||||
});
|
});
|
||||||
|
|
||||||
#ifdef SHOW_SPEEDLIMIT
|
#ifdef SHOW_SPEEDLIMIT
|
||||||
|
@ -388,7 +402,8 @@ void handle_message(UIState *s, Message * msg) {
|
||||||
cereal_read_ControlsState(&datad, eventd.controlsState);
|
cereal_read_ControlsState(&datad, eventd.controlsState);
|
||||||
|
|
||||||
s->controls_timeout = 1 * UI_FREQ;
|
s->controls_timeout = 1 * UI_FREQ;
|
||||||
s->controls_seen = true;
|
s->scene.frontview = datad.rearViewCam;
|
||||||
|
if (!s->scene.frontview){s->controls_seen = true;}
|
||||||
|
|
||||||
if (datad.vCruise != s->scene.v_cruise) {
|
if (datad.vCruise != s->scene.v_cruise) {
|
||||||
s->scene.v_cruise_update_ts = eventd.logMonoTime;
|
s->scene.v_cruise_update_ts = eventd.logMonoTime;
|
||||||
|
@ -401,8 +416,6 @@ void handle_message(UIState *s, Message * msg) {
|
||||||
s->scene.gps_planner_active = datad.gpsPlannerActive;
|
s->scene.gps_planner_active = datad.gpsPlannerActive;
|
||||||
s->scene.monitoring_active = datad.driverMonitoringOn;
|
s->scene.monitoring_active = datad.driverMonitoringOn;
|
||||||
|
|
||||||
s->scene.frontview = datad.rearViewCam;
|
|
||||||
|
|
||||||
s->scene.decel_for_model = datad.decelForModel;
|
s->scene.decel_for_model = datad.decelForModel;
|
||||||
|
|
||||||
if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
|
if (datad.alertSound != cereal_CarControl_HUDControl_AudibleAlert_none && datad.alertSound != s->alert_sound) {
|
||||||
|
@ -430,7 +443,6 @@ void handle_message(UIState *s, Message * msg) {
|
||||||
} else {
|
} else {
|
||||||
s->scene.alert_text2[0] = '\0';
|
s->scene.alert_text2[0] = '\0';
|
||||||
}
|
}
|
||||||
s->scene.awareness_status = datad.awarenessStatus;
|
|
||||||
|
|
||||||
s->scene.alert_ts = eventd.logMonoTime;
|
s->scene.alert_ts = eventd.logMonoTime;
|
||||||
|
|
||||||
|
@ -543,24 +555,8 @@ void handle_message(UIState *s, Message * msg) {
|
||||||
s->scene.thermalStatus = datad.thermalStatus;
|
s->scene.thermalStatus = datad.thermalStatus;
|
||||||
s->scene.paTemp = datad.pa0;
|
s->scene.paTemp = datad.pa0;
|
||||||
|
|
||||||
s->started = datad.started;
|
s->thermal_started = datad.started;
|
||||||
|
|
||||||
// Handle onroad/offroad transition
|
|
||||||
if (!datad.started) {
|
|
||||||
if (s->status != STATUS_STOPPED) {
|
|
||||||
update_status(s, STATUS_STOPPED);
|
|
||||||
s->alert_sound_timeout = 0;
|
|
||||||
s->vision_seen = false;
|
|
||||||
s->controls_seen = false;
|
|
||||||
s->active_app = cereal_UiLayoutState_App_home;
|
|
||||||
update_offroad_layout_state(s);
|
|
||||||
}
|
|
||||||
} else if (s->status == STATUS_STOPPED) {
|
|
||||||
update_status(s, STATUS_DISENGAGED);
|
|
||||||
|
|
||||||
s->active_app = cereal_UiLayoutState_App_none;
|
|
||||||
update_offroad_layout_state(s);
|
|
||||||
}
|
|
||||||
} else if (eventd.which == cereal_Event_ubloxGnss) {
|
} else if (eventd.which == cereal_Event_ubloxGnss) {
|
||||||
struct cereal_UbloxGnss datad;
|
struct cereal_UbloxGnss datad;
|
||||||
cereal_read_UbloxGnss(&datad, eventd.ubloxGnss);
|
cereal_read_UbloxGnss(&datad, eventd.ubloxGnss);
|
||||||
|
@ -575,7 +571,43 @@ void handle_message(UIState *s, Message * msg) {
|
||||||
|
|
||||||
s->scene.hwType = datad.hwType;
|
s->scene.hwType = datad.hwType;
|
||||||
s->hardware_timeout = 5*30; // 5 seconds at 30 fps
|
s->hardware_timeout = 5*30; // 5 seconds at 30 fps
|
||||||
|
} else if (eventd.which == cereal_Event_driverState) {
|
||||||
|
struct cereal_DriverState datad;
|
||||||
|
cereal_read_DriverState(&datad, eventd.driverState);
|
||||||
|
|
||||||
|
s->scene.face_prob = datad.faceProb;
|
||||||
|
|
||||||
|
capn_list32 fxy_list = datad.facePosition;
|
||||||
|
capn_resolve(&fxy_list.p);
|
||||||
|
s->scene.face_x = capn_to_f32(capn_get32(fxy_list, 0));
|
||||||
|
s->scene.face_y = capn_to_f32(capn_get32(fxy_list, 1));
|
||||||
|
} else if (eventd.which == cereal_Event_dMonitoringState) {
|
||||||
|
struct cereal_DMonitoringState datad;
|
||||||
|
cereal_read_DMonitoringState(&datad, eventd.dMonitoringState);
|
||||||
|
|
||||||
|
s->scene.is_rhd = datad.isRHD;
|
||||||
|
s->scene.awareness_status = datad.awarenessStatus;
|
||||||
|
s->preview_started = datad.isPreview;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
s->started = s->thermal_started || s->preview_started ;
|
||||||
|
// Handle onroad/offroad transition
|
||||||
|
if (!s->started) {
|
||||||
|
if (s->status != STATUS_STOPPED) {
|
||||||
|
update_status(s, STATUS_STOPPED);
|
||||||
|
s->alert_sound_timeout = 0;
|
||||||
|
s->vision_seen = false;
|
||||||
|
s->controls_seen = false;
|
||||||
|
s->active_app = cereal_UiLayoutState_App_home;
|
||||||
|
update_offroad_layout_state(s);
|
||||||
|
}
|
||||||
|
} else if (s->status == STATUS_STOPPED) {
|
||||||
|
update_status(s, STATUS_DISENGAGED);
|
||||||
|
|
||||||
|
s->active_app = cereal_UiLayoutState_App_none;
|
||||||
|
update_offroad_layout_state(s);
|
||||||
|
}
|
||||||
|
|
||||||
capn_free(&ctx);
|
capn_free(&ctx);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -69,6 +69,7 @@ const int box_y = bdr_s;
|
||||||
const int box_w = vwp_w-sbr_w-(bdr_s*2);
|
const int box_w = vwp_w-sbr_w-(bdr_s*2);
|
||||||
const int box_h = vwp_h-(bdr_s*2);
|
const int box_h = vwp_h-(bdr_s*2);
|
||||||
const int viz_w = vwp_w-(bdr_s*2);
|
const int viz_w = vwp_w-(bdr_s*2);
|
||||||
|
const int ff_xoffset = 32;
|
||||||
const int header_h = 420;
|
const int header_h = 420;
|
||||||
const int footer_h = 280;
|
const int footer_h = 280;
|
||||||
const int footer_y = vwp_h-bdr_s-footer_h;
|
const int footer_y = vwp_h-bdr_s-footer_h;
|
||||||
|
@ -140,6 +141,10 @@ typedef struct UIScene {
|
||||||
int lead_status2;
|
int lead_status2;
|
||||||
float lead_d_rel2, lead_y_rel2, lead_v_rel2;
|
float lead_d_rel2, lead_y_rel2, lead_v_rel2;
|
||||||
|
|
||||||
|
float face_prob;
|
||||||
|
bool is_rhd;
|
||||||
|
float face_x, face_y;
|
||||||
|
|
||||||
int front_box_x, front_box_y, front_box_width, front_box_height;
|
int front_box_x, front_box_y, front_box_width, front_box_height;
|
||||||
|
|
||||||
uint64_t alert_ts;
|
uint64_t alert_ts;
|
||||||
|
@ -216,6 +221,8 @@ typedef struct UIState {
|
||||||
SubSocket *thermal_sock;
|
SubSocket *thermal_sock;
|
||||||
SubSocket *health_sock;
|
SubSocket *health_sock;
|
||||||
SubSocket *ubloxgnss_sock;
|
SubSocket *ubloxgnss_sock;
|
||||||
|
SubSocket *driverstate_sock;
|
||||||
|
SubSocket *dmonitoring_sock;
|
||||||
PubSocket *offroad_sock;
|
PubSocket *offroad_sock;
|
||||||
Poller * poller;
|
Poller * poller;
|
||||||
Poller * ublox_poller;
|
Poller * ublox_poller;
|
||||||
|
@ -281,6 +288,7 @@ typedef struct UIState {
|
||||||
float alert_blinking_alpha;
|
float alert_blinking_alpha;
|
||||||
bool alert_blinked;
|
bool alert_blinked;
|
||||||
bool started;
|
bool started;
|
||||||
|
bool thermal_started, preview_started;
|
||||||
bool vision_seen;
|
bool vision_seen;
|
||||||
|
|
||||||
float light_sensor;
|
float light_sensor;
|
||||||
|
|
Loading…
Reference in New Issue