diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt
index 1617f18..0970d6a 100644
--- a/rosplane/CMakeLists.txt
+++ b/rosplane/CMakeLists.txt
@@ -79,7 +79,7 @@ install(TARGETS
 # Planner
 add_executable(rosplane_path_planner
             src/path_planner.cpp)
-ament_target_dependencies(rosplane_path_planner rosplane_msgs rclcpp rclpy Eigen3)
+ament_target_dependencies(rosplane_path_planner rosplane_msgs std_srvs rclcpp rclpy Eigen3)
 install(TARGETS
   rosplane_path_planner
   DESTINATION lib/${PROJECT_NAME})
diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp
index 6859331..70ca413 100644
--- a/rosplane/include/controller_base.hpp
+++ b/rosplane/include/controller_base.hpp
@@ -130,6 +130,7 @@ class controller_base : public rclcpp::Node
     double gravity;              /**< gravity in m/s^2 */
     bool pitch_tuning_debug_override;
     bool roll_tuning_debug_override;
+    double max_roll;
   };
 
   /**
@@ -215,7 +216,8 @@ class controller_base : public rclcpp::Node
           /* mass */ 2.28,
           /* gravity */ 9.8,
           /* pitch_tuning_debug_override*/ false,
-          /* roll_tuning_debug_override*/ false
+          /* roll_tuning_debug_override*/ false,
+          /* max_roll*/ 25.0
     };
 
   /**
diff --git a/rosplane/include/path_follower_base.hpp b/rosplane/include/path_follower_base.hpp
index 9e0b547..7bd18a8 100644
--- a/rosplane/include/path_follower_base.hpp
+++ b/rosplane/include/path_follower_base.hpp
@@ -40,6 +40,7 @@ class path_follower_base : public rclcpp::Node
     float h;                /** altitude */
     float Va;               /** airspeed */
     float chi;              /** course angle */
+    float psi;              /** heading angle */
   };
 
   struct output_s
diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp
index 5b584f1..4f841e4 100644
--- a/rosplane/include/path_manager_base.hpp
+++ b/rosplane/include/path_manager_base.hpp
@@ -67,6 +67,7 @@ class path_manager_base : public rclcpp::Node
   struct params_s
   {
     double R_min;
+    bool orbit_last;
   };
 
   virtual void manage(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
@@ -87,7 +88,10 @@ class path_manager_base : public rclcpp::Node
   void vehicle_state_callback(const rosplane_msgs::msg::State &msg);
   bool state_init_;
   void new_waypoint_callback(const rosplane_msgs::msg::Waypoint &msg);
-  void current_path_publish(); 
+  void current_path_publish();
+
+  OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
+  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters);
 };
 } //end namespace
 #endif // PATH_MANAGER_BASE_H
diff --git a/rosplane/launch/rosplane.launch.py b/rosplane/launch/rosplane.launch.py
index 17efb3c..d957822 100644
--- a/rosplane/launch/rosplane.launch.py
+++ b/rosplane/launch/rosplane.launch.py
@@ -39,6 +39,7 @@ def generate_launch_description():
             package='rosplane',
             executable='rosplane_path_manager',
             name='path_manager',
+            parameters = [autopilot_params],
         ),
         Node(
             package='rosplane',
diff --git a/rosplane/params/succesive_loop_controller_params.yaml b/rosplane/params/succesive_loop_controller_params.yaml
index 998d7e8..cd06d27 100644
--- a/rosplane/params/succesive_loop_controller_params.yaml
+++ b/rosplane/params/succesive_loop_controller_params.yaml
@@ -3,9 +3,9 @@ autopilot: # node name.
     alt_hz: 10.0
     alt_toz: 5.0
     tau: 50.0
-    c_kp: 2.37
+    c_kp: 3.5
     c_kd: 0.0
-    c_ki: 0.4
+    c_ki: 0.1
     r_kp: 0.06
     r_kd: 0.04
     r_ki: 0.0
@@ -39,3 +39,9 @@ autopilot: # node name.
     max_takeoff_throttle: 0.55
     mass: 2.28
     gravity: 9.8
+    max_roll: 25.0
+
+path_manager:
+  ros__parameters:
+    R_min: 100.0
+    orbit_last: True
diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp
index d32e3b2..f7e72b0 100644
--- a/rosplane/src/controller_base.cpp
+++ b/rosplane/src/controller_base.cpp
@@ -196,6 +196,7 @@ void controller_base::actuator_controls_publish()
       this->declare_parameter("gravity", params_.gravity);
       this->declare_parameter("pitch_tuning_debug_override", params_.pitch_tuning_debug_override);
       this->declare_parameter("roll_tuning_debug_override", params_.roll_tuning_debug_override);
+      this->declare_parameter("max_roll", params_.max_roll);
     }
 
 void controller_base::set_parameters() {
@@ -243,6 +244,7 @@ void controller_base::set_parameters() {
   params_.gravity = this->get_parameter("gravity").as_double();
   params_.roll_tuning_debug_override = this->get_parameter("roll_tuning_debug_override").as_bool();
   params_.pitch_tuning_debug_override = this->get_parameter("pitch_tuning_debug_override").as_bool();
+  params_.max_roll = this->get_parameter("max_roll").as_double();
 
 }
 
@@ -295,6 +297,7 @@ rcl_interfaces::msg::SetParametersResult controller_base::parametersCallback(con
     else if (param.get_name() == "gravity") params_.gravity = param.as_double();
     else if (param.get_name() == "roll_tuning_debug_override") params_.roll_tuning_debug_override = param.as_bool();
     else if (param.get_name() == "pitch_tuning_debug_override") params_.pitch_tuning_debug_override = param.as_bool();
+    else if (param.get_name() == "max_roll") params_.max_roll = param.as_double();
     else{
 
       // If the parameter given doesn't match any of the parameters return false.
diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp
index 56da8ec..b8c91de 100644
--- a/rosplane/src/controller_successive_loop.cpp
+++ b/rosplane/src/controller_successive_loop.cpp
@@ -174,7 +174,7 @@ float controller_successive_loop::course_hold(float chi_c, float chi, float phi_
   float ui = params.c_ki*c_integrator_;
   float ud = params.c_kd*r;
 
-  float phi_c = sat(up + ui + ud + phi_ff, 15.0*3.14/180.0, -15.0*3.14/180.0);
+  float phi_c = sat(up + ui + ud + phi_ff, params.max_roll*3.14/180.0, -params.max_roll*3.14/180.0);
   if (fabs(params.c_ki) >= 0.00001)
   {
     float phi_c_unsat = up + ui + ud + phi_ff;
diff --git a/rosplane/src/path_follower_base.cpp b/rosplane/src/path_follower_base.cpp
index b2a8791..8a18d9a 100644
--- a/rosplane/src/path_follower_base.cpp
+++ b/rosplane/src/path_follower_base.cpp
@@ -68,6 +68,7 @@ void path_follower_base::vehicle_state_callback(const rosplane_msgs::msg::State:
   input_.pe = msg->position[1];               /** position east */
   input_.h = -msg->position[2];                /** altitude */
   input_.chi = msg->chi;
+  input_.psi = msg->psi;
   input_.Va = msg->va;
 
   RCLCPP_DEBUG_STREAM(this->get_logger(), "FROM STATE -- input.chi: " << input_.chi);
@@ -95,34 +96,34 @@ void path_follower_base::current_path_callback(const rosplane_msgs::msg::Current
   current_path_init_ = true;
 }
 
-    rcl_interfaces::msg::SetParametersResult path_follower_base::parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
-        rcl_interfaces::msg::SetParametersResult result;
-        result.successful = false;
-        result.reason = "";
-
-        for (const auto &param : parameters) {
-
-            if (param.get_name() == "CHI_INFTY"){
-                params_.chi_infty = param.as_double();
-                result.successful = true;
-                result.reason = "success";
-            }
-            else if (param.get_name() == "K_PATH"){
-                params_.k_path = param.as_double();
-                result.successful = true;
-                result.reason = "success";
-            }
-            else if (param.get_name() == "K_ORBIT"){
-                params_.k_orbit = param.as_double();
-                result.successful = true;
-                result.reason = "success";
-            }
+rcl_interfaces::msg::SetParametersResult path_follower_base::parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = false;
+    result.reason = "";
 
+    for (const auto &param : parameters) {
+
+        if (param.get_name() == "CHI_INFTY"){
+            params_.chi_infty = param.as_double();
+            result.successful = true;
+            result.reason = "success";
+        }
+        else if (param.get_name() == "K_PATH"){
+            params_.k_path = param.as_double();
+            result.successful = true;
+            result.reason = "success";
+        }
+        else if (param.get_name() == "K_ORBIT"){
+            params_.k_orbit = param.as_double();
+            result.successful = true;
+            result.reason = "success";
         }
 
-        return result;
     }
 
+    return result;
+}
+
 } //end namespace
 
 int main(int argc, char **argv)
diff --git a/rosplane/src/path_follower_example.cpp b/rosplane/src/path_follower_example.cpp
index 3564ff6..5c18d79 100644
--- a/rosplane/src/path_follower_example.cpp
+++ b/rosplane/src/path_follower_example.cpp
@@ -1,4 +1,5 @@
 #include "path_follower_example.hpp"
+#include <rclcpp/logging.hpp>
 
 
 namespace rosplane
@@ -54,7 +55,8 @@ void path_follower_example::follow(const params_s &params, const input_s &input,
     // commanded altitude is the height of the orbit
     float h_d = -input.c_orbit[2];
     output.h_c = h_d;
-    output.phi_ff = (norm_orbit_error < 0.5 ? input.lam_orbit*atanf(input.Va*input.Va/(9.8*input.rho_orbit)) : 0);
+    output.phi_ff = input.lam_orbit * std::atan(pow(input.Va, 2)/(9.81*input.rho_orbit*std::cos(input.chi - input.psi)));
+
   }
   output.Va_c = input.Va_d;
 }
diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp
index 14f4e99..cc9091d 100755
--- a/rosplane/src/path_manager_base.cpp
+++ b/rosplane/src/path_manager_base.cpp
@@ -2,6 +2,7 @@
 #include "path_manager_example.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "iostream"
+#include <rclcpp/logging.hpp>
 
 
 namespace rosplane
@@ -15,16 +16,45 @@ path_manager_base::path_manager_base() : Node("rosplane_path_manager")
   update_timer_      = this->create_wall_timer(10ms, std::bind(&path_manager_base::current_path_publish, this));
   // interesting read on wall timer
   // https://answers.ros.org/question/375561/create-wall-timer-using-callback-with-parameters-ros2-c/
+  //
+  // Set the parameter callback, for when parameters are changed.
+  parameter_callback_handle_ = this->add_on_set_parameters_callback(
+            std::bind(&path_manager_base::parametersCallback, this, std::placeholders::_1));
 
   this->declare_parameter("R_min", 25.0);
+  this->declare_parameter("orbit_last", false);
 
   params_.R_min = this->get_parameter("R_min").as_double();
+  params_.orbit_last = this->get_parameter("orbit_last").as_bool();
 
   num_waypoints_ = 0;
 
   state_init_ = false;
 }
 
+rcl_interfaces::msg::SetParametersResult path_manager_base::parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+
+  // Check each parameter in the incoming vector of parameters to change, and change the appropriate parameter.
+  for (const auto &param : parameters) {
+
+    if (param.get_name() == "R_min") params_.R_min = param.as_double();
+    else if (param.get_name() == "orbit_last") params_.orbit_last = param.as_bool();
+    else{
+
+      // If the parameter given doesn't match any of the parameters return false.
+      result.successful = false;
+      result.reason = "One of the parameters given does not is not a parameter of the controller node.";
+      break;
+    }
+
+  }
+
+  return result;
+}
+
 void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State &msg)
 {
 
@@ -131,4 +161,4 @@ int main(int argc, char **argv)
   rclcpp::spin(std::make_shared<rosplane::path_manager_example>());
 
   return 0;
-}
\ No newline at end of file
+}
diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp
index 1c4ecb8..f1d30d7 100755
--- a/rosplane/src/path_manager_example.cpp
+++ b/rosplane/src/path_manager_example.cpp
@@ -3,6 +3,8 @@
 #include "path_manager_example.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include <cmath>
+#include <rclcpp/logging.hpp>
+#include <string>
 
 namespace rosplane
 {
@@ -53,6 +55,34 @@ void path_manager_example::manage_line(const params_s &params, const input_s &in
   int idx_c;
   if (idx_a_ == num_waypoints_ - 1)
   {
+    
+    if (params.orbit_last)
+    {
+      output.flag = false;
+      output.va_d = waypoints_[idx_a_].va_d;
+      output.c[0] = waypoints_[idx_a_].w[0]; 
+      output.c[1] = waypoints_[idx_a_].w[1];
+      output.c[2] = waypoints_[idx_a_].w[2];
+      output.r[0] = 0.0;
+      output.r[1] = 0.0;
+      output.r[2] = 0.0;
+      output.q[0] = 0.0;
+      output.q[1] = 0.0;
+      output.q[2] = 0.0;
+      output.rho = params.R_min;
+
+      if (waypoints_[idx_a_].chi_d < M_PI)
+      {
+        output.lamda = 1; 
+      }
+      else 
+      {
+        output.lamda = -1;
+      }
+
+      return;
+    }
+
     idx_b = 0;
     idx_c = 1;
   }
@@ -64,8 +94,7 @@ void path_manager_example::manage_line(const params_s &params, const input_s &in
   else
   {
     idx_b = idx_a_ + 1;
-    idx_c = idx_b + 1;
-  }
+    idx_c = idx_b + 1; }
 
   Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
   Eigen::Vector3f w_i(waypoints_[idx_b].w);
@@ -86,9 +115,13 @@ void path_manager_example::manage_line(const params_s &params, const input_s &in
   if ((p - w_i).dot(n_i) > 0.0f)
   {
     if (idx_a_ == num_waypoints_ - 1)
+    {
       idx_a_ = 0;
+    }
     else
+    {
       idx_a_++;
+    }
   }
 
 }
@@ -108,13 +141,51 @@ void path_manager_example::manage_fillet(const params_s &params, const input_s &
   int idx_c;
   if (idx_a_ == num_waypoints_ - 1)
   {
+
+    if (params.orbit_last)
+    {
+      output.flag = false;
+      output.va_d = waypoints_[idx_a_].va_d;
+      output.c[0] = waypoints_[idx_a_].w[0]; 
+      output.c[1] = waypoints_[idx_a_].w[1];
+      output.c[2] = waypoints_[idx_a_].w[2];
+      output.r[0] = 0.0;
+      output.r[1] = 0.0;
+      output.r[2] = 0.0;
+      output.q[0] = 0.0;
+      output.q[1] = 0.0;
+      output.q[2] = 0.0;
+      output.rho = params.R_min;
+
+      if (waypoints_[idx_a_].chi_d < M_PI)
+      {
+        output.lamda = 1; 
+      }
+      else 
+      {
+        output.lamda = -1;
+      }
+
+      return;
+    }
+
     idx_b = 0;
     idx_c = 1;
   }
   else if (idx_a_ == num_waypoints_ - 2)
   {
     idx_b = num_waypoints_ - 1;
-    idx_c = 0;
+
+    if (params.orbit_last)
+    {
+      idx_c = 0;
+      idx_a_ = idx_b;
+      return;
+    }
+    else 
+    {
+      idx_c = 0;
+    }
   }
   else
   {
diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp
index e7098fc..7042bf5 100644
--- a/rosplane/src/path_planner.cpp
+++ b/rosplane/src/path_planner.cpp
@@ -1,41 +1,98 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rosplane_msgs/msg/waypoint.hpp"
+#include <rclcpp/executors.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/service.hpp>
+#include <rclcpp/utilities.hpp>
+#include <rosplane_msgs/msg/detail/waypoint__struct.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
-#define num_waypoints 3
+#define num_waypoints 7
+#define num_waypoints_to_publish 3
 
-int main(int argc, char **argv)
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+namespace rosplane {
+  
+class path_planner : public rclcpp::Node
 {
-  rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("rosplane_simple_path_planner");
+public:
+  path_planner();
+  ~path_planner();
+
+private:
+  rclcpp::Publisher<rosplane_msgs::msg::Waypoint>::SharedPtr waypoint_publisher;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr waypoint_service;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  bool publish_next_waypoint(
+    const std_srvs::srv::Trigger::Request::SharedPtr & req,
+    const std_srvs::srv::Trigger::Response::SharedPtr & res);
+  
+  void waypoint_publish();
+  
+  int num_waypoints_published;
+
+  float wps[5*num_waypoints];
+  
+};
+
+path_planner::path_planner() : Node("path_planner") {
 
-  auto waypoint_publisher = node->create_publisher<rosplane_msgs::msg::Waypoint>("waypoint_path", 10); // !!! n vs nh_ example I am following used n
-  rclcpp::Rate loop_rate(10);
-  float va = 20;
-  float wps[5*num_waypoints] =
+  waypoint_publisher = this->create_publisher<rosplane_msgs::msg::Waypoint>("waypoint_path", 10);
+
+  waypoint_service = this->create_service<std_srvs::srv::Trigger>("publish_next_waypoint", std::bind(&path_planner::publish_next_waypoint, this, std::placeholders::_1, std::placeholders::_2));
+  
+  timer_ = this->create_wall_timer(1000ms, std::bind(&path_planner::waypoint_publish, this));
+
+  num_waypoints_published = 0;
+
+  float va = 25;
+  float wps_to_add[5*num_waypoints] =
   {
-    200, 0, -30, 45*M_PI/180, va,
-    0, 200, -30, 45*M_PI/180, va,
-    200, 200, -30, 225*M_PI/180, va,
+    0, 100, -90, 45*M_PI/180, va,
+    0, 300, -90, 45*M_PI/180, va,
+    0, 500, -90, 45*M_PI/180, va,
+    0, 700, -90, 45*M_PI/180, va,
+    0, 900, -90, 45*M_PI/180, va,
+    0, 1100, -90, 45*M_PI/180, va,
+    0, 1300, -90, 45*M_PI/180, va
   };
 
-  rclcpp::Clock wait;
-  for (int i(0); i < num_waypoints; i++)
+  for (int i = 0; i < 5*num_waypoints; i++)
   {
+    wps[i] = wps_to_add[i];
+  }
+
+}
+
+path_planner::~path_planner() {
+}
+
+bool path_planner::publish_next_waypoint(
+  const std_srvs::srv::Trigger::Request::SharedPtr & req,
+  const std_srvs::srv::Trigger::Response::SharedPtr & res)
+{
 
-    wait.sleep_for(rclcpp::Duration(0,int(5e8)));
+  RCLCPP_INFO_STREAM(this->get_logger(), "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published);
 
+  if (num_waypoints_published < num_waypoints)
+  {
     rosplane_msgs::msg::Waypoint new_waypoint;
 
-    rclcpp::Time now = node->get_clock()->now();
+    rclcpp::Time now = this->get_clock()->now();
 
     new_waypoint.header.stamp = now;
 
+    int i = num_waypoints_published;
+
     new_waypoint.w[0] = wps[i*5 + 0];
     new_waypoint.w[1] = wps[i*5 + 1];
     new_waypoint.w[2] = wps[i*5 + 2];
     new_waypoint.chi_d = wps[i*5 + 3];
 
-    new_waypoint.chi_valid = true;
+    new_waypoint.chi_valid = false;
     new_waypoint.va_d = wps[i*5 + 4];
     if (i == 0)
       new_waypoint.set_current = true;
@@ -43,17 +100,59 @@ int main(int argc, char **argv)
       new_waypoint.set_current = false;
     new_waypoint.clear_wp_list = false;
 
-    RCLCPP_DEBUG_STREAM(node->get_logger(), "Publishing Waypoint!");
+    waypoint_publisher->publish(new_waypoint);
+
+    num_waypoints_published++;
+
+  }
+
+  return true;
+}
+
+void path_planner::waypoint_publish()
+{
+
+  if (num_waypoints_published < num_waypoints_to_publish)
+  {
+    
+    rosplane_msgs::msg::Waypoint new_waypoint;
 
-    RCLCPP_DEBUG_STREAM(node->get_logger(), "North: " << new_waypoint.w[0]);
-    RCLCPP_DEBUG_STREAM(node->get_logger(), "East: " << new_waypoint.w[1]);
-    RCLCPP_DEBUG_STREAM(node->get_logger(), "Down: " << new_waypoint.w[2]);
-    RCLCPP_DEBUG_STREAM(node->get_logger(), "chi_d: " << new_waypoint.chi_d);
+    rclcpp::Time now = this->get_clock()->now();
+
+    new_waypoint.header.stamp = now;
+
+    int i = num_waypoints_published;
+
+    new_waypoint.w[0] = wps[i*5 + 0];
+    new_waypoint.w[1] = wps[i*5 + 1];
+    new_waypoint.w[2] = wps[i*5 + 2];
+    new_waypoint.chi_d = wps[i*5 + 3];
 
+    new_waypoint.chi_valid = false;
+    new_waypoint.va_d = wps[i*5 + 4];
+    if (i == 0)
+      new_waypoint.set_current = true;
+    else
+      new_waypoint.set_current = false;
+    new_waypoint.clear_wp_list = false;
 
     waypoint_publisher->publish(new_waypoint);
+
+    num_waypoints_published++;
+
   }
 
-  wait.sleep_for(rclcpp::Duration(1,int(5e8)));
+
+}
+}
+
+int main(int argc, char **argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<rosplane::path_planner>();
+
+  rclcpp::spin(node);
+
   return 0;
 }