openpilot v0.9.6 release

date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6

system/ubloxd/ubloxd.cc

@@ -0,0 +1,65 @@
+#include <cassert>
+
+#include <kaitai/kaitaistream.h>
+
+#include "cereal/messaging/messaging.h"
+#include "common/swaglog.h"
+#include "common/util.h"
+#include "system/ubloxd/ublox_msg.h"
+
+ExitHandler do_exit;
+using namespace ublox;
+
+int main() {
+  LOGW("starting ubloxd");
+  AlignedBuffer aligned_buf;
+  UbloxMsgParser parser;
+
+  PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
+
+  std::unique_ptr<Context> context(Context::create());
+  std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "ubloxRaw"));
+  assert(subscriber != NULL);
+  subscriber->setTimeout(100);
+
+
+  while (!do_exit) {
+    std::unique_ptr<Message> msg(subscriber->receive());
+    if (!msg) {
+      if (errno == EINTR) {
+        do_exit = true;
+      }
+      continue;
+    }
+
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+    auto ubloxRaw = event.getUbloxRaw();
+    float log_time = 1e-9 * event.getLogMonoTime();
+
+    const uint8_t *data = ubloxRaw.begin();
+    size_t len = ubloxRaw.size();
+    size_t bytes_consumed = 0;
+
+    while (bytes_consumed < len && !do_exit) {
+      size_t bytes_consumed_this_time = 0U;
+      if (parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
+
+        try {
+          auto ublox_msg = parser.gen_msg();
+          if (ublox_msg.second.size() > 0) {
+            auto bytes = ublox_msg.second.asBytes();
+            pm.send(ublox_msg.first.c_str(), bytes.begin(), bytes.size());
+          }
+        } catch (const std::exception& e) {
+          LOGE("Error parsing ublox message %s", e.what());
+        }
+
+        parser.reset();
+      }
+      bytes_consumed += bytes_consumed_this_time;
+    }
+  }
+
+  return 0;
+}