fixed yet another options of crashing

ctu-mrs · Mar 24, 2024 · 47eb10e · 47eb10e
1 parent 88c80e2
commit 47eb10e
Showing 1 changed file with 10 additions and 3 deletions.
diff --git a/src/automatic_start.cpp b/src/automatic_start.cpp
@@ -134,6 +134,8 @@ class AutomaticStart : public nodelet::Nodelet {
   ros::Time offboard_time_;
   bool      offboard_ = false;
 
+  bool we_toggled_output_ = false;
+
   // | ------------------------ routines ------------------------ |
 
   bool takeoff();
@@ -472,10 +474,13 @@ void AutomaticStart::timerMain([[maybe_unused]] const ros::TimerEvent& event) {
 
           ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: -- the UAV is also armed!! finishing to prevent unwanted system activation");
 
-          bool res = toggleControlOutput(false);
+          if (we_toggled_output_) {
 
-          if (!res) {
-            ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output OFF");
+            bool res = toggleControlOutput(false);
+
+            if (!res) {
+              ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output OFF");
+            }
           }
 
           changeState(STATE_FINISHED);
@@ -513,6 +518,8 @@ void AutomaticStart::timerMain([[maybe_unused]] const ros::TimerEvent& event) {
 
           if (!res) {
             ROS_WARN_THROTTLE(1.0, "[AutomaticStart]: could not set control output ON");
+          } else {
+            we_toggled_output_ = true;
           }
         }