ran clang-format

.clang-format

@@ -8,12 +8,6 @@ AlignOperands: DontAlign
 AllowAllArgumentsOnNextLine: false
 AllowAllConstructorInitializersOnNextLine: false
 AllowAllParametersOfDeclarationOnNextLine: false
-AllowShortBlocksOnASingleLine: Always
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: Always
-AllowShortLambdasOnASingleLine: All
-AllowShortLoopsOnASingleLine: true
 AlwaysBreakAfterReturnType: None
 AlwaysBreakTemplateDeclarations: Yes
 BreakBeforeBraces: Custom

rosplane/include/controller_base.hpp

@@ -64,7 +64,6 @@ class ControllerBase : public rclcpp::Node
   float get_theta_c() { return controller_commands_.theta_c; };
 
 protected:
-
   /**
    * This struct holds all of the inputs to the control algorithm.
    */
@@ -90,12 +89,12 @@ class ControllerBase : public rclcpp::Node
    */
   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 */
+    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 */
     AltZones current_zone; /**< The current altitude zone for the control */
   };
 
@@ -136,7 +135,7 @@ class ControllerBase : public rclcpp::Node
    * This timer controls how often commands are published by the autopilot.
    */
   rclcpp::TimerBase::SharedPtr timer_;
-  
+
   /**
    * Period of the timer that controlls how often commands are published.
    */

rosplane/include/controller_state_machine.hpp

@@ -17,8 +17,7 @@ class ControllerStateMachine : public ControllerBase
  * @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 Input & input,
-                       Output & output);
+  virtual void control(const Input & input, Output & output);
 
 protected:
   /**
@@ -31,24 +30,21 @@ class ControllerStateMachine : public ControllerBase
    * @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 Input & input,
-                        Output & 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 Input & input,
-                     Output & 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 Input & input,
-                             Output & 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

rosplane/include/controller_successive_loop.hpp

@@ -21,26 +21,23 @@ class ControllerSucessiveLoop : public ControllerStateMachine
    * @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 Input & input,
-                        Output & 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 Input & input,
-                     Output & 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 Input & input,
-                             Output & 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
@@ -73,16 +70,14 @@ class ControllerSucessiveLoop : public ControllerStateMachine
    * @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 Input & input,
-                                             Output & 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 Input & input,
-                                     Output & output);
+  virtual void climb_lateral_control(const Input & input, Output & output);
 
   /**
    * This function runs the longitudinal control loops for the climb zone.
@@ -103,8 +98,7 @@ class ControllerSucessiveLoop : public ControllerStateMachine
    * @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 Input & input,
-                                             Output & output);
+  virtual void take_off_longitudinal_control(const Input & input, Output & output);
 
   /**
    * The control loop for moving to and holding a commanded course.
@@ -219,7 +213,7 @@ class ControllerSucessiveLoop : public ControllerStateMachine
   float delta_r_delay_;
   float r_delay_;
 
- /**
+  /**
  * Saturate a given value to a maximum or minimum of the limits.
  * @param value The value to saturate.
  * @param up_limit The maximum the value can take on.

rosplane/include/controller_total_energy.hpp

@@ -20,8 +20,7 @@ class ControllerTotalEnergy : public ControllerSucessiveLoop
    * @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 Input & input,
-                                             Output & output);
+  virtual void take_off_longitudinal_control(const Input & input, Output & output);
 
   /**
    * This function overrides the longitudinal control loops for the climb zone.
@@ -35,8 +34,7 @@ class ControllerTotalEnergy : public ControllerSucessiveLoop
    * @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 Input & input,
-                                             Output & 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

rosplane/include/estimator_continuous_discrete.hpp

@@ -41,38 +41,57 @@ class EstimatorContinuousDiscrete : public EstimatorEKF
   float Vwhat_;
   float phihat_;
   float thetahat_;
-  float psihat_;     // TODO: link to an inital condiditons param
-
-
-  Eigen::VectorXf attitude_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements);
-  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> attitude_dynamics_model;
-
-  Eigen::MatrixXf attitude_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates);
-  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> attitude_jacobian_model;
-
-  Eigen::MatrixXf attitude_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates);
-  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> attitude_input_jacobian_model;
-
-  Eigen::VectorXf attitude_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs);
-  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> attitude_measurement_model;
-
-  Eigen::MatrixXf attitude_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs);
-  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)> attitude_measurement_jacobian_model;
-
-  Eigen::VectorXf position_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements);
-  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> position_dynamics_model;
-
-  Eigen::MatrixXf position_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements);
-  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> position_jacobian_model;
-
-  Eigen::MatrixXf position_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs);
-  std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> position_input_jacobian_model;
-
-  Eigen::VectorXf position_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& input);
-  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> position_measurement_model;
-
-  Eigen::MatrixXf position_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& input);
-  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)> position_measurement_jacobian_model;
+  float psihat_; // TODO: link to an inital condiditons param
+
+  Eigen::VectorXf attitude_dynamics(const Eigen::VectorXf & state,
+                                    const Eigen::VectorXf & measurements);
+  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)>
+    attitude_dynamics_model;
+
+  Eigen::MatrixXf attitude_jacobian(const Eigen::VectorXf & state,
+                                    const Eigen::VectorXf & angular_rates);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf &, const Eigen::VectorXf &)>
+    attitude_jacobian_model;
+
+  Eigen::MatrixXf attitude_input_jacobian(const Eigen::VectorXf & state,
+                                          const Eigen::VectorXf & angular_rates);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf &, const Eigen::VectorXf &)>
+    attitude_input_jacobian_model;
+
+  Eigen::VectorXf attitude_measurement_prediction(const Eigen::VectorXf & state,
+                                                  const Eigen::VectorXf & inputs);
+  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)>
+    attitude_measurement_model;
+
+  Eigen::MatrixXf attitude_measurement_jacobian(const Eigen::VectorXf & state,
+                                                const Eigen::VectorXf & inputs);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)>
+    attitude_measurement_jacobian_model;
+
+  Eigen::VectorXf position_dynamics(const Eigen::VectorXf & state,
+                                    const Eigen::VectorXf & measurements);
+  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)>
+    position_dynamics_model;
+
+  Eigen::MatrixXf position_jacobian(const Eigen::VectorXf & state,
+                                    const Eigen::VectorXf & measurements);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf &, const Eigen::VectorXf &)>
+    position_jacobian_model;
+
+  Eigen::MatrixXf position_input_jacobian(const Eigen::VectorXf & state,
+                                          const Eigen::VectorXf & inputs);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf &, const Eigen::VectorXf &)>
+    position_input_jacobian_model;
+
+  Eigen::VectorXf position_measurement_prediction(const Eigen::VectorXf & state,
+                                                  const Eigen::VectorXf & input);
+  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)>
+    position_measurement_model;
+
+  Eigen::MatrixXf position_measurement_jacobian(const Eigen::VectorXf & state,
+                                                const Eigen::VectorXf & input);
+  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)>
+    position_measurement_jacobian_model;
 
   Eigen::Vector2f xhat_a_; // 2
   Eigen::Matrix2f P_a_;    // 2x2
@@ -119,7 +138,7 @@ class EstimatorContinuousDiscrete : public EstimatorEKF
    * @brief Initializes the state covariance matrix with the ROS2 parameters
    */
   void initialize_state_covariances();
-}; 
+};
 
 } // namespace rosplane
 

rosplane/include/estimator_ekf.hpp

@@ -19,34 +19,30 @@ class EstimatorEKF : public EstimatorROS
   EstimatorEKF();
 
 protected:
-  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> measurement_update(Eigen::VectorXf x,
-                                                                  Eigen::VectorXf inputs,
-                                                                  std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> measurement_model,
-                                                                  Eigen::VectorXf y,
-                                                                  std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)> measurement_jacobian,
-                                                                  Eigen::MatrixXf R,
-                                                                  Eigen::MatrixXf P);
-
-  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> propagate_model(Eigen::VectorXf x,
-                                                               std::function<Eigen::VectorXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> dynamic_model,
-                                                               std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> jacobian,
-                                                               Eigen::VectorXf inputs,
-                                                               std::function<Eigen::MatrixXf(const Eigen::VectorXf&, const Eigen::VectorXf&)> input_jacobian,
-                                                               Eigen::MatrixXf P,
-                                                               Eigen::MatrixXf Q,
-                                                               Eigen::MatrixXf Q_g,
-                                                               float Ts);          
-
-  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> single_measurement_update(float measurement,
-                                                                         float mesurement_prediction,
-                                                                         float measurement_uncertainty,
-                                                                         Eigen::VectorXf measurement_jacobian,
-                                                                         Eigen::VectorXf x,
-                                                                         Eigen::MatrixXf P);
+  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> measurement_update(
+    Eigen::VectorXf x, Eigen::VectorXf inputs,
+    std::function<Eigen::VectorXf(const Eigen::VectorXf, const Eigen::VectorXf)> measurement_model,
+    Eigen::VectorXf y,
+    std::function<Eigen::MatrixXf(const Eigen::VectorXf, const Eigen::VectorXf)>
+      measurement_jacobian,
+    Eigen::MatrixXf R, Eigen::MatrixXf P);
+
+  std::tuple<Eigen::MatrixXf, Eigen::VectorXf> propagate_model(
+    Eigen::VectorXf x,
+    std::function<Eigen::VectorXf(const Eigen::VectorXf &, const Eigen::VectorXf &)> dynamic_model,
+    std::function<Eigen::MatrixXf(const Eigen::VectorXf &, const Eigen::VectorXf &)> jacobian,
+    Eigen::VectorXf inputs,
+    std::function<Eigen::MatrixXf(const Eigen::VectorXf &, const Eigen::VectorXf &)> input_jacobian,
+    Eigen::MatrixXf P, Eigen::MatrixXf Q, Eigen::MatrixXf Q_g, float Ts);
+
+  std::tuple<Eigen::MatrixXf, Eigen::VectorXf>
+  single_measurement_update(float measurement, float mesurement_prediction,
+                            float measurement_uncertainty, Eigen::VectorXf measurement_jacobian,
+                            Eigen::VectorXf x, Eigen::MatrixXf P);
 
 private:
   virtual void estimate(const Input & input, Output & output) override = 0;
-}; 
+};
 
 } // namespace rosplane
 

rosplane/include/estimator_ros.hpp

@@ -82,15 +82,14 @@ class EstimatorROS : public rclcpp::Node
 
   bool baro_init_; /**< Initial barometric pressure */
 
-  virtual void estimate(const Input & input,
-                        Output & output) = 0;
+  virtual void estimate(const Input & input, Output & output) = 0;
 
   ParamManager params_;
   bool gps_init_;
-  double init_lat_ = 0.0;                 /**< Initial latitude in degrees */
-  double init_lon_ = 0.0;                 /**< Initial longitude in degrees */
-  float init_alt_ = 0.0;                  /**< Initial altitude in meters above MSL  */
-  float init_static_;                     /**< Initial static pressure (mbar)  */
+  double init_lat_ = 0.0; /**< Initial latitude in degrees */
+  double init_lon_ = 0.0; /**< Initial longitude in degrees */
+  float init_alt_ = 0.0;  /**< Initial altitude in meters above MSL  */
+  float init_static_;     /**< Initial static pressure (mbar)  */
 
 private:
   rclcpp::Publisher<rosplane_msgs::msg::State>::SharedPtr vehicle_state_pub_;

rosplane/include/param_manager/param_manager.hpp

@@ -15,7 +15,7 @@
 
 namespace rosplane
 {
-    
+
 class ParamManager
 {
 public:
@@ -66,7 +66,7 @@ class ParamManager
    * Helper function to declare parameters in the param_manager object
    * Inserts a parameter into the parameter object and declares it with the ROS system
   */
-  void declare_int(std::string param_name, int64_t value);  
+  void declare_int(std::string param_name, int64_t value);
 
   /**
    * Helper function to declare parameters in the param_manager object
@@ -103,7 +103,7 @@ class ParamManager
    * and the ROS system.
    */
   void set_string(std::string param_name, std::string value);
-  
+
   /**
    * This function should be called in the parametersCallback function in a containing ROS node.
    * It takes in a vector of changed parameters and updates them within the params_ object.
@@ -121,5 +121,5 @@ class ParamManager
   rclcpp::Node * container_node_;
 };
 
-}   // namespace rosplane
+} // namespace rosplane
 #endif // PARAM_MANAGER_H