Added latching of orbit direction for orbit following.

rosflight · Jun 11, 2024 · 6667eaf · 6667eaf
1 parent b6423a0
commit 6667eaf
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 1 deletion.
diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp
@@ -46,6 +46,7 @@ class path_manager_base : public rclcpp::Node
   int idx_a_; /** index to the waypoint that was most recently achieved */
 
   bool temp_waypoint_ = false;
+  int orbit_dir = 0;
 
   struct input_s
   {

diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp
@@ -57,6 +57,7 @@ void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State &
 void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg)
 {
   double R_min = params.get_double("R_min");
+  orbit_dir = 0;
 
   if (msg.clear_wp_list == true) {
     waypoints_.clear();

diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp
@@ -563,6 +563,11 @@ void path_manager_example::declare_parameters()
 
 int path_manager_example::orbit_direction(float pn, float pe, float chi, float c_n, float c_e)
 {
+  if (orbit_dir != 0)
+  {
+    return orbit_dir;
+  }
+
   Eigen::Vector3f pos;
   pos << pn, pe, 0.0;
 
@@ -576,9 +581,11 @@ int path_manager_example::orbit_direction(float pn, float pe, float chi, float c
 
   if (d.cross(course)(2) >= 0 ) 
   {
+    orbit_dir = 1;
     return 1;
   }
-
+
+  orbit_dir = -1;
   return -1;
 }