2020-11-21, 2020-11-23, 2020-11-24 changes in master-ci

selfdrive/modeld/SConscript

@@ -19,7 +19,11 @@ if arch == "aarch64":
     lenv['CFLAGS'].append("-DUSE_THNEED")
     lenv['CXXFLAGS'].append("-DUSE_THNEED")
 elif arch == "larch64":
-  libs += ['gsl', 'CB', 'pthread']
+  libs += ['gsl', 'CB', 'pthread', 'dl']
+  if not TEST_THNEED:
+    common_src += ["thneed/thneed.cc"]
+    lenv['CFLAGS'].append("-DUSE_THNEED")
+    lenv['CXXFLAGS'].append("-DUSE_THNEED")
 else:
   libs += ['pthread']
 

selfdrive/modeld/dmonitoringmodeld.cc

@@ -3,6 +3,7 @@
 #include <unistd.h>
 #include <signal.h>
 #include <cassert>
+#include <sys/resource.h>
 
 #include "common/visionbuf.h"
 #include "common/visionipc.h"
@@ -23,7 +24,7 @@ static void set_do_exit(int sig) {
 
 int main(int argc, char **argv) {
   int err;
-  set_realtime_priority(51);
+  setpriority(PRIO_PROCESS, 0, -15);
 
 #ifdef QCOM2
   set_core_affinity(5);
@@ -65,7 +66,7 @@ int main(int argc, char **argv) {
       double t2 = millis_since_boot();
 
       // send dm packet
-      dmonitoring_publish(pm, extra.frame_id, res);
+      dmonitoring_publish(pm, extra.frame_id, res, (t2-t1)/1000.0);
 
       LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
       last = t1;

selfdrive/modeld/models/dmonitoring.cc

@@ -114,24 +114,39 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
                     resized_width, resized_height,
                     mode);
 
+  // prerotate to be cache aware
+  uint8_t *resized_buf_rot = get_buffer(s->resized_buf_rot, resized_width*resized_height*3/2);
+  uint8_t *resized_y_buf_rot = resized_buf_rot;
+  uint8_t *resized_u_buf_rot = resized_y_buf_rot + (resized_width * resized_height);
+  uint8_t *resized_v_buf_rot = resized_u_buf_rot + ((resized_width/2) * (resized_height/2));
+
+  libyuv::I420Rotate(resized_y_buf, resized_width,
+                     resized_u_buf, resized_width/2,
+                     resized_v_buf, resized_width/2,
+                     resized_y_buf_rot, resized_height,
+                     resized_u_buf_rot, resized_height/2,
+                     resized_v_buf_rot, resized_height/2,
+                     // negative height causes a vertical flip to match previous
+                     resized_width, -resized_height, libyuv::kRotate90);
+
   int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
   float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
   // one shot conversion, O(n) anyway
   // yuvframe2tensor, normalize
-  for (int r = 0; r < MODEL_HEIGHT/2; r++) {
-    for (int c = 0; c < MODEL_WIDTH/2; c++) {
+  for (int c = 0; c < MODEL_WIDTH/2; c++) {
+    for (int r = 0; r < MODEL_HEIGHT/2; r++) {
       // Y_ul
-      net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]);
-      // Y_ur
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c)*resized_height]);
       // Y_dl
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c)*resized_height]);
+      // Y_ur
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c+1)*resized_height]);
       // Y_dr
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c+1)*resized_height]);
       // U
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + r + (c*resized_height/2)]);
       // V
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]);
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + r + (c*resized_height/2)]);
     }
   }
 
@@ -140,6 +155,10 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   //fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file);
   //fclose(dump_yuv_file);
 
+  // *** testing ***
+  // idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320)
+  // imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420))
+
   //FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb");
   //fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2);
   //fclose(dump_yuv_file2);
@@ -165,11 +184,12 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   return ret;
 }
 
-void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res){
+void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time){
   // make msg
   MessageBuilder msg;
   auto framed = msg.initEvent().initDriverState();
   framed.setFrameId(frame_id);
+  framed.setModelExecutionTime(execution_time);
 
   kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
   kj::ArrayPtr<const float> face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta));

selfdrive/modeld/runners/snpemodel.cc

@@ -1,6 +1,7 @@
 #pragma clang diagnostic ignored "-Wexceptions"
 
 #include <cassert>
+#include <string.h>
 #include <stdlib.h>
 #include "common/util.h"
 #include "snpemodel.h"

selfdrive/modeld/thneed/thneed.cc

@@ -1,10 +1,11 @@
-#include "thneed.h"
 #include <cassert>
 #include <sys/mman.h>
 #include <dlfcn.h>
 #include <map>
 #include <string>
+#include <string.h>
 #include <errno.h>
+#include "thneed.h"
 
 Thneed *g_thneed = NULL;
 int g_fd = -1;
@@ -31,6 +32,7 @@ extern "C" {
 int (*my_ioctl)(int filedes, unsigned long request, void *argp) = NULL;
 #undef ioctl
 int ioctl(int filedes, unsigned long request, void *argp) {
+  request &= 0xFFFFFFFF;  // needed on QCOM2
   if (my_ioctl == NULL) my_ioctl = reinterpret_cast<decltype(my_ioctl)>(dlsym(RTLD_NEXT, "ioctl"));
   Thneed *thneed = g_thneed;
 
@@ -440,7 +442,14 @@ cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, c
 #endif
 
 void *dlsym(void *handle, const char *symbol) {
+  // TODO: Find dlsym in a better way. Currently this is hand looked up in libdl.so
+#if defined QCOM
   void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen-0x2d4);
+#elif defined QCOM2
+  void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen+0x138);
+#else
+  #error "Unsupported platform for thneed"
+#endif
   if (memcmp("REAL_", symbol, 5) == 0) {
     return my_dlsym(handle, symbol+5);
   } else if (strcmp("clEnqueueNDRangeKernel", symbol) == 0) {

selfdrive/modeld/thneed/thneed.h

@@ -1,5 +1,10 @@
 #pragma once
 
+#ifndef __user
+  #define __user __attribute__(())
+#endif
+
+#include <stdlib.h>
 #include <stdint.h>
 #include "include/msm_kgsl.h"
 #include <vector>