fixed non-positive timestamp error before first rtk message arrives

ctu-mrs · Apr 20, 2024 · 1cea367 · 1cea367
1 parent 4742c73
commit 1cea367
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h
@@ -1736,6 +1736,10 @@ bool Correction<n_measurements>::isTimestampOk() {
 template <int n_measurements>
 bool Correction<n_measurements>::isMsgComing() {
 
+  if (first_timestamp_) {
+    return true;
+  }
+
   const ros::Time msg_time = mrs_lib::get_mutexed(mtx_msg_time_, msg_time_);
   const double    delta    = ros::Time::now().toSec() - msg_time.toSec();