waiting for rtk fix to initialize rtk estimator

ctu-mrs · Oct 19, 2023 · 6247d7c · 6247d7c
1 parent 54dd5db
commit 6247d7c
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 11 deletions.
diff --git 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
@@ -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
@@ -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());