openpilot release

selfdrive/common/framebuffer.cc

@@ -0,0 +1,135 @@
+
+#include <cstdio>
+#include <cstdlib>
+#include <cassert>
+
+#include <ui/DisplayInfo.h>
+
+#include <gui/ISurfaceComposer.h>
+#include <gui/Surface.h>
+#include <gui/SurfaceComposerClient.h>
+
+
+#include <GLES2/gl2.h>
+#include <EGL/eglext.h>
+
+#define BACKLIGHT_CONTROL "/sys/class/leds/lcd-backlight/brightness"
+#define BACKLIGHT_LEVEL "205"
+
+using namespace android;
+
+struct FramebufferState {
+    sp<SurfaceComposerClient> session;
+    sp<IBinder> dtoken;
+    DisplayInfo dinfo;
+    sp<SurfaceControl> control;
+
+    sp<Surface> s;
+    EGLDisplay display;
+
+    EGLint egl_major, egl_minor;
+    EGLConfig config;
+    EGLSurface surface;
+    EGLContext context;
+};
+
+extern "C" FramebufferState* framebuffer_init(
+    const char* name, int32_t layer,
+    EGLDisplay *out_display, EGLSurface *out_surface,
+    int *out_w, int *out_h) {
+  status_t status;
+  int success;
+
+  FramebufferState *s = new FramebufferState;
+
+  s->session = new SurfaceComposerClient();
+  assert(s->session != NULL);
+
+  s->dtoken = SurfaceComposerClient::getBuiltInDisplay(
+                ISurfaceComposer::eDisplayIdMain);
+  assert(s->dtoken != NULL);
+
+  status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
+  assert(status == 0);
+
+  int orientation = 3; // rotate framebuffer 270 degrees
+  if(orientation == 1 || orientation == 3) {
+      int temp = s->dinfo.h;
+      s->dinfo.h = s->dinfo.w;
+      s->dinfo.w = temp;
+  }
+
+  printf("dinfo %dx%d\n", s->dinfo.w, s->dinfo.h);
+
+  Rect destRect(s->dinfo.w, s->dinfo.h);
+  s->session->setDisplayProjection(s->dtoken, orientation, destRect, destRect);
+
+  s->control = s->session->createSurface(String8(name),
+                  s->dinfo.w, s->dinfo.h, PIXEL_FORMAT_RGBX_8888);
+  assert(s->control != NULL);
+
+  SurfaceComposerClient::openGlobalTransaction();
+  status = s->control->setLayer(layer);
+  SurfaceComposerClient::closeGlobalTransaction();
+  assert(status == 0);
+
+  s->s = s->control->getSurface();
+  assert(s->s != NULL);
+
+  // init opengl and egl
+  const EGLint attribs[] = {
+    EGL_RED_SIZE,     8,
+    EGL_GREEN_SIZE,   8,
+    EGL_BLUE_SIZE,    8,
+    EGL_DEPTH_SIZE,   0,
+    EGL_STENCIL_SIZE, 8,
+    EGL_RENDERABLE_TYPE, EGL_OPENGL_ES3_BIT_KHR,
+    EGL_NONE,
+  };
+
+  s->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+  assert(s->display != EGL_NO_DISPLAY);
+
+  success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
+  assert(success);
+
+  printf("egl version %d.%d\n", s->egl_major, s->egl_minor);
+  
+  EGLint num_configs;
+  success = eglChooseConfig(s->display, attribs, &s->config, 1, &num_configs);
+  assert(success);
+
+  s->surface = eglCreateWindowSurface(s->display, s->config, s->s.get(), NULL);
+  assert(s->surface != EGL_NO_SURFACE);
+
+  const EGLint context_attribs[] = {
+    EGL_CONTEXT_CLIENT_VERSION, 3,
+    EGL_NONE,
+  };
+  s->context = eglCreateContext(s->display, s->config, NULL, context_attribs);
+  assert(s->context != EGL_NO_CONTEXT);
+
+  EGLint w, h;
+  eglQuerySurface(s->display, s->surface, EGL_WIDTH, &w);
+  eglQuerySurface(s->display, s->surface, EGL_HEIGHT, &h);
+  printf("egl w %d h %d\n", w, h);
+
+  success = eglMakeCurrent(s->display, s->surface, s->surface, s->context);
+  assert(success);
+
+  printf("gl version %s\n", glGetString(GL_VERSION));
+
+
+  // set brightness
+  int brightness_fd = open(BACKLIGHT_CONTROL, O_RDWR);
+  const char brightness_level[] = BACKLIGHT_LEVEL;
+  write(brightness_fd, brightness_level, strlen(brightness_level));
+
+
+  if (out_display) *out_display = s->display;
+  if (out_surface) *out_surface = s->surface;
+  if (out_w) *out_w = w;
+  if (out_h) *out_h = h;
+
+  return s;
+}

selfdrive/common/framebuffer.h

@@ -0,0 +1,21 @@
+#ifndef FRAMEBUFFER_H
+#define FRAMEBUFFER_H
+
+#include <EGL/eglext.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct FramebufferState FramebufferState;
+
+FramebufferState* framebuffer_init(
+    const char* name, int32_t layer,
+    EGLDisplay *out_display, EGLSurface *out_surface,
+    int *out_w, int *out_h);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/mat.h

@@ -0,0 +1,68 @@
+#ifndef COMMON_MAT_H
+#define COMMON_MAT_H
+
+typedef struct vec3 {
+	float v[3];
+} vec3;
+
+typedef struct vec4 {
+  float v[4];
+} vec4;
+
+typedef struct mat3 {
+	float v[3*3];
+} mat3;
+
+typedef struct mat4 {
+  float v[4*4];
+} mat4;
+
+static inline mat3 matmul3(const mat3 a, const mat3 b) {
+  mat3 ret = {{0.0}};
+  for (int r=0; r<3; r++) {
+    for (int c=0; c<3; c++) {
+      float v = 0.0;
+      for (int k=0; k<3; k++) {
+        v += a.v[r*3+k] * b.v[k*3+c];
+      }
+      ret.v[r*3+c] = v;
+    }
+  }
+  return ret;
+}
+
+static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
+  vec3 ret = {{0.0}};
+  for (int r=0; r<3; r++) {
+    for (int c=0; c<3; c++) {
+      ret.v[r] += a.v[r*3+c] * b.v[c];
+    }
+  }
+  return ret;
+}
+
+static inline mat4 matmul(const mat4 a, const mat4 b) {
+  mat4 ret = {{0.0}};
+  for (int r=0; r<4; r++) {
+    for (int c=0; c<4; c++) {
+      float v = 0.0;
+      for (int k=0; k<4; k++) {
+        v += a.v[r*4+k] * b.v[k*4+c];
+      }
+      ret.v[r*4+c] = v;
+    }
+  }
+  return ret;
+}
+
+static inline vec4 matvecmul(const mat4 a, const vec4 b) {
+  vec4 ret = {{0.0}};
+  for (int r=0; r<4; r++) {
+    for (int c=0; c<4; c++) {
+      ret.v[r] += a.v[r*4+c] * b.v[c];
+    }
+  }
+  return ret;
+}
+
+#endif

selfdrive/common/modeldata.h

@@ -0,0 +1,23 @@
+#ifndef MODELDATA_H
+#define MODELDATA_H
+
+typedef struct PathData {
+  float points[50];
+  float prob;
+  float std;
+} PathData;
+
+typedef struct LeadData {
+  float dist;
+  float prob;
+  float std;
+} LeadData;
+
+typedef struct ModelData {
+  PathData path;
+  PathData left_lane;
+  PathData right_lane;
+  LeadData lead;
+} ModelData;
+
+#endif

selfdrive/common/mutex.h

@@ -0,0 +1,13 @@
+#ifndef COMMON_MUTEX_H
+#define COMMON_MUTEX_H
+
+#include <pthread.h>
+
+static inline void mutex_init_reentrant(pthread_mutex_t *mutex) {
+  pthread_mutexattr_t attr;
+  pthread_mutexattr_init(&attr);
+  pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+  pthread_mutex_init(mutex, &attr);
+}
+
+#endif

selfdrive/common/swaglog.c

@@ -0,0 +1,90 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+
+#include <pthread.h>
+#include <zmq.h>
+#include <json.h>
+
+#include "common/timing.h"
+
+#include "swaglog.h"
+
+typedef struct LogState {
+  pthread_mutex_t lock;
+  bool inited;
+  JsonNode *ctx_j;
+  void *zctx;
+  void *sock;
+} LogState;
+
+static LogState s = {
+  .lock = PTHREAD_MUTEX_INITIALIZER,
+};
+
+static void cloudlog_init() {
+  if (s.inited) return;
+  s.ctx_j = json_mkobject();
+  s.zctx = zmq_ctx_new();
+  s.sock = zmq_socket(s.zctx, ZMQ_PUSH);
+  zmq_connect(s.sock, "ipc:///tmp/logmessage");
+  s.inited = true;
+}
+
+void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* srctime, 
+                const char* fmt, ...) {
+  pthread_mutex_lock(&s.lock);
+  cloudlog_init();
+
+  char* msg_buf = NULL;
+  va_list args;
+  va_start(args, fmt);
+  vasprintf(&msg_buf, fmt, args);
+  va_end(args);
+
+  if (!msg_buf) {
+    pthread_mutex_unlock(&s.lock);
+    return;
+  }
+
+  if (levelnum >= CLOUDLOG_PRINT_LEVEL) {
+    printf("%s: %s\n", filename, msg_buf);
+  }
+
+  JsonNode *log_j = json_mkobject();
+  assert(log_j);
+
+  json_append_member(log_j, "msg", json_mkstring(msg_buf));
+  json_append_member(log_j, "ctx", s.ctx_j);
+  json_append_member(log_j, "levelnum", json_mknumber(levelnum));
+  json_append_member(log_j, "filename", json_mkstring(filename));
+  json_append_member(log_j, "lineno", json_mknumber(lineno));
+  json_append_member(log_j, "funcname", json_mkstring(func));
+  json_append_member(log_j, "srctime", json_mkstring(srctime));
+  json_append_member(log_j, "created", json_mknumber(seconds_since_epoch()));
+
+  char* log_s = json_encode(log_j);
+  assert(log_s);
+
+  json_remove_from_parent(s.ctx_j);  
+
+  json_delete(log_j);
+  free(msg_buf);
+
+  char levelnum_c = levelnum;
+  zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE);
+  zmq_send(s.sock, log_s, strlen(log_s), ZMQ_NOBLOCK);
+  free(log_s);
+
+  pthread_mutex_unlock(&s.lock);
+}
+
+void cloudlog_bind(const char* k, const char* v) {
+  pthread_mutex_lock(&s.lock);
+  cloudlog_init();
+  json_append_member(s.ctx_j, k, json_mkstring(v));
+  pthread_mutex_unlock(&s.lock);
+}

selfdrive/common/swaglog.h

@@ -0,0 +1,26 @@
+#ifndef SWAGLOG_H
+#define SWAGLOG_H
+
+#define CLOUDLOG_DEBUG 10
+#define CLOUDLOG_INFO 20
+#define CLOUDLOG_WARNING 30
+#define CLOUDLOG_ERROR 40
+#define CLOUDLOG_CRITICAL 50
+
+#define CLOUDLOG_PRINT_LEVEL CLOUDLOG_WARNING
+
+void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* srctime, 
+                const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+
+void cloudlog_bind(const char* k, const char* v);
+
+#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
+                                           __func__, __DATE__ " " __TIME__, \
+                                           fmt, ## __VA_ARGS__)
+
+#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
+#endif

selfdrive/common/timing.h

@@ -0,0 +1,31 @@
+#ifndef COMMON_TIMING_H
+#define COMMON_TIMING_H
+
+#include <stdint.h>
+#include <time.h>
+
+static inline uint64_t nanos_since_boot() {
+  struct timespec t;
+  clock_gettime(CLOCK_BOOTTIME, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline double millis_since_boot() {
+  struct timespec t;
+  clock_gettime(CLOCK_BOOTTIME, &t);
+  return t.tv_sec * 1000.0 + t.tv_nsec / 1000000.0;
+}
+
+static inline uint64_t nanos_since_epoch() {
+  struct timespec t;
+  clock_gettime(CLOCK_REALTIME, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline double seconds_since_epoch() {
+  struct timespec t;
+  clock_gettime(CLOCK_REALTIME, &t);
+  return (double)t.tv_sec + t.tv_nsec / 1000000000.0;
+}
+
+#endif

selfdrive/common/util.h

@@ -0,0 +1,22 @@
+#ifndef COMMON_UTIL_H
+#define COMMON_UTIL_H
+
+#define min(a,b) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+   _a < _b ? _a : _b; })
+
+#define max(a,b) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+   _a > _b ? _a : _b; })
+
+#define clamp(a,b,c) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+     __typeof__ (c) _c = (c); \
+   _a < _b ? _b : (_a > _c ? _c : _a); })
+
+#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
+
+#endif

selfdrive/common/visionipc.c

@@ -0,0 +1,127 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <assert.h>
+#include <errno.h>
+
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include "visionipc.h"
+
+typedef struct VisionPacketWire {
+  int type;
+  VisionPacketData d;
+} VisionPacketWire;
+
+int vipc_connect() {
+  int err;
+
+  int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
+  assert(sock >= 0);
+  struct sockaddr_un addr = {
+    .sun_family = AF_UNIX,
+    .sun_path = VIPC_SOCKET_PATH,
+  };
+  err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
+  if (err != 0) {
+    close(sock);
+    return -1;
+  }
+
+  return sock;
+}
+
+static int sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
+                             int *out_num_fds) {
+  int err;
+
+  char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
+  memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
+
+  struct iovec iov = {
+    .iov_base = buf,
+    .iov_len = buf_size,
+  };
+  struct msghdr msg = {
+    .msg_iov = &iov,
+    .msg_iovlen = 1,
+  };
+
+  if (num_fds > 0) {
+    assert(fds);
+
+    msg.msg_control = control_buf;
+    msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
+  }
+
+  if (send) {
+    if (num_fds) {
+      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
+      assert(cmsg);
+      cmsg->cmsg_level = SOL_SOCKET;
+      cmsg->cmsg_type = SCM_RIGHTS;
+      cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
+      memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
+      // printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
+    }
+    return sendmsg(fd, &msg, 0);
+  } else {
+    int r = recvmsg(fd, &msg, 0);
+    if (r < 0) return r;
+
+    int recv_fds = 0;
+    if (msg.msg_controllen > 0) {
+      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
+      assert(cmsg);
+      assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
+      recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
+      assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
+      recv_fds /= sizeof(int);
+      // printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
+      // assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
+
+      assert(fds && recv_fds <= num_fds);
+      memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
+    }
+
+    if (msg.msg_flags) {
+      for (int i=0; i<recv_fds; i++) {
+        close(fds[i]);
+      }
+      return -1;
+    }
+
+    if (fds) {
+      assert(out_num_fds);
+      *out_num_fds = recv_fds;
+    }
+    return r;
+  }
+}
+
+int vipc_recv(int fd, VisionPacket *out_p) {
+  VisionPacketWire p = {0};
+  VisionPacket p2 = {0};
+  int ret = sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
+  if (ret < 0) {
+    printf("vipc_recv err: %s\n", strerror(errno));
+  } else {
+    p2.type = p.type;
+    p2.d = p.d;
+    *out_p = p2;
+  }
+  return ret;
+}
+
+int vipc_send(int fd, const VisionPacket p2) {
+  assert(p2.num_fds <= VIPC_MAX_FDS);
+
+  VisionPacketWire p = {
+    .type = p2.type,
+    .d = p2.d,
+  };
+  return sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2.fds, p2.num_fds, NULL);
+}
+

selfdrive/common/visionipc.h

@@ -0,0 +1,50 @@
+#ifndef VISIONIPC_H
+#define VISIONIPC_H
+
+#define VIPC_SOCKET_PATH "/tmp/vision_socket"
+#define VIPC_MAX_FDS 64
+
+
+#define VISION_INVALID 0
+#define VISION_UI_SUBSCRIBE 1
+#define VISION_UI_BUFS 2
+#define VISION_UI_ACQUIRE 3
+#define VISION_UI_RELEASE 4
+
+typedef struct VisionUIBufs {
+  int width, height, stride;
+  int front_width, front_height, front_stride;
+
+  int big_box_x, big_box_y;
+  int big_box_width, big_box_height;
+  int transformed_width, transformed_height;
+
+  int front_box_x, front_box_y;
+  int front_box_width, front_box_height;
+
+  size_t buf_len;
+  int num_bufs;
+  size_t front_buf_len;
+  int num_front_bufs;
+} VisionUIBufs;
+
+typedef union VisionPacketData {
+  VisionUIBufs ui_bufs;
+  struct {
+    bool front;
+    int idx;
+  } ui_acq, ui_rel;
+} VisionPacketData;
+
+typedef struct VisionPacket {
+  int type;
+  VisionPacketData d;
+  int num_fds;
+  int fds[VIPC_MAX_FDS];
+} VisionPacket;
+
+int vipc_connect();
+int vipc_recv(int fd, VisionPacket *out_p);
+int vipc_send(int fd, const VisionPacket p);
+
+#endif