Offset topdown map by 128 meters (#28216)

We drive forward :)
old-commit-hash: d528481d8d

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) {