dragonpilot beta3

date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295

common/api/__init__.py

@@ -2,8 +2,8 @@ import jwt
 import os
 import requests
 from datetime import datetime, timedelta
-from common.basedir import PERSIST
-from system.version import get_version
+from openpilot.common.basedir import PERSIST
+from openpilot.system.version import get_version
 
 API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
 

common/basedir.py

@@ -1,7 +1,7 @@
 import os
 from pathlib import Path
 
-from system.hardware import PC
+from openpilot.system.hardware import PC
 
 BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
 

common/clock.pyx

@@ -1,24 +0,0 @@
-# distutils: language = c++
-# cython: language_level = 3
-from posix.time cimport clock_gettime, timespec, CLOCK_MONOTONIC_RAW, clockid_t
-
-IF UNAME_SYSNAME == "Darwin":
-  # Darwin doesn't have a CLOCK_BOOTTIME
-  CLOCK_BOOTTIME = CLOCK_MONOTONIC_RAW
-ELSE:
-  from posix.time cimport CLOCK_BOOTTIME
-
-cdef double readclock(clockid_t clock_id):
-  cdef timespec ts
-  cdef double current
-
-  clock_gettime(clock_id, &ts)
-  current = ts.tv_sec + (ts.tv_nsec / 1000000000.)
-  return current
-
-def monotonic_time():
-  return readclock(CLOCK_MONOTONIC_RAW)
-
-def sec_since_boot():
-  return readclock(CLOCK_BOOTTIME)
-

common/clutil.h

@@ -22,6 +22,7 @@
   })
 
 cl_device_id cl_get_device_id(cl_device_type device_type);
+cl_context cl_create_context(cl_device_id device_id);
 cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
 cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
 cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);

common/gpio.h

@@ -5,7 +5,7 @@
   #define GPIO_HUB_RST_N        30
   #define GPIO_UBLOX_RST_N      32
   #define GPIO_UBLOX_SAFEBOOT_N 33
-  #define GPIO_UBLOX_PWR_EN     34
+  #define GPIO_GNSS_PWR_EN      34 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
   #define GPIO_STM_RST_N        124
   #define GPIO_STM_BOOT0        134
   #define GPIO_BMX_ACCEL_INT    21
@@ -17,7 +17,7 @@
   #define GPIO_HUB_RST_N        0
   #define GPIO_UBLOX_RST_N      0
   #define GPIO_UBLOX_SAFEBOOT_N 0
-  #define GPIO_UBLOX_PWR_EN     0
+  #define GPIO_GNSS_PWR_EN      0 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
   #define GPIO_STM_RST_N        0
   #define GPIO_STM_BOOT0        0
   #define GPIO_BMX_ACCEL_INT    0

common/gpio.py

@@ -1,3 +1,4 @@
+import os
 from functools import lru_cache
 from typing import Optional, List
 
@@ -26,6 +27,9 @@ def gpio_read(pin: int) -> Optional[bool]:
   return val
 
 def gpio_export(pin: int) -> None:
+  if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
+    return
+
   try:
     with open("/sys/class/gpio/export", 'w') as f:
       f.write(str(pin))

common/i18n.py

@@ -1,5 +1,5 @@
 import gettext
-from common.params import Params
+from openpilot.common.params import Params
 
 locale_dir = "/data/openpilot/selfdrive/assets/locales"
 # supported_language = ["en-US", "zh-TW", "zh-CN", "ja-JP", "ko-KR"]

common/i2c.h

@@ -1,12 +1,14 @@
 #pragma once
 
 #include <cstdint>
+#include <mutex>
 
 #include <sys/types.h>
 
 class I2CBus {
   private:
     int i2c_fd;
+    std::mutex m;
 
   public:
     I2CBus(uint8_t bus_id);

common/kalman/simple_kalman.py

@@ -1,3 +1,12 @@
-# pylint: skip-file
-from common.kalman.simple_kalman_impl import KF1D as KF1D
+from openpilot.common.kalman.simple_kalman_impl import KF1D as KF1D
 assert KF1D
+import numpy as np
+
+def get_kalman_gain(dt, A, C, Q, R, iterations=100):
+  P = np.zeros_like(Q)
+  for _ in range(iterations):
+    P = A.dot(P).dot(A.T) + dt * Q
+    S = C.dot(P).dot(C.T) + R
+    K = P.dot(C.T).dot(np.linalg.inv(S))
+    P = (np.eye(len(P)) - K.dot(C)).dot(P)
+  return K

common/kalman/tests/test_simple_kalman.py

@@ -3,8 +3,8 @@ import random
 import timeit
 import numpy as np
 
-from common.kalman.simple_kalman import KF1D
-from common.kalman.simple_kalman_old import KF1D as KF1D_old
+from openpilot.common.kalman.simple_kalman import KF1D
+from openpilot.common.kalman.simple_kalman_old import KF1D as KF1D_old
 
 
 class TestSimpleKalman(unittest.TestCase):
@@ -54,8 +54,8 @@ class TestSimpleKalman(unittest.TestCase):
     setup = """
 import numpy as np
 
-from common.kalman.simple_kalman import KF1D
-from common.kalman.simple_kalman_old import KF1D as KF1D_old
+from openpilot.common.kalman.simple_kalman import KF1D
+from openpilot.common.kalman.simple_kalman_old import KF1D as KF1D_old
 
 dt = 0.01
 x0_0 = 0.0

common/logging_extra.py

@@ -197,7 +197,7 @@ class SwagLogger(logging.Logger):
       filename = os.path.normcase(co.co_filename)
 
       # TODO: is this pylint exception correct?
-      if filename == _srcfile:  # pylint: disable=comparison-with-callable
+      if filename == _srcfile:
         f = f.f_back
         continue
       sinfo = None

common/mat.h

@@ -1,7 +1,7 @@
 #pragma once
 
 typedef struct vec3 {
-	float v[3];
+  float v[3];
 } vec3;
 
 typedef struct vec4 {
@@ -9,7 +9,7 @@ typedef struct vec4 {
 } vec4;
 
 typedef struct mat3 {
-	float v[3*3];
+  float v[3*3];
 } mat3;
 
 typedef struct mat4 {

common/params.h

@@ -15,7 +15,11 @@ enum ParamKeyType {
 
 class Params {
 public:
-  Params(const std::string &path = {});
+  explicit Params(const std::string &path = {});
+  // Not copyable.
+  Params(const Params&) = delete;
+  Params& operator=(const Params&) = delete;
+
   std::vector<std::string> allKeys() const;
   bool checkKey(const std::string &key);
   ParamKeyType getKeyType(const std::string &key);

common/params.py

@@ -1,4 +1,5 @@
-from common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, put_bool_nonblocking # pylint: disable=no-name-in-module, import-error
+from openpilot.common.params_pyx import Params, ParamKeyType, UnknownKeyName, put_nonblocking, \
+                                        put_bool_nonblocking
 assert Params
 assert ParamKeyType
 assert UnknownKeyName

common/params_pyx.cpp

@@ -2132,20 +2132,6 @@ static void __pyx_insert_code_object(int code_line, PyCodeObject* code_object);
 static void __Pyx_AddTraceback(const char *funcname, int c_line,
                                int py_line, const char *filename);
 
-/* GCCDiagnostics.proto */
-#if !defined(__INTEL_COMPILER) && defined(__GNUC__) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
-#define __Pyx_HAS_GCC_DIAGNOSTIC
-#endif
-
-/* CIntFromPy.proto */
-static CYTHON_INLINE enum ParamKeyType __Pyx_PyInt_As_enum__ParamKeyType(PyObject *);
-
-/* CIntToPy.proto */
-static CYTHON_INLINE PyObject* __Pyx_PyInt_From_enum__ParamKeyType(enum ParamKeyType value);
-
-/* CIntToPy.proto */
-static CYTHON_INLINE PyObject* __Pyx_PyInt_From_int(int value);
-
 /* CppExceptionConversion.proto */
 #ifndef __Pyx_CppExn2PyErr
 #include <new>
@@ -2188,6 +2174,20 @@ static void __Pyx_CppExn2PyErr() {
 }
 #endif
 
+/* GCCDiagnostics.proto */
+#if !defined(__INTEL_COMPILER) && defined(__GNUC__) && (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6))
+#define __Pyx_HAS_GCC_DIAGNOSTIC
+#endif
+
+/* CIntFromPy.proto */
+static CYTHON_INLINE enum ParamKeyType __Pyx_PyInt_As_enum__ParamKeyType(PyObject *);
+
+/* CIntToPy.proto */
+static CYTHON_INLINE PyObject* __Pyx_PyInt_From_enum__ParamKeyType(enum ParamKeyType value);
+
+/* CIntToPy.proto */
+static CYTHON_INLINE PyObject* __Pyx_PyInt_From_int(int value);
+
 /* CIntFromPy.proto */
 static CYTHON_INLINE long __Pyx_PyInt_As_long(PyObject *);
 
@@ -6738,6 +6738,7 @@ static int __pyx_pf_6common_10params_pyx_6Params___cinit__(struct __pyx_obj_6com
   PyObject *__pyx_t_3 = NULL;
   int __pyx_t_4;
   std::string __pyx_t_5;
+  Params *__pyx_t_6;
   int __pyx_lineno = 0;
   const char *__pyx_filename = NULL;
   int __pyx_clineno = 0;
@@ -6799,7 +6800,19 @@ static int __pyx_pf_6common_10params_pyx_6Params___cinit__(struct __pyx_obj_6com
  * 
  *   def __dealloc__(self):
  */
-        __pyx_v_self->p = new Params(__pyx_v_path);
+        try {
+          __pyx_t_6 = new Params(__pyx_v_path);
+        } catch(...) {
+          #ifdef WITH_THREAD
+          PyGILState_STATE __pyx_gilstate_save = __Pyx_PyGILState_Ensure();
+          #endif
+          __Pyx_CppExn2PyErr();
+          #ifdef WITH_THREAD
+          __Pyx_PyGILState_Release(__pyx_gilstate_save);
+          #endif
+          __PYX_ERR(0, 41, __pyx_L4_error)
+        }
+        __pyx_v_self->p = __pyx_t_6;
       }
 
       /* "common/params_pyx.pyx":40
@@ -6817,6 +6830,13 @@ static int __pyx_pf_6common_10params_pyx_6Params___cinit__(struct __pyx_obj_6com
           #endif
           goto __pyx_L5;
         }
+        __pyx_L4_error: {
+          #ifdef WITH_THREAD
+          __Pyx_FastGIL_Forget();
+          Py_BLOCK_THREADS
+          #endif
+          goto __pyx_L1_error;
+        }
         __pyx_L5:;
       }
   }

common/params_pyx.pyx

@@ -14,7 +14,7 @@ cdef extern from "common/params.h":
     ALL
 
   cdef cppclass c_Params "Params":
-    c_Params(string) nogil
+    c_Params(string) except + nogil
     string get(string, bool) nogil
     bool getBool(string, bool) nogil
     int remove(string) nogil

common/ratekeeper.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+class RateKeeper {
+public:
+  RateKeeper(const std::string &name, float rate, float print_delay_threshold = 0);
+  ~RateKeeper() {}
+  bool keepTime();
+  bool monitorTime();
+  inline double frame() const { return frame_; }
+  inline double remaining() const { return remaining_; }
+
+private:
+  double interval;
+  double next_frame_time;
+  double last_monitor_time;
+  double remaining_ = 0;
+  float print_delay_threshold = 0;
+  uint64_t frame_ = 0;
+  std::string name;
+};

common/realtime.py

@@ -5,10 +5,9 @@ import time
 from collections import deque
 from typing import Optional, List, Union
 
-from setproctitle import getproctitle  # pylint: disable=no-name-in-module
+from setproctitle import getproctitle
 
-from common.clock import sec_since_boot  # pylint: disable=no-name-in-module, import-error
-from system.hardware import PC
+from openpilot.system.hardware import PC
 
 
 # time step for each process
@@ -31,12 +30,12 @@ class Priority:
 
 def set_realtime_priority(level: int) -> None:
   if not PC:
-    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))  # pylint: disable=no-member
+    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level))
 
 
 def set_core_affinity(cores: List[int]) -> None:
   if not PC:
-    os.sched_setaffinity(0, cores)  # pylint: disable=no-member
+    os.sched_setaffinity(0, cores)
 
 
 def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None:
@@ -50,13 +49,13 @@ class Ratekeeper:
   def __init__(self, rate: float, print_delay_threshold: Optional[float] = 0.0) -> None:
     """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
     self._interval = 1. / rate
-    self._next_frame_time = sec_since_boot() + self._interval
+    self._next_frame_time = time.monotonic() + self._interval
     self._print_delay_threshold = print_delay_threshold
     self._frame = 0
     self._remaining = 0.0
     self._process_name = getproctitle()
     self._dts = deque([self._interval], maxlen=100)
-    self._last_monitor_time = sec_since_boot()
+    self._last_monitor_time = time.monotonic()
 
   @property
   def frame(self) -> int:
@@ -82,11 +81,11 @@ class Ratekeeper:
   # this only monitor the cumulative lag, but does not enforce a rate
   def monitor_time(self) -> bool:
     prev = self._last_monitor_time
-    self._last_monitor_time = sec_since_boot()
+    self._last_monitor_time = time.monotonic()
     self._dts.append(self._last_monitor_time - prev)
 
     lagged = False
-    remaining = self._next_frame_time - sec_since_boot()
+    remaining = self._next_frame_time - time.monotonic()
     self._next_frame_time += self._interval
     if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
       print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")

common/spinner.py

@@ -1,6 +1,6 @@
 import os
 import subprocess
-from common.basedir import BASEDIR
+from openpilot.common.basedir import BASEDIR
 
 
 class Spinner():

common/swaglog.h

@@ -44,7 +44,7 @@ void cloudlog_te(int levelnum, const char* filename, int lineno, const char* fun
   int __millis = (millis);                          \
   uint64_t __ts = nanos_since_boot();               \
                                                     \
-  if (!__begin) __begin = __ts;                     \
+  if (!__begin) { __begin = __ts; }                 \
                                                     \
   if (__begin + __millis*1000000ULL < __ts) {       \
     if (__missed) {                                 \

common/text_window.py

@@ -2,7 +2,7 @@
 import os
 import time
 import subprocess
-from common.basedir import BASEDIR
+from openpilot.common.basedir import BASEDIR
 
 
 class TextWindow:

common/transformations/camera.py

@@ -1,6 +1,6 @@
 import numpy as np
 
-import common.transformations.orientation as orient
+import openpilot.common.transformations.orientation as orient
 
 ## -- hardcoded hardware params --
 eon_f_focal_length = 910.0

common/transformations/coordinates.hpp

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <eigen3/Eigen/Dense>
+
 #define DEG2RAD(x) ((x) * M_PI / 180.0)
 #define RAD2DEG(x) ((x) * 180.0 / M_PI)
 

common/transformations/coordinates.py

@@ -1,8 +1,7 @@
-# pylint: skip-file
-from common.transformations.orientation import numpy_wrap
-from common.transformations.transformations import (ecef2geodetic_single,
+from openpilot.common.transformations.orientation import numpy_wrap
+from openpilot.common.transformations.transformations import (ecef2geodetic_single,
                                                     geodetic2ecef_single)
-from common.transformations.transformations import LocalCoord as LocalCoord_single
+from openpilot.common.transformations.transformations import LocalCoord as LocalCoord_single
 
 
 class LocalCoord(LocalCoord_single):

common/transformations/model.py

@@ -1,7 +1,9 @@
 import numpy as np
 
-from common.transformations.camera import (FULL_FRAME_SIZE,
-                                           get_view_frame_from_calib_frame)
+from openpilot.common.transformations.orientation import rot_from_euler
+from openpilot.common.transformations.camera import (
+  FULL_FRAME_SIZE, get_view_frame_from_calib_frame, view_frame_from_device_frame,
+  eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics)
 
 # segnet
 SEGNET_SIZE = (512, 384)
@@ -57,61 +59,20 @@ medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
 
 medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
 
+calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
+calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
 
-### This function mimics the update_calibration logic in modeld.cc
-### Manually verified to give similar results to xx.uncommon.utils.transform_img
-def get_warp_matrix(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from common.transformations.orientation import rot_from_euler
-  from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
+# This function is verified to give similar results to xx.uncommon.utils.transform_img
+def get_warp_matrix(device_from_calib_euler: np.ndarray, wide_camera: bool = False, bigmodel_frame: bool = False, tici: bool = True) -> np.ndarray:
+  if tici and wide_camera:
+    cam_intrinsics = tici_ecam_intrinsics
   elif tici:
-    intrinsics = tici_fcam_intrinsics
+    cam_intrinsics = tici_fcam_intrinsics
   else:
-    intrinsics = eon_fcam_intrinsics
+    cam_intrinsics = eon_fcam_intrinsics
 
-  if big_model:
-    sbigmodel_from_calib = sbigmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(sbigmodel_from_calib)
-  else:
-    medmodel_from_calib = medmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(medmodel_from_calib)
-  device_from_calib = rot_from_euler(rpy_calib)
-  camera_from_calib = intrinsics.dot(view_frame_from_device_frame.dot(device_from_calib))
-  warp_matrix = camera_from_calib.dot(calib_from_model)
-  return warp_matrix
-
-
-### This is old, just for debugging
-def get_warp_matrix_old(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from common.transformations.orientation import rot_from_euler
-  from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-
-  def get_view_frame_from_road_frame(roll, pitch, yaw, height):
-    device_from_road = rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
-    view_from_road = view_frame_from_device_frame.dot(device_from_road)
-    return np.hstack((view_from_road, [[0], [height], [0]]))
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
-  elif tici:
-    intrinsics = tici_fcam_intrinsics
-  else:
-    intrinsics = eon_fcam_intrinsics
-
-  model_height = 1.22
-  if big_model:
-    model_from_road = np.dot(sbigmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  else:
-    model_from_road = np.dot(medmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  ground_from_model = np.linalg.inv(model_from_road[:, (0, 1, 3)])
-
-  E = get_view_frame_from_road_frame(*rpy_calib, 1.22)
-  camera_frame_from_road_frame = intrinsics.dot(E)
-  camera_frame_from_ground = camera_frame_from_road_frame[:,(0,1,3)]
-  warp_matrix = camera_frame_from_ground .dot(ground_from_model)
+  calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
+  device_from_calib = rot_from_euler(device_from_calib_euler)
+  camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib
+  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
   return warp_matrix

common/transformations/orientation.hpp

@@ -1,6 +1,6 @@
 #pragma once
 #include <eigen3/Eigen/Dense>
-#include "coordinates.hpp"
+#include "common/transformations/coordinates.hpp"
 
 
 Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat);

common/transformations/orientation.py

@@ -1,8 +1,7 @@
-# pylint: skip-file
 import numpy as np
 from typing import Callable
 
-from common.transformations.transformations import (ecef_euler_from_ned_single,
+from openpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
                                                     euler2quat_single,
                                                     euler2rot_single,
                                                     ned_euler_from_ecef_single,

common/transformations/transformations.cpp

@@ -7,15 +7,20 @@
             "common/transformations/coordinates.cc",
             "common/transformations/coordinates.hpp",
             "common/transformations/orientation.cc",
-            "common/transformations/orientation.hpp"
+            "common/transformations/orientation.hpp",
+            "openpilot/common/transformations/coordinates.cc",
+            "openpilot/common/transformations/coordinates.hpp",
+            "openpilot/common/transformations/orientation.cc",
+            "openpilot/common/transformations/orientation.hpp"
         ],
         "include_dirs": [
-            "common/transformations"
+            "common/transformations",
+            "openpilot/common/transformations"
         ],
         "language": "c++",
         "name": "common.transformations.transformations",
         "sources": [
-            "/data/openpilot/common/transformations/transformations.pyx"
+            "/data/dp-private/common/transformations/transformations.pyx"
         ]
     },
     "module_name": "common.transformations.transformations"
@@ -2528,6 +2533,8 @@ static CYTHON_INLINE char *__pyx_f_5numpy_7ndarray_4data_data(PyArrayObject *__p
 
 /* Module declarations from "libcpp" */
 
+/* Module declarations from "openpilot.common.transformations.transformations" */
+
 /* Module declarations from "cython" */
 
 /* Module declarations from "libc.string" */
@@ -2720,6 +2727,8 @@ typedef struct {
   #endif
   #if CYTHON_USE_MODULE_STATE
   #endif
+  #if CYTHON_USE_MODULE_STATE
+  #endif
   PyTypeObject *__pyx_ptype_7cpython_4type_type;
   #if CYTHON_USE_MODULE_STATE
   #endif
@@ -3269,6 +3278,8 @@ static int __pyx_m_traverse(PyObject *m, visitproc visit, void *arg) {
 #endif
 #if CYTHON_USE_MODULE_STATE
 #endif
+#if CYTHON_USE_MODULE_STATE
+#endif
 #define __pyx_ptype_7cpython_4type_type __pyx_mstate_global->__pyx_ptype_7cpython_4type_type
 #if CYTHON_USE_MODULE_STATE
 #endif
@@ -10123,7 +10134,7 @@ if (!__Pyx_RefNanny) {
   /* "common/transformations/transformations.pyx":1
  * # distutils: language = c++             # <<<<<<<<<<<<<<
  * # cython: language_level = 3
- * from common.transformations.transformations cimport Matrix3, Vector3, Quaternion
+ * from openpilot.common.transformations.transformations cimport Matrix3, Vector3, Quaternion
  */
   __pyx_t_2 = __Pyx_PyDict_NewPresized(0); if (unlikely(!__pyx_t_2)) __PYX_ERR(0, 1, __pyx_L1_error)
   __Pyx_GOTREF(__pyx_t_2);

common/transformations/transformations.pyx

@@ -1,20 +1,20 @@
 # distutils: language = c++
 # cython: language_level = 3
-from common.transformations.transformations cimport Matrix3, Vector3, Quaternion
-from common.transformations.transformations cimport ECEF, NED, Geodetic
+from openpilot.common.transformations.transformations cimport Matrix3, Vector3, Quaternion
+from openpilot.common.transformations.transformations cimport ECEF, NED, Geodetic
 
-from common.transformations.transformations cimport euler2quat as euler2quat_c
-from common.transformations.transformations cimport quat2euler as quat2euler_c
-from common.transformations.transformations cimport quat2rot as quat2rot_c
-from common.transformations.transformations cimport rot2quat as rot2quat_c
-from common.transformations.transformations cimport euler2rot as euler2rot_c
-from common.transformations.transformations cimport rot2euler as rot2euler_c
-from common.transformations.transformations cimport rot_matrix as rot_matrix_c
-from common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
-from common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
-from common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
-from common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
-from common.transformations.transformations cimport LocalCoord_c
+from openpilot.common.transformations.transformations cimport euler2quat as euler2quat_c
+from openpilot.common.transformations.transformations cimport quat2euler as quat2euler_c
+from openpilot.common.transformations.transformations cimport quat2rot as quat2rot_c
+from openpilot.common.transformations.transformations cimport rot2quat as rot2quat_c
+from openpilot.common.transformations.transformations cimport euler2rot as euler2rot_c
+from openpilot.common.transformations.transformations cimport rot2euler as rot2euler_c
+from openpilot.common.transformations.transformations cimport rot_matrix as rot_matrix_c
+from openpilot.common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
+from openpilot.common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
+from openpilot.common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
+from openpilot.common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
+from openpilot.common.transformations.transformations cimport LocalCoord_c
 
 
 import cython

common/util.h

@@ -77,6 +77,8 @@ float getenv(const char* key, float default_val);
 
 std::string hexdump(const uint8_t* in, const size_t size);
 std::string dir_name(std::string const& path);
+bool starts_with(const std::string &s1, const std::string &s2);
+bool ends_with(const std::string &s1, const std::string &s2);
 
 // ***** random helpers *****
 int random_int(int min, int max);
@@ -115,7 +117,7 @@ public:
 #ifndef __APPLE__
     std::signal(SIGPWR, (sighandler_t)set_do_exit);
 #endif
-  };
+  }
   inline static std::atomic<bool> power_failure = false;
   inline static std::atomic<int> signal = 0;
   inline operator bool() { return do_exit; }
@@ -151,12 +153,18 @@ struct unique_fd {
 
 class FirstOrderFilter {
 public:
-  FirstOrderFilter(float x0, float ts, float dt) {
+  FirstOrderFilter(float x0, float ts, float dt, bool initialized = true) {
     k_ = (dt / ts) / (1.0 + dt / ts);
     x_ = x0;
+    initialized_ = initialized;
   }
   inline float update(float x) {
-    x_ = (1. - k_) * x_ + k_ * x;
+    if (initialized_) {
+      x_ = (1. - k_) * x_ + k_ * x;
+    } else {
+      initialized_ = true;
+      x_ = x;
+    }
     return x_;
   }
   inline void reset(float x) { x_ = x; }
@@ -164,12 +172,13 @@ public:
 
 private:
   float x_, k_;
+  bool initialized_;
 };
 
 template<typename T>
 void update_max_atomic(std::atomic<T>& max, T const& value) {
   T prev = max;
-  while(prev < value && !max.compare_exchange_weak(prev, value)) {}
+  while (prev < value && !max.compare_exchange_weak(prev, value)) {}
 }
 
 class LogState {
@@ -179,9 +188,9 @@ class LogState {
   void *zctx = nullptr;
   void *sock = nullptr;
   int print_level;
-  const char* endpoint;
+  std::string endpoint;
 
-  LogState(const char* _endpoint) {
+  LogState(std::string _endpoint) {
     endpoint = _endpoint;
   }
 
@@ -193,7 +202,7 @@ class LogState {
     int timeout = 100;
     zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout));
 
-    zmq_connect(sock, endpoint);
+    zmq_connect(sock, endpoint.c_str());
     initialized = true;
   }
 

common/version.h

@@ -1 +1 @@
-#define COMMA_VERSION "2023.08.22"
+#define COMMA_VERSION "2023.10.09"