RHD support for driver monitoring (#1299)

old-commit-hash: 133b1a20b4

apk/ai.comma.plus.offroad.apk

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:b6d3698d82870268439609527eea6b350dc65f5ddacbb12120ba2ef5351c2dc4
+oid sha256:e98a8d4a73ee84ace6fe545d1f98176ede04cc31d7b62c9275ee653d5e0f4d2b
-size 13691663
+size 13732603

apks

@@ -1 +1 @@
-Subproject commit e4bf49bc65e8e680e5afc629240b4ebbcedc4ebd
+Subproject commit f5d2c1715c9482d898062110ce4c612093aa5d4f

cereal

@@ -1 +1 @@
-Subproject commit c8e5db830d30e5ea0d0abef0479d9c48cfd24504
+Subproject commit 442e914d73b163823fceb91c2b57062965fad045

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],

models/dmonitoring_model.current

@@ -1 +1 @@
-2ad118ee-df4f-49ff-a374-6dd693cee37b
+43221d85-46fd-40b9-bff0-2b1b18a86b07

models/dmonitoring_model.keras

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:72ebd64de360c9e6fabb8f45b03644d6b4405ded318f656d9e71e67b6c598513
+oid sha256:5c39a2096f7058541b5339ec36bc4c468955e67285078080ed6d8802fed06c1d
-size 813336
+size 814176

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

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/*
 

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<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);
     if (msg != NULL) {
       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
       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_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;
@@ -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_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,};
@@ -336,6 +357,7 @@ void* frontview_thread(void *arg) {
   }
 
   delete monitoring_sock;
+  delete dmonstate_sock;
 
   return NULL;
 }

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)
 

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_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_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 \
+                          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
     # first order filters

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()

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

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<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();
 
@@ -74,6 +94,7 @@ int main(int argc, char **argv) {
   visionstream_destroy(&stream);
 
   delete dmonitoring_sock;
+  delete dmonstate_sock;
   dmonitoring_free(&dmonitoringmodel);
 
   return 0;

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);

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;
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-ea2820c0708a95e8f392d503c7b76090ade380b0
+3523742130b9e0554bab4ac5bc5ab535f1342e90

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")

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);}
   }
 
 

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);
 }
 

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;