diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index 203470bb42..5c7e17cd51 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -15,7 +15,9 @@ const int HEIGHT = 256, WIDTH = 256; const int NUM_VIPC_BUFFERS = 4; const int EARTH_CIRCUMFERENCE_METERS = 40075000; +const int EARTH_RADIUS_METERS = 6378137; const int PIXELS_PER_TILE = 256; +const int MAP_OFFSET = 128; const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE"); const int LLK_DECIMATION = TEST_MODE ? 1 : 10; @@ -26,6 +28,14 @@ float get_zoom_level_for_scale(float lat, float meters_per_pixel) { return log2(num_tiles) - 1; } +QMapbox::Coordinate get_point_along_line(float lat, float lon, float bearing, float dist) { + float ang_dist = dist / EARTH_RADIUS_METERS; + float lat1 = DEG2RAD(lat), lon1 = DEG2RAD(lon), bearing1 = DEG2RAD(bearing); + float lat2 = asin(sin(lat1)*cos(ang_dist) + cos(lat1)*sin(ang_dist)*cos(bearing1)); + float lon2 = lon1 + atan2(sin(bearing1)*sin(ang_dist)*cos(lat1), cos(ang_dist)-sin(lat1)*sin(lat2)); + return QMapbox::Coordinate(RAD2DEG(lat2), RAD2DEG(lon2)); +} + MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_settings(settings) { QSurfaceFormat fmt; @@ -70,7 +80,7 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set if (online) { vipc_server.reset(new VisionIpcServer("navd")); - vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH/2, HEIGHT/2); + vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT); vipc_server->start_listener(); pm.reset(new PubMaster({"navThumbnail", "mapRenderState"})); @@ -93,7 +103,8 @@ void MapRenderer::msgUpdate() { bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid(); if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) { - updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2])); + float bearing = RAD2DEG(orientation.getValue()[2]); + updatePosition(get_point_along_line(pos.getValue()[0], pos.getValue()[1], bearing, MAP_OFFSET), bearing); // TODO: use the static rendering mode if (!loaded() && frame_id > 0) {