reset estimator

ctu-mrs · Jun 25, 2024 · 660847b · 660847b
1 parent 347f523
commit 660847b
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 5 deletions.
diff --git a/src/estimators/altitude/alt_generic.cpp b/src/estimators/altitude/alt_generic.cpp
@@ -201,6 +201,7 @@ bool AltGeneric::reset(void) {
     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
   }
 
+  changeState(INITIALIZED_STATE);
   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
 
   return true;

diff --git a/src/estimators/heading/hdg_generic.cpp b/src/estimators/heading/hdg_generic.cpp
@@ -196,6 +196,7 @@ bool HdgGeneric::reset(void) {
     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_.at(0), rep_buffer_size_);
   }
 
+  changeState(INITIALIZED_STATE);
   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
 
   return true;

diff --git a/src/estimators/lateral/lat_generic.cpp b/src/estimators/lateral/lat_generic.cpp
@@ -115,7 +115,7 @@ void LatGeneric::initialize(ros::NodeHandle &nh, const std::shared_ptr<CommonHan
   }
 
   // | ------------------ timers initialization ----------------- |
-  timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerUpdate, this);  // not running after init
+  timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerUpdate, this); 
   /* timer_check_health_       = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &LatGeneric::timerCheckHealth, this); */
 
   // | --------------- subscribers initialization --------------- |
@@ -207,6 +207,7 @@ bool LatGeneric::reset(void) {
     lkf_rep_           = std::make_unique<mrs_lib::Repredictor<varstep_lkf_t>>(x0, P0, u0, Q_, t0, models_[0], rep_buffer_size_);
   }
 
+  changeState(INITIALIZED_STATE);
   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
 
   return true;

diff --git a/src/estimators/state/state_generic.cpp b/src/estimators/state/state_generic.cpp
@@ -48,7 +48,7 @@ void StateGeneric::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<
   ns_frame_id_ = ch_->uav_name + "/" + frame_id_;
 
   // | ------------------ timers initialization ----------------- |
-  timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerUpdate, this);  // not running after init
+  timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerUpdate, this);
   /* timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerCheckHealth, this); */
   timer_pub_attitude_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &StateGeneric::timerPubAttitude, this);
 
@@ -193,10 +193,11 @@ bool StateGeneric::reset(void) {
     return false;
   }
 
-  est_lat_->pause();
-  est_alt_->pause();
-  est_hdg_->pause();
   changeState(STOPPED_STATE);
+  est_lat_->reset();
+  est_alt_->reset();
+  est_hdg_->reset();
+  changeState(INITIALIZED_STATE);
 
   ROS_INFO("[%s]: Estimator reset", getPrintName().c_str());
 
@@ -245,6 +246,10 @@ void StateGeneric::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) {
 
       ROS_INFO_THROTTLE(1.0, "[%s]: %s convergence of LKF", getPrintName().c_str(), Support::waiting_for_string.c_str());
 
+      if (est_lat_->isError() || est_alt_->isError() || est_hdg_->isError()) {
+        changeState(ERROR_STATE);
+      }
+
       if (est_lat_->isRunning() && est_alt_->isRunning() && est_hdg_->isRunning()) {
         ROS_INFO_THROTTLE(1.0, "[%s]: Subestimators converged", getPrintName().c_str());
         changeState(RUNNING_STATE);