Line data    Source code

       1             : #ifndef CONTROLS_CONTROLLER_CONTROLLER_CONFIG_H
       2             : #define CONTROLS_CONTROLLER_CONTROLLER_CONFIG_H
       3             : 
       4             : #include <utils/Transforms.h>
       5             : 
       6             : /**
       7             :  * @brief Tuning parameters for the combined feed-forward / feedback controller
       8             :  *
       9             :  * All gain arrays follow the axis ordering [X, Y, Z, Roll, Pitch, Yaw].
      10             :  */
      11           1 : struct ControllerConfig {
      12             :     /** @brief Proportional PID gains for underwater operation */
      13             :     Array6d Kp = Array6d::Zero();
      14             : 
      15             :     /** @brief Integral PID gains for underwater operation */
      16             :     Array6d Ki = Array6d::Zero();
      17             : 
      18             :     /** @brief Derivative PID gains for underwater operation */
      19             :     Array6d Kd = Array6d::Zero();
      20             : 
      21             :     /** @brief Proportional PID gains used when the vehicle is near the surface */
      22             :     Array6d Kp_surface = Array6d::Zero();
      23             : 
      24             :     /** @brief Integral PID gains used when the vehicle is near the surface */
      25             :     Array6d Ki_surface = Array6d::Zero();
      26             : 
      27             :     /** @brief Derivative PID gains used when the vehicle is near the surface */
      28             :     Array6d Kd_surface = Array6d::Zero();
      29             : 
      30             :     /** @brief Scalar weight applied to the feed-forward force contribution */
      31             :     double ff_gain = 0.0;
      32             : 
      33             :     /** @brief Scalar weight applied to the feedback (PID) force contribution */
      34             :     double fb_gain = 1.0;
      35             : 
      36             :     /** @brief Scalar weight applied to the buoyancy compensation force contribution */
      37             :     double buoyancy_gain = 0.0;
      38             : 
      39             :     /** @brief Enables switching between surface and underwater PID gains based on depth */
      40             :     bool surface_gain_scheduling = false;
      41             : 
      42             :     /** @brief Depth threshold in metres below which surface gains are applied */
      43             :     double surface_threshold = 0.1;
      44             : 
      45             :     /** @brief Per-axis output scaling applied after all controller contributions are summed */
      46             :     Array6d axis_gain = Array6d::Ones();
      47             : 
      48             :     // ── Adaptive buoyancy bias estimation (heave) ──────────────────────────
      49             : 
      50             :     /** @brief Enables the adaptive heave bias (learns and applies the residual buoyancy force) */
      51             :     bool buoyancy_adaptive_z = false;
      52             : 
      53             :     /** @brief Adaptation rate for the heave bias estimator (N per m-s of depth error) */
      54             :     double buoyancy_adaptive_rate = 0.5;
      55             : 
      56             :     /** @brief Maximum adaptive heave bias as a fraction of net buoyancy force (e.g. 0.15 = 15%) */
      57             :     double buoyancy_adaptive_clamp_pct = 0.15;
      58             : 
      59             :     /** @brief Depth error threshold for steady-state detection on heave (m) */
      60             :     double buoyancy_adaptive_pos_thresh = 0.15;
      61             : 
      62             :     /** @brief Heave velocity threshold for steady-state detection (m/s); <= 0 disables the gate */
      63             :     double buoyancy_adaptive_vel_thresh = 0.05;
      64             : 
      65             :     // ── Adaptive buoyancy bias estimation (roll/pitch) ─────────────────────
      66             :     // Independent of the heave switch. Static G(q) torque is never output on
      67             :     // roll/pitch; only this adaptive bias. Roll and pitch share these values.
      68             : 
      69             :     /** @brief Enables the adaptive roll/pitch bias */
      70             :     bool buoyancy_adaptive_rp = false;
      71             : 
      72             :     /** @brief Roll/pitch bias adaptation speed (higher learns faster) */
      73             :     double buoyancy_adaptive_rate_rp = 5.0;
      74             : 
      75             :     /** @brief Attitude error threshold for steady-state detection on roll/pitch (deg) */
      76             :     double buoyancy_adaptive_pos_thresh_rp = 0.2;
      77             : 
      78             :     /** @brief Body angular-rate threshold for steady-state detection on roll/pitch (deg/s); <= 0 disables the gate */
      79             :     double buoyancy_adaptive_vel_thresh_rp = 1.0;
      80             : };
      81             : 
      82             : #endif  // CONTROLS_CONTROLLER_CONTROLLER_CONFIG_H