fixed takeoff disarming bug during repeated takeoff

ctu-mrs · Jul 17, 2024 · 7f36bb7 · 7f36bb7
1 parent 9283b3f
commit 7f36bb7
Showing 1 changed file with 11 additions and 10 deletions.
diff --git a/src/automatic_start.cpp b/src/automatic_start.cpp
@@ -570,22 +570,23 @@ void AutomaticStart::timerMain([[maybe_unused]] const ros::TimerEvent& event) {
       // when armed and in offboard, takeoff
       if (armed && offboard && control_output_enabled) {
 
-        ros::Duration armed_time_diff    = ros::Time::now() - armed_time;
-        ros::Duration offboard_time_diff = ros::Time::now() - offboard_time;
+        if (!_handle_takeoff_) {
+          changeState(STATE_FINISHED);
+        } else {
 
-        if (armed_time_diff > ros::Duration(_safety_timeout_) && offboard_time_diff > ros::Duration(_safety_timeout_)) {
+          ros::Duration armed_time_diff    = ros::Time::now() - armed_time;
+          ros::Duration offboard_time_diff = ros::Time::now() - offboard_time;
+
+          if (armed_time_diff > ros::Duration(_safety_timeout_) && offboard_time_diff > ros::Duration(_safety_timeout_)) {
 
-          if (_handle_takeoff_) {
             changeState(STATE_TAKEOFF);
-          } else {
-            changeState(STATE_FINISHED);
-          }
 
-        } else {
+          } else {
 
-          double min = (armed_time_diff < offboard_time_diff) ? armed_time_diff.toSec() : offboard_time_diff.toSec();
+            double min = (armed_time_diff < offboard_time_diff) ? armed_time_diff.toSec() : offboard_time_diff.toSec();
 
-          ROS_WARN_THROTTLE(1.0, "taking off in %.0f", (_safety_timeout_ - min));
+            ROS_WARN_THROTTLE(1.0, "taking off in %.0f", (_safety_timeout_ - min));
+          }
         }
       }