diff --git a/cereal b/cereal index 7765176413..8825337204 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 7765176413c0bb14143fe2469d5390ea0ea61a1e +Subproject commit 88253372042197ac92776cddc35a29d0eb384a3e diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index a9c34b5bd5..904768d101 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -65,7 +65,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT); vipc_server->start_listener(); - pm.reset(new PubMaster({"navThumbnail"})); + pm.reset(new PubMaster({"navThumbnail", "mapRenderState"})); sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}, {"liveLocationKalman"})); timer = new QTimer(this); @@ -122,14 +122,16 @@ bool MapRenderer::loaded() { } void MapRenderer::update() { + double start_t = millis_since_boot(); gl_functions->glClear(GL_COLOR_BUFFER_BIT); m_map->render(); gl_functions->glFlush(); + double end_t = millis_since_boot(); - sendVipc(); + publish((end_t - start_t) / 1000.0); } -void MapRenderer::sendVipc() { +void MapRenderer::publish(const double render_time) { if (!vipc_server || !loaded()) { return; } @@ -139,7 +141,7 @@ void MapRenderer::sendVipc() { VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); VisionIpcBufExtra extra = { .frame_id = frame_id, - .timestamp_sof = ts, + .timestamp_sof = sm->rcv_time("liveLocationKalman"), .timestamp_eof = ts, }; @@ -173,6 +175,13 @@ void MapRenderer::sendVipc() { pm->send("navThumbnail", msg); } + // Send state msg + MessageBuilder msg; + auto state = msg.initEvent().initMapRenderState(); + state.setLocationMonoTime(sm->rcv_time("liveLocationKalman")); + state.setRenderTime(render_time); + pm->send("mapRenderState", msg); + frame_id++; } diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h index 7f725c93fe..1c7024361d 100644 --- a/selfdrive/navd/map_renderer.h +++ b/selfdrive/navd/map_renderer.h @@ -34,7 +34,7 @@ private: std::unique_ptr vipc_server; std::unique_ptr pm; std::unique_ptr sm; - void sendVipc(); + void publish(const double render_time); QMapboxGLSettings m_settings; QScopedPointer m_map;