[REBASE!] [FIX] tracking of single channel for landing

aau-cns · Oct 20, 2023 · 9016d64 · 9016d64
1 parent df1ce4a
commit 9016d64
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 8 deletions.
diff --git a/include/autonomy_core/autonomy_options.h b/include/autonomy_core/autonomy_options.h
@@ -110,7 +110,8 @@ struct autonomyOptions
        << " - Landing detection:                   " << getStringfromBool(activate_landing_detection) << '\n'
        << " - In-flight sensors init:              " << getStringfromBool(inflight_sensors_init_service) << '\n'
        << " - Hover after mission completion:      " << getStringfromBool(hover_after_mission_completion) << '\n'
-       << " - Sequence multiple in-flight:         " << getStringfromBool(sequence_multiple_in_flight) << '\n';
+       << " - Sequence multiple in-flight:         " << getStringfromBool(sequence_multiple_in_flight) << '\n'
+       << " - Register RC AUX channels:            " << getStringfromBool(register_aux) << '\n';
 
     if (!activate_user_interface)
     {

diff --git a/src/autonomy_core/autonomy.cpp b/src/autonomy_core/autonomy.cpp
@@ -1137,10 +1137,13 @@ void Autonomy::rcCallback(const mavros_msgs::RCInConstPtr& msg)
     }
   }
 
-  if (aux_registered_ && !register_aux_)
+  if (aux_registered_ && register_aux_)
   {
     // Check for changes with tollerance of 50
-    for (size_t id = 0; id < msg->channels.size(); ++id)
+    // for (size_t id = 0; id < msg->channels.size(); ++id)
+    // {
+    size_t id = opts_->landing_aux_channel;
+    if (msg->channels.size() >= id)
     {
       if (std::abs(msg->channels.at(id) - aux_.getValue(id)) > 50)
       {
@@ -1151,14 +1154,15 @@ void Autonomy::rcCallback(const mavros_msgs::RCInConstPtr& msg)
         aux_.setValue(id, msg->channels.at(id));
 
         // Check if landing aux changed
-        if (id == opts_->landing_aux_channel)
-        {
-          stateTransition("land");
-        }
+        // if (id == opts_->landing_aux_channel)
+        // {
+        stateTransition("land");
+        // }
 
         // Manage other aux
       }
     }
+    // }
   }
 }
 

diff --git a/src/state_machine/states/initialization.cpp b/src/state_machine/states/initialization.cpp
@@ -40,7 +40,8 @@ void Initialization::onEntry(Autonomy& autonomy)
       autonomy.stateTransition("nominal");
     }
   }
-  else if (autonomy.opts_->register_aux)
+
+  if (autonomy.opts_->register_aux)
   {
     if (!autonomy.registerRCAux())
     {