Added the warning messages back into the estimator.

rosflight · Nov 28, 2023 · a109542 · a109542
1 parent 583dc7f
commit a109542
Showing 1 changed file with 3 additions and 6 deletions.
diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp
@@ -338,15 +338,15 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
   }
 
   bool problem = false;
-//  int prob_index;
+  int prob_index;
   for (int i = 0; i < 7; i++)
   {
     if (!std::isfinite(xhat_p_(i)))
     {
       if (!problem)
       {
         problem = true;
-//        prob_index = i;
+        prob_index = i;
       }
       switch (i)
       {
@@ -380,7 +380,7 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
   }
   if (problem)
   {
-//    RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", prob_index); TODO come back and track down why pn is going infinite.
+    RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", prob_index);
   }
   if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f))
   {
@@ -396,9 +396,6 @@ void estimator_example::estimate(const params_s &params, const input_s &input, o
   float wehat = xhat_p_(5);
   float psihat = xhat_p_(6);
 
-
-//    RCLCPP_INFO_STREAM(this->get_logger(), "chihat: " << chihat);
-
   output.pn = pnhat;
   output.pe = pehat;
   output.h = hhat;