Skip to content

Commit

Permalink
Merge pull request #66 from rosflight/rename-classes-enums-structs
Browse files Browse the repository at this point in the history
Naming, headers, and declaration changes to match style guide
  • Loading branch information
bsutherland333 authored Jul 11, 2024
2 parents ea240ed + a4afb3c commit 7a4ced0
Show file tree
Hide file tree
Showing 29 changed files with 868 additions and 862 deletions.
30 changes: 15 additions & 15 deletions rosplane/include/controller_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,14 +11,14 @@
#define CONTROLLER_BASE_H

#include <chrono>
#include <cstring>
#include <ament_index_cpp/get_package_share_directory.hpp>

#include <rclcpp/rclcpp.hpp>
#include <rosflight_msgs/msg/command.hpp>
#include <rosplane_msgs/msg/controller_commands.hpp>
#include <rosplane_msgs/msg/controller_internals.hpp>
#include <rosplane_msgs/msg/state.hpp>
#include <param_manager.hpp>

#include "param_manager.hpp"
#include "rosplane_msgs/msg/controller_commands.hpp"
#include "rosplane_msgs/msg/controller_internals.hpp"
#include "rosplane_msgs/msg/state.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;
Expand All @@ -28,7 +28,7 @@ namespace rosplane
/**
* This defines the different portions of the control algorithm.
*/
enum class alt_zones
enum class AltZones
{
TAKE_OFF, /**< In the take off zone where the aircraft gains speed and altitude */
CLIMB, /**< In the climb zone the aircraft proceeds to commanded altitude without course change. */
Expand All @@ -39,13 +39,13 @@ enum class alt_zones
* This class implements all of the basic functionality of a controller interfacing with ROS2.
*/

class controller_base : public rclcpp::Node
class ControllerBase : public rclcpp::Node
{
public:
/**
* Constructor for ROS2 setup and parameter initialization.
*/
controller_base();
ControllerBase();

/**
* Gets the current phi_c value from the current private command message.
Expand All @@ -68,7 +68,7 @@ class controller_base : public rclcpp::Node
/**
* This struct holds all of the inputs to the control algorithm.
*/
struct input_s
struct Input
{
float Ts; /**< time step */
float h; /**< altitude */
Expand All @@ -88,28 +88,28 @@ class controller_base : public rclcpp::Node
/**
* This struct holds all of the outputs of the control algorithm.
*/
struct output_s
struct Output
{
float theta_c; /**< The commanded pitch angle from the altitude control loop */
float phi_c; /**< The commanded roll angle from the course control loop */
float delta_e; /**< The commanded elevator deflection */
float delta_a; /**< The commanded aileron deflection */
float delta_r; /**< The commanded rudder deflection */
float delta_t; /**< The commanded throttle deflection */
alt_zones current_zone; /**< The current altitude zone for the control */
AltZones current_zone; /**< The current altitude zone for the control */
};

/**
* Parameter manager object. Contains helper functions to interface parameters with ROS.
*/
param_manager params;
ParamManager params_;

/**
* Interface for control algorithm.
* @param input Inputs to the control algorithm.
* @param output Outputs of the controller, including selected intermediate values and final control efforts.
*/
virtual void control(const struct input_s & input, struct output_s & output) = 0;
virtual void control(const Input & input, Output & output) = 0;

private:
/**
Expand Down Expand Up @@ -165,7 +165,7 @@ class controller_base : public rclcpp::Node
/**
* Convert from deflection angle in radians to pwm.
*/
void convert_to_pwm(struct output_s & output);
void convert_to_pwm(Output & output);

/**
* Calls the control function and publishes outputs and intermediate values to the command and controller internals
Expand Down
22 changes: 11 additions & 11 deletions rosplane/include/controller_state_machine.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,49 +6,49 @@
namespace rosplane
{

class controller_state_machine : public controller_base
class ControllerStateMachine : public ControllerBase
{

public:
controller_state_machine();
ControllerStateMachine();

/**
* The state machine for the control algorithm for the autopilot.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void control(const struct input_s & input,
struct output_s & output);
virtual void control(const Input & input,
Output & output);

protected:
/**
* The current zone of the control algorithm based on altitude.
*/
alt_zones current_zone;
AltZones current_zone_;

/**
* This function continually loops while the aircraft is in the take-off zone. It is implemented by the child.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void take_off(const struct input_s & input,
struct output_s & output) = 0;
virtual void take_off(const Input & input,
Output & output) = 0;

/**
* This function continually loops while the aircraft is in the climb zone. It is implemented by the child.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void climb(const struct input_s & input,
struct output_s & output) = 0;
virtual void climb(const Input & input,
Output & output) = 0;

/**
* This function continually loops while the aircraft is in the altitude hold zone. It is implemented by the child.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void altitude_hold(const struct input_s & input,
struct output_s & output) = 0;
virtual void altitude_hold(const Input & input,
Output & output) = 0;

/**
* This function runs when the aircraft exits the take-off zone this is often used to reset integrator values. It is
Expand Down
38 changes: 19 additions & 19 deletions rosplane/include/controller_successive_loop.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,13 @@
namespace rosplane
{

class controller_successive_loop : public controller_state_machine
class ControllerSucessiveLoop : public ControllerStateMachine
{
public:
/**
* Constructor to initialize node.
*/
controller_successive_loop();
ControllerSucessiveLoop();

protected:
/**
Expand All @@ -21,26 +21,26 @@ class controller_successive_loop : public controller_state_machine
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void take_off(const struct input_s & input,
struct output_s & output);
virtual void take_off(const Input & input,
Output & output);

/**
* This function continually loops while the aircraft is in the climb zone. The lateral and longitudinal control
* for the climb zone is called in this function.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void climb(const struct input_s & input,
struct output_s & output);
virtual void climb(const Input & input,
Output & output);

/**
* This function continually loops while the aircraft is in the altitude hold zone. The lateral and longitudinal
* control for the altitude hold zone is called in this function.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void altitude_hold(const struct input_s & input,
struct output_s & output);
virtual void altitude_hold(const Input & input,
Output & output);

/**
* This function runs when the aircraft exits the take-off zone. Any changes to the controller that need to happen
Expand All @@ -66,45 +66,45 @@ class controller_successive_loop : public controller_state_machine
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void alt_hold_lateral_control(const struct input_s & input, struct output_s & output);
virtual void alt_hold_lateral_control(const Input & input, Output & output);

/**
* This function runs the longitudinal control loops for the altitude hold zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void alt_hold_longitudinal_control(const struct input_s & input,
struct output_s & output);
virtual void alt_hold_longitudinal_control(const Input & input,
Output & output);

/**
* This function runs the lateral control loops for the climb zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void climb_lateral_control(const struct input_s & input,
struct output_s & output);
virtual void climb_lateral_control(const Input & input,
Output & output);

/**
* This function runs the longitudinal control loops for the climb zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void climb_longitudinal_control(const struct input_s & input, struct output_s & output);
virtual void climb_longitudinal_control(const Input & input, Output & output);

/**
* This function runs the lateral control loops for the take-off zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void take_off_lateral_control(const struct input_s & input, struct output_s & output);
virtual void take_off_lateral_control(const Input & input, Output & output);

/**
* This function runs the longitudinal control loops for the take-off zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void take_off_longitudinal_control(const struct input_s & input,
struct output_s & output);
virtual void take_off_longitudinal_control(const Input & input,
Output & output);

/**
* The control loop for moving to and holding a commanded course.
Expand Down Expand Up @@ -216,8 +216,8 @@ class controller_successive_loop : public controller_state_machine
// float ct_differentiator_;
float yaw_damper(float r);

float delta_r_delay;
float r_delay;
float delta_r_delay_;
float r_delay_;

/**
* Saturate a given value to a maximum or minimum of the limits.
Expand Down
20 changes: 10 additions & 10 deletions rosplane/include/controller_total_energy.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,37 +6,37 @@
namespace rosplane
{

class controller_total_energy : public controller_successive_loop
class ControllerTotalEnergy : public ControllerSucessiveLoop
{
public:
/**
* Constructor to initialize node.
*/
controller_total_energy();
ControllerTotalEnergy();

protected:
/**
* This function overrides the longitudinal control loops for the take-off zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void take_off_longitudinal_control(const struct input_s & input,
struct output_s & output);
virtual void take_off_longitudinal_control(const Input & input,
Output & output);

/**
* This function overrides the longitudinal control loops for the climb zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void climb_longitudinal_control(const struct input_s & input, struct output_s & output);
virtual void climb_longitudinal_control(const Input & input, Output & output);

/**
* This function overrides the longitudinal control loops for the altitude hold zone.
* @param input The command inputs to the controller such as course and airspeed.
* @param output The control efforts calculated and selected intermediate values.
*/
virtual void alt_hold_longitudinal_control(const struct input_s & input,
struct output_s & output);
virtual void alt_hold_longitudinal_control(const Input & input,
Output & output);

/**
* This function overrides when the aircraft exits the take-off zone. Any changes to the controller that need to happen
Expand Down Expand Up @@ -101,17 +101,17 @@ class controller_total_energy : public controller_successive_loop
/**
* This is the current reference (desired) kinetic energy.
*/
float K_ref;
float K_ref_;

/**
* This is the current error in the kinetic energy.
*/
float K_error;
float K_error_;

/**
* This is the current error in the potential energy.
*/
float U_error;
float U_error_;

/**
* The previous error in the energy balance.
Expand Down
Loading

0 comments on commit 7a4ced0

Please sign in to comment.