From e63d94d8d24975d308e17b7ce71fdb6b5529dac4 Mon Sep 17 00:00:00 2001
From: Harald Schafer <harald.the.engineer@gmail.com>
Date: Wed, 4 Aug 2021 10:41:49 -0700
Subject: [PATCH] Revert "nav: use calibrated orientation (#21853)"

This reverts commit 4b209f31a11546a3a56032558c656c2aa0b3175f.
---
 selfdrive/locationd/locationd.cc         | 2 --
 selfdrive/test/process_replay/ref_commit | 2 +-
 selfdrive/ui/qt/maps/map.cc              | 2 +-
 3 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index 305a1466b..3f86ecf33 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -114,7 +114,6 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
 
   VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
-  VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
   //orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
   VectorXd nextfix_ecef = fix_ecef + vel_ecef;
   VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
@@ -138,7 +137,6 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
   init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true);
   init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
   init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
-  init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated);
   init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
   init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
   init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index cd0a47420..7aed78b31 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-db2abbe672c77bb7fbf904f195174cbfe9fc6f27
\ No newline at end of file
+658824c1198bd9e4c2b2c6d1c9de9bcae04c7057
\ No newline at end of file
diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc
index 41777ec1b..4db9ce5e7 100644
--- a/selfdrive/ui/qt/maps/map.cc
+++ b/selfdrive/ui/qt/maps/map.cc
@@ -129,7 +129,7 @@ void MapWindow::timerUpdate() {
 
     if (localizer_valid) {
       auto pos = location.getPositionGeodetic();
-      auto orientation = location.getCalibratedOrientationNED();
+      auto orientation = location.getOrientationNED();
 
       float velocity = location.getVelocityCalibrated().getValue()[0];
       float bearing = RAD2DEG(orientation.getValue()[2]);