diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index 38aea35a..b0cdd7c4 100644 --- a/apk/ai.comma.plus.offroad.apk +++ b/apk/ai.comma.plus.offroad.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b6d3698d82870268439609527eea6b350dc65f5ddacbb12120ba2ef5351c2dc4 -size 13691663 +oid sha256:e98a8d4a73ee84ace6fe545d1f98176ede04cc31d7b62c9275ee653d5e0f4d2b +size 13732603 diff --git a/apks b/apks index e4bf49bc..f5d2c171 160000 --- a/apks +++ b/apks @@ -1 +1 @@ -Subproject commit e4bf49bc65e8e680e5afc629240b4ebbcedc4ebd +Subproject commit f5d2c1715c9482d898062110ce4c612093aa5d4f diff --git a/cereal b/cereal index c8e5db83..442e914d 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit c8e5db830d30e5ea0d0abef0479d9c48cfd24504 +Subproject commit 442e914d73b163823fceb91c2b57062965fad045 diff --git a/common/params.py b/common/params.py index 24e31bff..787235bf 100755 --- a/common/params.py +++ b/common/params.py @@ -70,6 +70,7 @@ keys = { "GithubSshKeys": [TxType.PERSISTENT], "HasAcceptedTerms": [TxType.PERSISTENT], "HasCompletedSetup": [TxType.PERSISTENT], + "IsDriverViewEnabled": [TxType.CLEAR_ON_MANAGER_START], "IsLdwEnabled": [TxType.PERSISTENT], "IsGeofenceEnabled": [TxType.PERSISTENT], "IsMetric": [TxType.PERSISTENT], diff --git a/models/dmonitoring_model.current b/models/dmonitoring_model.current index d48d14fe..9c95aff3 100644 --- a/models/dmonitoring_model.current +++ b/models/dmonitoring_model.current @@ -1 +1 @@ -2ad118ee-df4f-49ff-a374-6dd693cee37b \ No newline at end of file +43221d85-46fd-40b9-bff0-2b1b18a86b07 \ No newline at end of file diff --git a/models/dmonitoring_model.keras b/models/dmonitoring_model.keras index 868dda12..9a835302 100644 --- a/models/dmonitoring_model.keras +++ b/models/dmonitoring_model.keras @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:72ebd64de360c9e6fabb8f45b03644d6b4405ded318f656d9e71e67b6c598513 -size 813336 +oid sha256:5c39a2096f7058541b5339ec36bc4c468955e67285078080ed6d8802fed06c1d +size 814176 diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc index f28846c7..ece16d45 100644 --- a/models/dmonitoring_model_q.dlc +++ b/models/dmonitoring_model_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9949299b7fe3959f741120707d60b4fdf9e96697565e652ca9669ec211bc27e6 +oid sha256:7460beafe1bf3c6532aaa93b7523cf11a0c0490d1119641054a2c02fbcdc3247 size 186615 diff --git a/release/files_common b/release/files_common index 8f8b79c3..6a40da7a 100644 --- a/release/files_common +++ b/release/files_common @@ -217,6 +217,7 @@ selfdrive/controls/lib/fcw.py selfdrive/controls/lib/long_mpc.py selfdrive/controls/lib/long_mpc_model.py selfdrive/controls/lib/gps_helpers.py +selfdrive/controls/lib/driverview.py selfdrive/controls/lib/cluster/* diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 9a40e3d8..f02f7824 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -114,6 +114,8 @@ struct VisionState { int rgb_front_width, rgb_front_height, rgb_front_stride; VisionBuf rgb_front_bufs[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_ymin, front_meteringbox_ymax; @@ -152,7 +154,9 @@ void* frontview_thread(void *arg) { // TODO: the loop is bad, ideally models shouldn't affect sensors Context *msg_context = Context::create(); 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(dmonstate_sock != NULL); cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); assert(err == 0); @@ -195,7 +199,24 @@ void* frontview_thread(void *arg) { clReleaseEvent(debayer_event); tbuffer_release(&s->cameras.front.camera_tb, buf_idx); 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((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(); + + s->rhd_front = event.getDMonitoringState().getIsRHD(); + s->rhd_front_checked = event.getDMonitoringState().getRhdChecked(); + + delete msg_dmon; + } + } + Message *msg = monitoring_sock->receive(true); if (msg != NULL) { auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); @@ -212,7 +233,7 @@ void* frontview_thread(void *arg) { // set front camera metering target 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_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; @@ -222,8 +243,8 @@ void* frontview_thread(void *arg) { { s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3; s->front_meteringbox_ymax = s->rgb_front_height * 1; - s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5; - s->front_meteringbox_xmax = s->rgb_front_width; + s->front_meteringbox_xmin = s->rhd_front ? 0:s->rgb_front_width * 3 / 5; + s->front_meteringbox_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width; } delete msg; @@ -252,8 +273,8 @@ void* frontview_thread(void *arg) { { y_start = s->rgb_front_height * 1 / 3; y_end = s->rgb_front_height * 1; - x_start = s->rgb_front_width * 3 / 5; - x_end = s->rgb_front_width; + x_start = s->rhd_front ? 0:s->rgb_front_width * 3 / 5; + x_end = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width; } uint32_t lum_binning[256] = {0,}; @@ -336,6 +357,7 @@ void* frontview_thread(void *arg) { } delete monitoring_sock; + delete dmonstate_sock; return NULL; } diff --git a/selfdrive/controls/dmonitoringd.py b/selfdrive/controls/dmonitoringd.py index 86afb9dc..c9c37044 100755 --- a/selfdrive/controls/dmonitoringd.py +++ b/selfdrive/controls/dmonitoringd.py @@ -1,12 +1,11 @@ #!/usr/bin/env python3 import gc from common.realtime import set_realtime_priority -from common.params import Params, put_nonblocking +from common.params import Params import cereal.messaging as messaging 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.locationd.calibration_helpers import Calibration -from selfdrive.controls.lib.gps_helpers import is_rhd_region def dmonitoringd_thread(sm=None, pm=None): gc.disable() @@ -21,12 +20,13 @@ def dmonitoringd_thread(sm=None, pm=None): pm = messaging.PubMaster(['dMonitoringState']) 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() is_rhd = params.get("IsRHD") if is_rhd is not None: driver_status.is_rhd_region = bool(int(is_rhd)) + driver_status.is_rhd_region_checked = True sm['liveCalibration'].calStatus = Calibration.INVALID sm['carState'].vEgo = 0. @@ -44,13 +44,6 @@ def dmonitoringd_thread(sm=None, pm=None): while True: 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 if sm.updated['liveCalibration']: if sm['liveCalibration'].calStatus == Calibration.CALIBRATED: @@ -99,6 +92,7 @@ def dmonitoringd_thread(sm=None, pm=None): "awarenessPassive": driver_status.awareness_passive, "isLowStd": driver_status.pose.low_std, "hiStdCount": driver_status.hi_stds, + "isPreview": False, } pm.send('dMonitoringState', dat) diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 8f0dbda7..1d5a7078 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -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 _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_MAX_COUNT = 360 # stop deweighting new data after 6 min, aka "short term memory" +_POSE_OFFSET_MIN_COUNT = 600 # valid data counts before calibration completes, 1 seg is 600 counts +_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_MIN = 1.25 # relative to minus step change @@ -49,7 +49,7 @@ class DistractedType(): BAD_POSE = 1 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 # 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 pitch -= rpy_calib[1] - yaw -= rpy_calib[2] + yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> += return roll, pitch, yaw 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: 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.yaw_std = driver_state.faceOrientationStd[1] # 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.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb>_EYE_THRESHOLD) 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 \ - not self.is_rhd_region + abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 # first order filters diff --git a/selfdrive/controls/lib/driverview.py b/selfdrive/controls/lib/driverview.py new file mode 100755 index 00000000..298858bd --- /dev/null +++ b/selfdrive/controls/lib/driverview.py @@ -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() \ No newline at end of file diff --git a/selfdrive/manager.py b/selfdrive/manager.py index bc22a139..fc8f9506 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -187,6 +187,7 @@ managed_processes = { "updated": "selfdrive.updated", "dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]), "modeld": ("selfdrive/modeld", ["./modeld"]), + "driverview": "selfdrive.controls.lib.driverview", } daemon_processes = { @@ -471,7 +472,7 @@ def manager_thread(): if msg.thermal.freeSpace < 0.05: logger_dead = True - if msg.thermal.started: + if msg.thermal.started and "driverview" not in running: for p in car_started_processes: if p == "loggerd" and logger_dead: kill_managed_process(p) @@ -481,6 +482,11 @@ def manager_thread(): logger_dead = False for p in reversed(car_started_processes): 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? 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 = [ ("CommunityFeaturesToggle", "0"), ("CompletedTrainingVersion", "0"), + ("IsRHD", "0"), ("IsMetric", "0"), ("RecordFront", "0"), ("HasAcceptedTerms", "0"), @@ -552,6 +559,7 @@ def main(): ("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')), ("OpenpilotEnabledToggle", "1"), ("LaneChangeEnabled", "1"), + ("IsDriverViewEnabled", "0"), ] # set unset params diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc index 9fb67b63..7acbffa6 100644 --- a/selfdrive/modeld/dmonitoringmodeld.cc +++ b/selfdrive/modeld/dmonitoringmodeld.cc @@ -28,6 +28,8 @@ int main(int argc, char **argv) { // messaging Context *msg_context = Context::create(); 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 DMonitoringModelState dmonitoringmodel; @@ -46,6 +48,7 @@ int main(int argc, char **argv) { LOGW("connected with buffer size: %d", buf_info.buf_len); double last = 0; + int chk_counter = 0; while (!do_exit) { VIPCBuf *buf; VIPCBufExtra extra; @@ -55,6 +58,23 @@ int main(int argc, char **argv) { break; } //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((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + dmonitoringmodel.is_rhd = event.getDMonitoringState().getIsRHD(); + dmonitoringmodel.is_rhd_checked = event.getDMonitoringState().getRhdChecked(); + } + chk_counter = 0; + } + chk_counter += 1; + } double t1 = millis_since_boot(); @@ -74,6 +94,7 @@ int main(int argc, char **argv) { visionstream_destroy(&stream); delete dmonitoring_sock; + delete dmonstate_sock; dmonitoring_free(&dmonitoringmodel); return 0; diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index 58464b53..4f1f3456 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -23,6 +23,8 @@ void dmonitoring_init(DMonitoringModelState* s) { const char* model_path = "../../models/dmonitoring_model.dlc"; #endif 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) { @@ -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_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++) { 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); diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 80aae5cd..dc5d116e 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -14,6 +14,7 @@ extern "C" { #endif #define OUTPUT_SIZE 33 +#define RHD_CHECK_INTERVAL 10 typedef struct DMonitoringResult { float face_orientation[3]; @@ -29,6 +30,8 @@ typedef struct DMonitoringResult { typedef struct DMonitoringModelState { RunModel *m; + bool is_rhd; + bool is_rhd_checked; float output[OUTPUT_SIZE]; } DMonitoringModelState; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 70ee41a9..c6b5354a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -ea2820c0708a95e8f392d503c7b76090ade380b0 \ No newline at end of file +3523742130b9e0554bab4ac5bc5ab535f1342e90 \ No newline at end of file diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index db03db93..4cb2174a 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -339,8 +339,9 @@ def thermald_thread(): # don't start while taking snapshot if not should_start_prev: + is_viewing_driver = params.get("IsDriverViewEnabled") == 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: params.delete("Offroad_PandaFirmwareMismatch") diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 02f8d32f..0f1d54e0 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -279,7 +279,6 @@ static void draw_steering(UIState *s, float curvature) { static void draw_frame(UIState *s) { const UIScene *scene = &s->scene; - float x1, x2, y1, y2; if (s->scene.frontview) { glBindVertexArray(s->frame_vao[1]); } 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); } +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) { const UIScene *scene = &s->scene; int ui_viz_rx = scene->ui_viz_rx; @@ -828,14 +921,18 @@ static void ui_draw_vision(UIState *s) { nvgRestore(s->vg); // 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) { // Controls Alerts ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2); } else { - ui_draw_vision_footer(s); + if (!scene->frontview){ui_draw_vision_footer(s);} } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index f3e62e1f..a5a12de6 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -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) { if (s->started && (touch_x >= s->scene.ui_viz_rx - bdr_s) && (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); } } @@ -226,6 +234,8 @@ static void ui_init(UIState *s) { s->thermal_sock = SubSocket::create(s->ctx, "thermal"); s->health_sock = SubSocket::create(s->ctx, "health"); 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"); assert(s->model_sock != NULL); @@ -236,6 +246,8 @@ static void ui_init(UIState *s) { assert(s->thermal_sock != NULL); assert(s->health_sock != NULL); assert(s->ubloxgnss_sock != NULL); + assert(s->driverstate_sock != NULL); + assert(s->dmonitoring_sock != NULL); assert(s->offroad_sock != NULL); s->poller = Poller::create({ @@ -246,7 +258,9 @@ static void ui_init(UIState *s) { s->radarstate_sock, s->thermal_sock, s->health_sock, - s->ubloxgnss_sock + s->ubloxgnss_sock, + s->driverstate_sock, + s->dmonitoring_sock }); #ifdef SHOW_SPEEDLIMIT @@ -388,7 +402,8 @@ void handle_message(UIState *s, Message * msg) { cereal_read_ControlsState(&datad, eventd.controlsState); 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) { 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.monitoring_active = datad.driverMonitoringOn; - s->scene.frontview = datad.rearViewCam; - s->scene.decel_for_model = datad.decelForModel; 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 { s->scene.alert_text2[0] = '\0'; } - s->scene.awareness_status = datad.awarenessStatus; s->scene.alert_ts = eventd.logMonoTime; @@ -543,24 +555,8 @@ void handle_message(UIState *s, Message * msg) { s->scene.thermalStatus = datad.thermalStatus; 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) { struct cereal_UbloxGnss datad; cereal_read_UbloxGnss(&datad, eventd.ubloxGnss); @@ -575,7 +571,43 @@ void handle_message(UIState *s, Message * msg) { s->scene.hwType = datad.hwType; 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); } diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 7da04423..941b9a8d 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -69,6 +69,7 @@ const int box_y = bdr_s; const int box_w = vwp_w-sbr_w-(bdr_s*2); const int box_h = vwp_h-(bdr_s*2); const int viz_w = vwp_w-(bdr_s*2); +const int ff_xoffset = 32; const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; @@ -140,6 +141,10 @@ typedef struct UIScene { int lead_status2; 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; uint64_t alert_ts; @@ -216,6 +221,8 @@ typedef struct UIState { SubSocket *thermal_sock; SubSocket *health_sock; SubSocket *ubloxgnss_sock; + SubSocket *driverstate_sock; + SubSocket *dmonitoring_sock; PubSocket *offroad_sock; Poller * poller; Poller * ublox_poller; @@ -281,6 +288,7 @@ typedef struct UIState { float alert_blinking_alpha; bool alert_blinked; bool started; + bool thermal_started, preview_started; bool vision_seen; float light_sensor;