Line data    Source code

       1             : #ifndef CONTROLS_CONTROLLER_FF_FB_VEHICLE_CONTROLLER_H
       2             : #define CONTROLS_CONTROLLER_FF_FB_VEHICLE_CONTROLLER_H
       3             : 
       4             : #include <controller/BuoyancyController.h>
       5             : #include <controller/FeedbackController.h>
       6             : #include <controller/FeedforwardController.h>
       7             : 
       8             : #include "controller/VehicleControllerInterface.h"
       9             : 
      10             : /**
      11             :  * @brief Vehicle controller combining feed-forward, PID feedback, and buoyancy compensation
      12             :  *
      13             :  * The total force is: ff_gain * FF + fb_gain * FB + buoyancy_gain * B,
      14             :  * scaled per-axis by axis_gain.  The feedback controller's integral term and
      15             :  * the buoyancy controller's adaptive bias are snapshotted at trajectory start
      16             :  * and restored on finish or stationkeep entry to prevent corruption across
      17             :  * trajectory segments.
      18             :  */
      19           1 : class FfFbVehicleController : public VehicleControllerInterface {
      20             : private:
      21             :     /** @brief Open-loop inverse-dynamics feed-forward controller */
      22             :     FeedforwardController ff_controller;
      23             : 
      24             :     /** @brief Closed-loop PID feedback controller */
      25             :     FeedbackController fb_controller;
      26             : 
      27             :     /** @brief Buoyancy/gravity restoring force controller (static model + optional adaptive bias) */
      28             :     BuoyancyController buoyancy_controller;
      29             : 
      30             :     /** @brief Unused; retained for ABI compatibility */
      31             :     double isSaturated;
      32             : 
      33             : public:
      34             :     /**
      35             :      * @brief Constructs the combined controller with shared dynamics and config
      36             :      *
      37             :      * @param systemProperties  [in] Shared vehicle dynamics model
      38             :      * @param controller_config [in] Shared controller tuning parameters
      39             :      */
      40           1 :     FfFbVehicleController(std::shared_ptr<SystemDynamicsBase> systemProperties,
      41             :                           std::shared_ptr<ControllerConfig> controller_config);
      42             : 
      43             :     /**
      44             :      * @brief Computes the total 6DOF body force as a weighted sum of all sub-controllers
      45             :      *
      46             :      * @param goal                          [in] Desired vehicle state
      47             :      * @param current                       [in] Current vehicle state
      48             :      * @param saturatedOnYawXYAxis          [in] True when X, Y, or Yaw axes are saturated
      49             :      * @param saturatedOnDepthRollPitchAxis [in] True when Z, Roll, or Pitch axes are saturated
      50             :      * @param dt                            [in] Time step in seconds
      51             :      *
      52             :      * @return 6-element body force/torque vector [Fx, Fy, Fz, Tx, Ty, Tz] in N and Nm
      53             :      */
      54           1 :     Vector6d spinControllerOnce(const State& goal, const State& current, const bool saturatedOnYawXYAxis,
      55             :                                 const bool saturatedOnDepthRollPitchAxis, double dt) override;
      56             : 
      57             :     /**
      58             :      * @brief Returns the most recently computed body-frame pose error from the feedback controller
      59             :      *
      60             :      * @return 6-element pose error vector
      61             :      */
      62           1 :     Vector6d getBodyPoseError() const override;
      63             : 
      64             :     /**
      65             :      * @brief Saves the feedback integral and buoyancy bias before trajectory execution begins
      66             :      */
      67           1 :     void onTrajectoryStarting() override;
      68             : 
      69             :     /**
      70             :      * @brief Restores the saved integral and buoyancy bias after a trajectory finishes naturally
      71             :      */
      72           1 :     void onTrajectoryFinished() override;
      73             : 
      74             :     /**
      75             :      * @brief Restores the saved integral and buoyancy bias when stationkeep is entered
      76             :      */
      77           1 :     void onStationkeepEntered() override;
      78             : 
      79             :     /**
      80             :      * @brief Ramps the feedback integral from its current value toward the saved snapshot
      81             :      *
      82             :      * @param progress [in] Fractional trajectory completion in [0, 1]
      83             :      */
      84           1 :     void onTrajectoryProgress(double progress) override;
      85             : };
      86             : 
      87             : #endif  // CONTROLS_CONTROLLER_FF_FB_VEHICLE_CONTROLLER_H