Navd Intergration

SConstruct

@@ -28,7 +28,12 @@ AddOption('--ubsan',
           action='store_true',
           help='turn on UBSan')
 
+AddOption('--clazy',
+          action='store_true',
+          help='build with clazy')
+
 SHARED = False
+real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
 
 lenv = {
   "PATH": os.environ['PATH'] + ":" + Dir(f"#libs/capnpc-java/{arch}/bin").abspath,
@@ -42,6 +47,7 @@ lenv = {
 
 libpath = [
       f"#libs/acados/{arch}/lib",
+      f'#libs/mapbox-gl-native-qt/x86_64/{arch}'
       ]
 
 cflags = []
@@ -93,6 +99,7 @@ env = Environment(
     "#cereal",
     "#libs",
     "#opendbc/can/",
+    "#libs/mapbox-gl-native-qt/x86_64",
     "#common",
     "#selfdrive/boardd",
     "#third_party",
@@ -107,6 +114,7 @@ env = Environment(
     "#libs/acados/include",
     "#libs/acados/include/blasfeo/include",
     "#libs/acados/include/hpipm/include",
+    "#libs/mapbox-gl-native-qt/include",
     "#cereal",
     "#opendbc/can",
     "#common",
@@ -134,6 +142,72 @@ else:
 
 Export('envCython')
 
+# # Qt build environment
+qt_env = env.Clone()
+qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus"]
+
+qt_libs = []
+if arch == "Darwin":
+  if real_arch == "arm64":
+    qt_env['QTDIR'] = "/opt/homebrew/opt/qt@5"
+  else:
+    qt_env['QTDIR'] = "/usr/local/opt/qt@5"
+  qt_dirs = [
+    os.path.join(qt_env['QTDIR'], "include"),
+  ]
+  qt_dirs += [f"{qt_env['QTDIR']}/include/Qt{m}" for m in qt_modules]
+  qt_env["LINKFLAGS"] += ["-F" + os.path.join(qt_env['QTDIR'], "lib")]
+  qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"]
+  qt_env.AppendENVPath('PATH', os.path.join(qt_env['QTDIR'], "bin"))
+else:
+  qt_install_prefix = subprocess.check_output(['qmake', '-query', 'QT_INSTALL_PREFIX'], encoding='utf8').strip()
+  qt_install_headers = subprocess.check_output(['qmake', '-query', 'QT_INSTALL_HEADERS'], encoding='utf8').strip()
+
+  qt_env['QTDIR'] = qt_install_prefix
+  qt_dirs = [
+    f"{qt_install_headers}",
+    f"{qt_install_headers}/QtGui/5.12.8/QtGui",
+  ]
+  qt_dirs += [f"{qt_install_headers}/Qt{m}" for m in qt_modules]
+
+  qt_libs = [f"Qt5{m}" for m in qt_modules]
+  if arch == "larch64":
+    qt_libs += ["GLESv2", "wayland-client"]
+  elif arch != "Darwin":
+    qt_libs += ["GL"]
+
+qt_env.Tool('qt')
+qt_env['CPPPATH'] += qt_dirs + ["#selfdrive/ui/qt/"]
+qt_flags = [
+  "-D_REENTRANT",
+  "-DQT_NO_DEBUG",
+  "-DQT_WIDGETS_LIB",
+  "-DQT_GUI_LIB",
+  "-DQT_QUICK_LIB",
+  "-DQT_QUICKWIDGETS_LIB",
+  "-DQT_QML_LIB",
+  "-DQT_CORE_LIB",
+  "-DQT_MESSAGELOGCONTEXT",
+]
+qt_flags
+qt_env['CXXFLAGS'] += qt_flags
+qt_env['LIBPATH'] += ['#selfdrive/ui']
+qt_env['LIBS'] = qt_libs
+# qt_env['QT_MOCHPREFIX'] = cache_dir + '/moc_files/moc_'
+
+if GetOption("clazy"):
+  checks = [
+    "level0",
+    "level1",
+    "no-range-loop",
+    "no-non-pod-global-static",
+  ]
+  qt_env['CXX'] = 'clazy'
+  qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
+  qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
+
+Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED')
+
 QCOM_REPLAY = False
 Export('env', 'arch', 'QCOM_REPLAY', 'SHARED')
 
@@ -221,6 +295,8 @@ SConscript(['selfdrive/controls/lib/long_mpc_lib/SConscript'])
 SConscript(['selfdrive/locationd/SConscript'])
 SConscript(['selfdrive/boardd/SConscript'])
 SConscript(['selfdrive/loggerd/SConscript'])
+SConscript(['selfdrive/ui/SConscript'])
+SConscript(['selfdrive/navd/SConscript'])
 
 if GetOption('test'):
   SConscript('panda/tests/safety/SConscript')

common/mat.h

@@ -0,0 +1,85 @@
+#pragma once
+
+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;
+}
+
+// scales the input and output space of a transformation matrix
+// that assumes pixel-center origin.
+static inline mat3 transform_scale_buffer(const mat3 &in, float s) {
+  // in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
+
+  mat3 transform_out = {{
+    1.0f/s, 0.0f, 0.5f,
+    0.0f, 1.0f/s, 0.5f,
+    0.0f, 0.0f, 1.0f,
+  }};
+
+  mat3 transform_in = {{
+    s,  0.0f, -0.5f*s,
+    0.0f, s, -0.5f*s,
+    0.0f, 0.0f, 1.0f,
+  }};
+
+  return matmul3(transform_in, matmul3(in, transform_out));
+}

common/modeldata.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <array>
+#include "common/mat.h"
+#include "system/hardware/hw.h"
+
+const int TRAJECTORY_SIZE = 33;
+const int LAT_MPC_N = 16;
+const int LON_MPC_N = 32;
+const float MIN_DRAW_DISTANCE = 10.0;
+const float MAX_DRAW_DISTANCE = 100.0;
+
+template <typename T, size_t size>
+constexpr std::array<T, size> build_idxs(float max_val) {
+  std::array<T, size> result{};
+  for (int i = 0; i < size; ++i) {
+    result[i] = max_val * ((i / (double)(size - 1)) * (i / (double)(size - 1)));
+  }
+  return result;
+}
+
+constexpr auto T_IDXS = build_idxs<double, TRAJECTORY_SIZE>(10.0);
+constexpr auto T_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(10.0);
+constexpr auto X_IDXS = build_idxs<double, TRAJECTORY_SIZE>(192.0);
+constexpr auto X_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(192.0);
+
+const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2,
+                                           0.0, 2648.0, 1208.0 / 2,
+                                           0.0, 0.0, 1.0}};
+
+// tici ecam focal probably wrong? magnification is not consistent across frame
+// Need to retrain model before this can be changed
+const mat3 ecam_intrinsic_matrix = (mat3){{567.0, 0.0, 1928.0 / 2,
+                                           0.0, 567.0, 1208.0 / 2,
+                                           0.0, 0.0, 1.0}};
+
+static inline mat3 get_model_yuv_transform() {
+  float db_s = 1.0;
+  const mat3 transform = (mat3){{
+    1.0, 0.0, 0.0,
+    0.0, 1.0, 0.0,
+    0.0, 0.0, 1.0
+  }};
+  // Can this be removed since scale is 1?
+  return transform_scale_buffer(transform, db_s);
+}

get_dependencies.sh

@@ -5,14 +5,18 @@ pip install Cython==0.29.32
 sudo apt-get install -y rsync clang capnproto libcapnp-dev libzmq3-dev cmake libjson11-1 libjson11-1-dev liblmdb-dev libusb-1.0-0-dev
 sudo apt-get install -y dfu-util gcc-arm-none-eabi libcurl4-openssl-dev libssl-dev
 
+SCRIPT=$(realpath "$0")
+DIR=$(dirname "$SCRIPT")
+
 # install capnpc-java
 if ! command -v capnpc-java --version &> /dev/null  # TODO: Running through scons misses this
 then
-    SCRIPT=$(realpath "$0")
-    DIR=$(dirname "$SCRIPT")
     sh $DIR/libs/capnpc-java/build.sh
 fi
 
+# install mapbox-gl-native-qt
+sh $DIR/libs/mapbox-gl-native-qt/build.sh
+
 # pycapnp without wheel build can fail on some systems, in this case, its built from scratch later in the process.
 pip install pycapnp==1.0.0 --install-option="--force-system-libcapnp" > /dev/null 2>&1
 pip install -r requirements.txt

libs/mapbox-gl-native-qt/.gitattributes

@@ -0,0 +1,2 @@
+x86_64 filter=lfs diff=lfs merge=lfs -text
+larch64 filter=lfs diff=lfs merge=lfs -text

libs/mapbox-gl-native-qt/build.sh

@@ -0,0 +1,7 @@
+#!/usr/bin/env sh
+cd /tmp
+git clone --recursive https://github.com/commaai/mapbox-gl-native.git
+cd mapbox-gl-native
+mkdir build && cd build
+cmake -DMBGL_WITH_QT=ON ..
+make -j$(nproc) mbgl-qt

libs/mapbox-gl-native-qt/include/QMapbox

@@ -0,0 +1 @@
+#include "qmapbox.hpp"

libs/mapbox-gl-native-qt/include/QMapboxGL

@@ -0,0 +1 @@
+#include "qmapboxgl.hpp"