From 6247d7c184dd6ba1efab918cf5f5ea14737e7a7f Mon Sep 17 00:00:00 2001
From: Matej Petrlik <matej.petrlik@fel.cvut.cz>
Date: Thu, 19 Oct 2023 15:19:11 +0200
Subject: [PATCH] waiting for rtk fix to initialize rtk estimator

---
 .../mrs_uav_state_estimators/estimators/correction.h  |  5 +++++
 src/estimators/altitude/alt_generic.cpp               |  3 +--
 src/estimators/lateral/lat_generic.cpp                | 11 ++---------
 3 files changed, 8 insertions(+), 11 deletions(-)

diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h
index 4e4a437..1bc69c4 100644
--- a/include/mrs_uav_state_estimators/estimators/correction.h
+++ b/include/mrs_uav_state_estimators/estimators/correction.h
@@ -1025,6 +1025,11 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
     return {};
   }
 
+  if (msg->fix_type.fix_type != mrs_msgs::RtkFixType::RTK_FIX) {
+    ROS_INFO_THROTTLE(1.0, "[%s] %s RTK FIX", getPrintName().c_str(), Support::waiting_for_string.c_str());
+    return {};
+  }
+
   rtk_pos.header = msg->header;
   mrs_lib::UTM(msg->gps.latitude, msg->gps.longitude, &rtk_pos.pose.position.x, &rtk_pos.pose.position.y);
   rtk_pos.pose.position.z  = msg->gps.altitude;
diff --git a/src/estimators/altitude/alt_generic.cpp b/src/estimators/altitude/alt_generic.cpp
index 8f6ddf3..a340f81 100644
--- a/src/estimators/altitude/alt_generic.cpp
+++ b/src/estimators/altitude/alt_generic.cpp
@@ -229,14 +229,13 @@ void AltGeneric::timerUpdate(const ros::TimerEvent &event) {
     }
 
     case INITIALIZED_STATE: {
-
       // initialize the estimator with current corrections
       for (auto correction : corrections_) {
         auto res = correction->getProcessedCorrection();
         if (res) {
           auto measurement_stamped = res.value();
           setState(measurement_stamped.value(0), correction->getStateId());
-          ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
+          ROS_INFO("[%s]: Setting initial state to: %.2f", getPrintName().c_str(), measurement_stamped.value(0));
         } else {
           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(),
                             correction->getNamespacedName().c_str());
diff --git a/src/estimators/lateral/lat_generic.cpp b/src/estimators/lateral/lat_generic.cpp
index 0bb4818..30059da 100644
--- a/src/estimators/lateral/lat_generic.cpp
+++ b/src/estimators/lateral/lat_generic.cpp
@@ -238,17 +238,10 @@ void LatGeneric::timerUpdate(const ros::TimerEvent &event) {
       for (auto correction : corrections_) {
         auto res = correction->getProcessedCorrection();
         if (res) {
-          /* auto res_raw = correction->getRawCorrection(); */
-          /* if (!res_raw) { */
-          /*   ROS_ERROR_THROTTLE(1.0, "[%s]: error getting raw correction when processed correction is available, should not happen", getPrintName().c_str());
-           */
-          /*   return; */
-          /* } */
-          auto measurement_stamped = res.value();  // we need to use raw correction for initialization to avoid saturation wrpt previous state (especially
-                                                   // when getting out of ERROR_STATE)
+          auto measurement_stamped = res.value(); 
           setState(measurement_stamped.value(AXIS_X), correction->getStateId(), AXIS_X);
           setState(measurement_stamped.value(AXIS_Y), correction->getStateId(), AXIS_Y);
-          ROS_INFO_THROTTLE(1.0, "[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
+          ROS_INFO("[%s]: Setting initial state to: %.2f %.2f", getPrintName().c_str(), measurement_stamped.value(AXIS_X),
                             measurement_stamped.value(AXIS_Y));
         } else {
           ROS_INFO_THROTTLE(1.0, "[%s]: %s correction %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), correction->getPrintName().c_str());