Merge branch 'main' into mixer-computation

rosplane/include/path_manager_example.hpp

@@ -37,6 +37,8 @@ class PathManagerExample : public PathManagerBase
   std::chrono::time_point<std::chrono::system_clock> start_time_;
   FilletState fil_state_;
 
+  bool first_;
+
   /**
    * @brief Determines the line type and calculates the line parameters to publish to path_follower
    */

rosplane/src/controller_successive_loop.cpp

@@ -250,11 +250,27 @@ float ControllerSucessiveLoop::roll_hold(float phi_c, float phi, float p)
   float up = r_kp * error;
   float ui = r_ki * r_integrator;
   float ud = r_kd * p;
+
+  if (std::isnan(up)) {
+    up = 0.0;
+    RCLCPP_WARN(this->get_logger(), "Proportional control on the roll loop is NAN");
+  }
+
+  if (std::isnan(ui)) {
+    r_integrator = 0.0;
+    ui = 0.0;
+    RCLCPP_WARN(this->get_logger(), "Integral control on the roll loop is NAN");
+  }
+
+  if (std::isnan(ud)) {
+    ud = 0.0;
+    RCLCPP_WARN(this->get_logger(), "Derivative control on the roll loop is NAN");
+  }
 
   float delta_a = sat(trim_a / pwm_rad_a + up + ui - ud, max_a, -max_a);
   float delta_a_unsat = trim_a / pwm_rad_a + up + ui - ud;
 
-  if (fabs(delta_a - delta_a_unsat) > 0.0001) {
+  if (fabs(delta_a - delta_a_unsat) > 0.0001 && fabs(r_ki) > 0.00001) {
     r_integrator = r_integrator_prev;
   }
 
@@ -284,11 +300,27 @@ float ControllerSucessiveLoop::pitch_hold(float theta_c, float theta, float q)
   float up = p_kp * error;
   float ui = p_ki * p_integrator_;
   float ud = p_kd * q;
+
+  if (std::isnan(up)) {
+    up = 0.0;
+    RCLCPP_WARN(this->get_logger(), "Proportional control on the roll loop is NAN");
+  }
+
+  if (std::isnan(ui)) {
+    p_integrator_ = 0.0;
+    ui = 0.0;
+    RCLCPP_WARN(this->get_logger(), "Integral control on the roll loop is NAN");
+  }
+
+  if (std::isnan(ud)) {
+    ud = 0.0;
+    RCLCPP_WARN(this->get_logger(), "Derivative control on the roll loop is NAN");
+  }
 
   float delta_e = sat(trim_e / pwm_rad_e + up + ui - ud, max_e, -max_e);
   float delta_e_unsat = trim_e / pwm_rad_e + up + ui - ud;
 
-  if (fabs(delta_e - delta_e_unsat) > 0.0001) {
+  if (fabs(delta_e - delta_e_unsat) > 0.0001 && fabs(p_ki) > 0.00001) {
     p_integrator_ = p_integrator_prev;
   }
 

rosplane/src/estimator_ros.cpp

@@ -53,6 +53,9 @@ EstimatorROS::EstimatorROS()
 
   param_filepath_ = full_path.string();
 
+  input_.diff_pres = 0.0; // Initalize the differential_pressure measurement to zero.
+  input_.static_pres = 0.0; // Initalize the differential_pressure measurement to zero.
+
   set_timer();
 }
 

rosplane/src/path_manager_base.cpp

@@ -172,6 +172,16 @@ void PathManagerBase::current_path_publish()
   input.chi = vehicle_state_.chi;
 
   Output output;
+  output.va_d = 0;
+  output.r[0] = 0;
+  output.r[1] = 0;
+  output.r[2] = 0;
+  output.q[0] = 0;
+  output.q[1] = 0;
+  output.q[2] = 0;
+  output.c[0] = 0;
+  output.c[1] = 0;
+  output.c[2] = 0;
 
   if (state_init_ == true) {
     manage(input, output);

rosplane/src/path_manager_example.cpp

@@ -20,6 +20,8 @@ PathManagerExample::PathManagerExample()
   params_.set_parameters();
 
   start_time_ = std::chrono::system_clock::now();
+
+  first_ = true;
 }
 
 void PathManagerExample::manage(const Input & input, Output & output)
@@ -154,7 +156,6 @@ void PathManagerExample::manage_fillet(const Input & input, Output & output)
   increment_indices(idx_a_, idx_b, idx_c, input, output);
 
   if (orbit_last && idx_a_ == num_waypoints_ - 1) {
-    // TODO: check to see if this is the correct behavior.
     return;
   }
 
@@ -399,6 +400,14 @@ void PathManagerExample::manage_dubins(const Input & input, Output & output)
         } else {
           idx_a_++;
           idx_b = idx_a_ + 1;
+
+          if (first_) {
+            first_ = false;
+            waypoints_.erase(waypoints_.begin());
+            num_waypoints_--;
+            idx_a_--;
+            idx_b = idx_a_ + 1;
+          }
         }
 
         // plan new Dubin's path to next waypoint configuration