Line data    Source code

       1             : #ifndef CONTROLS_CONTROLLER_BUOYANCY_CONTROLLER_H
       2             : #define CONTROLS_CONTROLLER_BUOYANCY_CONTROLLER_H
       3             : 
       4             : #include <controller/BaseControllerInterface.h>
       5             : 
       6             : /**
       7             :  * @brief Buoyancy/gravity restoring compensation with optional adaptive bias
       8             :  *
       9             :  * Heave outputs static G(q) plus an adaptive bias (`buoyancy_adaptive_z`).
      10             :  * Roll/pitch output only an adaptive bias (`buoyancy_adaptive_rp`); their static
      11             :  * G(q) torque is dropped to preserve the passive metacentric righting. X/Y and
      12             :  * yaw are uncompensated. The bias is a conditional integrator updated only while
      13             :  * stationkeeping. Outputs zero near the surface under gain scheduling.
      14             :  */
      15           1 : class BuoyancyController : public BaseControllerInterface {
      16             :     static constexpr double MAX_DT            = 0.1;
      17             :     static constexpr double EXPLODE_THRESHOLD = 1e4;
      18             : 
      19             :     /** @brief Largest plausible horizontal CoG/CoB offset (m); bounds the roll/pitch bias torque */
      20             :     static constexpr double MAX_CG_CB_OFFSET = 0.01;
      21             : 
      22             : private:
      23             :     /** @brief Accumulated adaptive restoring bias [Fx, Fy, Fz, Tx, Ty, Tz]; only Z/Roll/Pitch are used */
      24             :     Vector6d buoyancy_bias_ = Vector6d::Zero();
      25             : 
      26             :     /** @brief True while the vehicle is stationkeeping; adaptation only runs in this mode */
      27             :     bool is_stationkeeping_ = false;
      28             : 
      29             :     /**
      30             :      * @brief Advances one axis of the conditional integrator in place
      31             :      *
      32             :      * Integrates error_sig only while the axis is settled and not converging,
      33             :      * then clamps.
      34             :      *
      35             :      * @param bias       [in,out] Accumulated bias for this axis (N or Nm)
      36             :      * @param error_sig  [in] Signed error driving the bias (positive => needs more bias)
      37             :      * @param rate       [in] Body velocity/rate on this axis, sharing error_sig's sign
      38             :      * @param adapt_rate [in] Integration rate
      39             :      * @param pos_thresh [in] Deadband on |error_sig| below which no adaptation occurs
      40             :      * @param vel_thresh [in] Settled threshold on |rate|; <= 0 disables the settled gate
      41             :      * @param clamp      [in] Symmetric magnitude clamp on the resulting bias
      42             :      * @param dt         [in] Time step in seconds
      43             :      *
      44             :      * @return True if the bias was updated this step
      45             :      */
      46             :     static bool adapt_axis(double& bias, double error_sig, double rate, double adapt_rate, double pos_thresh,
      47             :                            double vel_thresh, double clamp, double dt);
      48             : 
      49             : public:
      50           0 :     using BaseControllerInterface::BaseControllerInterface;
      51             : 
      52             :     /**
      53             :      * @brief Resets the adaptive buoyancy bias to zero
      54             :      */
      55           1 :     void reset() override;
      56             : 
      57             :     /**
      58             :      * @brief Sets whether the vehicle is stationkeeping (gates adaptation on/off)
      59             :      */
      60           1 :     void set_stationkeeping(bool stationkeeping);
      61             : 
      62             :     /**
      63             :      * @brief Computes the buoyancy compensation wrench
      64             :      *
      65             :      * @param goal [in] Desired vehicle state
      66             :      * @param curr [in] Current vehicle state
      67             :      * @param dt   [in] Time step in seconds
      68             :      *
      69             :      * @return 6-element force/torque vector [Fx, Fy, Fz, Tx, Ty, Tz] in N and Nm
      70             :      */
      71           1 :     Vector6d get_force(const State& goal, const State& curr, double dt) override;
      72             : };
      73             : 
      74             : #endif  // CONTROLS_CONTROLLER_BUOYANCY_CONTROLLER_H