diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index e81f0c1..f3ea5ff 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -120,7 +121,7 @@ class controller_base : public rclcpp::Node /** * This publisher publishes the current commands in the control algorithm. */ - rclcpp::Publisher::SharedPtr controller_commands_pub_; + rclcpp::Publisher::SharedPtr controller_internals_pub_; /** * This subscriber subscribes to the commands the controller uses to calculate control effort. diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index 0034f2b..2b894f0 100644 --- a/rosplane/src/controller_base.cpp +++ b/rosplane/src/controller_base.cpp @@ -13,8 +13,8 @@ controller_base::controller_base() // Advertise published topics. actuators_pub_ = this->create_publisher("command", 10); - controller_commands_pub_ = this->create_publisher( - "controller_commands_out", 10); + controller_internals_pub_ = this->create_publisher( + "controller_internals", 10); // Advertise subscribed topics and set bound callbacks. controller_commands_sub_ = this->create_subscription( @@ -120,25 +120,24 @@ void controller_base::actuator_controls_publish() actuators_pub_->publish(actuators); // Publish the current control values - rosplane_msgs::msg::ControllerCommands controller_commands; - controller_commands.header.stamp = now; - controller_commands.phi_c = output.phi_c; - controller_commands.theta_c = output.theta_c; + rosplane_msgs::msg::ControllerInternals controller_internals; + controller_internals.header.stamp = now; + controller_internals.phi_c = output.phi_c; + controller_internals.theta_c = output.theta_c; switch (output.current_zone) { case alt_zones::TAKE_OFF: - controller_commands.alt_zone = controller_commands.ZONE_TAKE_OFF; + controller_internals.alt_zone = controller_internals.ZONE_TAKE_OFF; break; case alt_zones::CLIMB: - controller_commands.alt_zone = controller_commands.ZONE_CLIMB; + controller_internals.alt_zone = controller_internals.ZONE_CLIMB; break; case alt_zones::ALTITUDE_HOLD: - controller_commands.alt_zone = controller_commands.ZONE_ALTITUDE_HOLD; + controller_internals.alt_zone = controller_internals.ZONE_ALTITUDE_HOLD; break; default: break; } - controller_commands.aux_valid = false; - controller_commands_pub_->publish(controller_commands); + controller_internals_pub_->publish(controller_internals); } } diff --git a/rosplane_msgs/CMakeLists.txt b/rosplane_msgs/CMakeLists.txt index 99941eb..ca6b44f 100644 --- a/rosplane_msgs/CMakeLists.txt +++ b/rosplane_msgs/CMakeLists.txt @@ -27,6 +27,7 @@ find_package(rosidl_default_generators REQUIRED) set(msg_files "msg/Command.msg" "msg/ControllerCommands.msg" + "msg/ControllerInternals.msg" "msg/CurrentPath.msg" "msg/State.msg" "msg/Waypoint.msg" diff --git a/rosplane_msgs/msg/ControllerCommands.msg b/rosplane_msgs/msg/ControllerCommands.msg index cf15dee..4bc9e2b 100644 --- a/rosplane_msgs/msg/ControllerCommands.msg +++ b/rosplane_msgs/msg/ControllerCommands.msg @@ -15,9 +15,3 @@ bool aux_valid # Auxiliary commands valid # parameter for the controller to use these commands. float32 theta_c # Commanded pitch (rad) float32 phi_c # Commanded roll (rad) -uint8 alt_zone # Zone in the altitude state machine - -uint8 ZONE_TAKE_OFF = 0 -uint8 ZONE_CLIMB = 1 -uint8 ZONE_DESEND = 2 -uint8 ZONE_ALTITUDE_HOLD = 3 diff --git a/rosplane_msgs/msg/ControllerInternals.msg b/rosplane_msgs/msg/ControllerInternals.msg new file mode 100644 index 0000000..408bd6a --- /dev/null +++ b/rosplane_msgs/msg/ControllerInternals.msg @@ -0,0 +1,12 @@ +# Internal controller commands output from the outer loops of the autopilot + +# header +std_msgs/Header header +float32 theta_c # Commanded pitch (rad) +float32 phi_c # Commanded roll (rad) +uint8 alt_zone # Zone in the altitude state machine + +uint8 ZONE_TAKE_OFF = 0 +uint8 ZONE_CLIMB = 1 +uint8 ZONE_DESEND = 2 +uint8 ZONE_ALTITUDE_HOLD = 3 diff --git a/rosplane_tuning/README.md b/rosplane_tuning/README.md index adf77b8..24071a0 100644 --- a/rosplane_tuning/README.md +++ b/rosplane_tuning/README.md @@ -10,7 +10,7 @@ Signal generator is a ROS2 node that will generate step inputs, square waves, si This is useful for tuning autopilots as we can give a clear and repeatable command to any portion of the autopilot and observe its response to that input. We can then tune gains, re-issue the same commands, and observe whether performance improved or worsened. -Signal generator works by publishing autopilot commands on the ROS network on the `controller_commands` and `tuning_debug` topics. Each control output (roll, pitch, altitude, course, airspeed) has a default values that is set by a ROS parameter. The signal generator will then add the generated signal to one of the control outputs with a set magnitude and frequency. +Signal generator works by publishing autopilot commands on the ROS network on the `controller_commands` topic. Each control output (roll, pitch, altitude, course, airspeed) has a default values that is set by a ROS parameter. The signal generator will then add the generated signal to one of the control outputs with a set magnitude and frequency. ### Signal Types - Step: This is a non-continuous signal, where the generated signal will jump between the default value and the default+magnitude every time the `toggle_step_signal` service is called.