convert locationd to c++ (commaai#20622)

* live_kf to c++

* first locationd code

* Running in process_replay

* locationd handle cam_odo and live_calib

* log event handlers

* working message receiving

* compiling message sending

* correctly sending some messages

* correct receiving and sending

* update ref_commit with some all_alive_and_valid being false, minor fixes

* fix std abs

* linking on device fix

* fix cpu usage test

* generate kf constants and defines

* fix replay test

* replay without acks, cleanup

* operate on bytearray messages

* cleanup

* send msg fix

* small sleep, less flaky test

* remove python locationd

* review feedback

* bump rednose

SConstruct

@@ -366,19 +366,19 @@ Export('common', 'gpucommon', 'visionipc')
 rednose_config = {
   'generated_folder': '#selfdrive/locationd/models/generated',
   'to_build': {
-    'live': ('#selfdrive/locationd/models/live_kf.py', True),
-    'car': ('#selfdrive/locationd/models/car_kf.py', True),
+    'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']),
+    'car': ('#selfdrive/locationd/models/car_kf.py', True, []),
   },
 }
 
 if arch != "aarch64":
   rednose_config['to_build'].update({
-    'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True),
-    'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True),
-    'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False),
-    'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False),
-    'feature_handler_5': ('#rednose/helpers/feature_handler.py', False),
-    'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True),
+    'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
+    'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
+    'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []),
+    'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []),
+    'feature_handler_5': ('#rednose/helpers/feature_handler.py', False, []),
+    'lane': ('#xx/pipeline/lib/ekf/lane_kf.py', True, []),
   })
 
 Export('rednose_config')

common/transformations/coordinates.cc

@@ -6,8 +6,6 @@
 
 #include "coordinates.hpp"
 
-#define DEG2RAD(x) ((x) * M_PI / 180.0)
-#define RAD2DEG(x) ((x) * 180.0 / M_PI)
 
 
 double a = 6378137; // lgtm [cpp/short-global-name]

common/transformations/coordinates.hpp

@@ -1,5 +1,8 @@
 #pragma once
 
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+#define RAD2DEG(x) ((x) * 180.0 / M_PI)
+
 struct ECEF {
   double x, y, z;
   Eigen::Vector3d to_vector(){
@@ -9,6 +12,9 @@ struct ECEF {
 
 struct NED {
   double n, e, d;
+  Eigen::Vector3d to_vector(){
+    return Eigen::Vector3d(n, e, d);
+  }
 };
 
 struct Geodetic {

release/files_common

@@ -289,12 +289,15 @@ selfdrive/locationd/generated/ubx.h
 selfdrive/locationd/generated/gps.cpp
 selfdrive/locationd/generated/gps.h
 
-selfdrive/locationd/locationd.py
+selfdrive/locationd/locationd.h
+selfdrive/locationd/locationd.cc
 selfdrive/locationd/paramsd.py
 selfdrive/locationd/models/.gitignore
 selfdrive/locationd/models/live_kf.py
 selfdrive/locationd/models/car_kf.py
 selfdrive/locationd/models/constants.py
+selfdrive/locationd/models/live_kf.h
+selfdrive/locationd/models/live_kf.cc
 
 selfdrive/locationd/calibrationd.py
 

selfdrive/locationd/.gitignore

@@ -1,4 +1,5 @@
 ubloxd
 ubloxd_test
 params_learner
-paramsd
+paramsd
+locationd

selfdrive/locationd/SConscript

@@ -1,12 +1,18 @@
-Import('env', 'common', 'cereal', 'messaging')
+Import('env', 'common', 'cereal', 'messaging', 'libkf', 'transformations')
 
 loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread']
 
-
 if GetOption('kaitai'):
   generated = Dir('generated').srcnode().abspath
   cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES"
   env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd)
   env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd)
 
 env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs)
+
+ekf_sym_cc = env.Object("#rednose/helpers/ekf_sym.cc")
+lenv = env.Clone()
+lenv["_LIBFLAGS"] += f' {libkf[0].get_labspath()}'
+locationd = lenv.Program("locationd", ["locationd.cc", "models/live_kf.cc", ekf_sym_cc],
+                         LIBS=loc_libs + transformations)
+lenv.Depends(locationd, libkf)