From cbf706e9aee98902a2a387ebc51b671ecc2199ed Mon Sep 17 00:00:00 2001
From: Martin Scheiber <martin.scheiber@aau.at>
Date: Fri, 20 Oct 2023 18:09:56 +0200
Subject: [PATCH] [FIX] tracking of single channel for landing

---
 include/autonomy_core/autonomy_options.h    |  3 ++-
 src/autonomy_core/autonomy.cpp              | 16 ++++++++++------
 src/state_machine/states/initialization.cpp | 19 +++++--------------
 3 files changed, 17 insertions(+), 21 deletions(-)

diff --git a/include/autonomy_core/autonomy_options.h b/include/autonomy_core/autonomy_options.h
index 506ad9a..74ee8e2 100644
--- a/include/autonomy_core/autonomy_options.h
+++ b/include/autonomy_core/autonomy_options.h
@@ -115,7 +115,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
index 2df036e..5d838bb 100644
--- a/src/autonomy_core/autonomy.cpp
+++ b/src/autonomy_core/autonomy.cpp
@@ -1198,10 +1198,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)
       {
@@ -1212,14 +1215,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
index f0137d7..8daa1e0 100644
--- a/src/state_machine/states/initialization.cpp
+++ b/src/state_machine/states/initialization.cpp
@@ -35,27 +35,18 @@ void Initialization::onEntry(Autonomy& autonomy)
     {
       autonomy.stateTransition("failure");
     }
-    else
-    {
-      autonomy.stateTransition("nominal");
-    }
   }
-  else if (autonomy.opts_->register_aux)
+
+  if (autonomy.opts_->register_aux)
   {
     if (!autonomy.registerRCAux())
     {
       autonomy.stateTransition("failure");
     }
-    else
-    {
-      autonomy.stateTransition("nominal");
-    }
-  }
-  else
-  {
-    // Transition to nominal
-    autonomy.stateTransition("nominal");
   }
+
+  // Transition to nominal
+  autonomy.stateTransition("nominal");
 }
 
 void Initialization::onExit(Autonomy&)