From 41c96c99c7b37eb3067d610fe46094daebc02df8 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Tue, 8 Jun 2021 16:00:31 +0200
Subject: [PATCH] nav: add current bearing to route request (#21186)

---
 selfdrive/ui/qt/maps/map.cc | 16 ++++++++++++++--
 selfdrive/ui/qt/maps/map.h  |  7 ++++++-
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc
index 9456e0e7ee..7870a3e164 100644
--- a/selfdrive/ui/qt/maps/map.cc
+++ b/selfdrive/ui/qt/maps/map.cc
@@ -118,14 +118,18 @@ void MapWindow::timerUpdate() {
     // Update map location, orientation and zoom on valid localizer output
     if (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) {
       auto pos = location.getPositionGeodetic();
-      float velocity = location.getVelocityCalibrated().getValue()[0];
       auto orientation = location.getOrientationNED();
+
+      float velocity = location.getVelocityCalibrated().getValue()[0];
+      float bearing = RAD2DEG(orientation.getValue()[2]);
       auto coordinate = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]);
+
       last_position = coordinate;
+      last_bearing = bearing;
 
       if (pan_counter == 0){
         m_map->setCoordinate(coordinate);
-        m_map->setBearing(RAD2DEG(orientation.getValue()[2]));
+        m_map->setBearing(bearing);
       } else {
         pan_counter--;
       }
@@ -268,6 +272,13 @@ void MapWindow::calculateRoute(QMapbox::Coordinate destination) {
   nav_destination = destination;
   QGeoRouteRequest request(to_QGeoCoordinate(last_position), to_QGeoCoordinate(destination));
   request.setFeatureWeight(QGeoRouteRequest::TrafficFeature, QGeoRouteRequest::AvoidFeatureWeight);
+
+  if (last_bearing) {
+    QVariantMap params;
+    params["bearing"] = *last_bearing;
+    request.setWaypointsMetadata({params});
+  }
+
   routing_manager->calculateRoute(request);
 }
 
@@ -383,6 +394,7 @@ void MapWindow::offroadTransition(bool offroad) {
     auto dest = coordinate_from_param("NavDestination");
     setVisible(dest.has_value());
   }
+  last_bearing = {};
 }
 
 MapInstructions::MapInstructions(QWidget * parent) : QWidget(parent){
diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h
index a7fe527b57..bdd19c0f59 100644
--- a/selfdrive/ui/qt/maps/map.h
+++ b/selfdrive/ui/qt/maps/map.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include <optional>
+
 #include <QGeoCoordinate>
 #include <QGeoManeuver>
 #include <QGeoRouteRequest>
@@ -56,6 +58,10 @@ private:
   int pan_counter = 0;
   int zoom_counter = 0;
 
+  // Position
+  QMapbox::Coordinate last_position = QMapbox::Coordinate(37.7393118509158, -122.46471285025565);
+  std::optional<float> last_bearing;
+
   // Route
   bool gps_ok = false;
   QGeoServiceProvider *geoservice_provider;
@@ -63,7 +69,6 @@ private:
   QGeoRoute route;
   QGeoRouteSegment segment;
   QWidget* map_instructions;
-  QMapbox::Coordinate last_position = QMapbox::Coordinate(37.7393118509158, -122.46471285025565);
   QMapbox::Coordinate nav_destination;
   double last_maneuver_distance = 1000;