mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-22 06:22:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Offset topdown map by 128 meters (#28216)

Browse Source 
We drive forward :)
This commit is contained in:
Mitchell Goff
2023-05-18 15:30:58 -07:00
committed by GitHub
parent d4dec3ba5e
commit d528481d8d
1 changed files with 13 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/navd/map_renderer.cc
+13 -2
View File
@@ -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) {