Line data    Source code

       1             : #ifndef CONTROLS_CONTROLLER_FEEDBACK_CONTROLLER_H
       2             : #define CONTROLS_CONTROLLER_FEEDBACK_CONTROLLER_H
       3             : 
       4             : #include <controller/BaseControllerInterface.h>
       5             : 
       6             : /**
       7             :  * @brief PID feedback controller with anti-windup integral clamping
       8             :  *
       9             :  * Computes proportional, integral, and derivative forces from the body-frame
      10             :  * pose, integral, and twist errors respectively.  Separate gain sets can be
      11             :  * activated near the water surface.  The integral term is clamped when any
      12             :  * thruster axis is saturated to prevent windup.
      13             :  */
      14           1 : class FeedbackController : public BaseControllerInterface {
      15             :     static constexpr double MAX_DT                = 0.1;
      16             :     static constexpr double EXPLODE_THRESHOLD     = 1e4;
      17             :     static constexpr double INTEGRAL_CLAMP        = 5.0;
      18             :     static constexpr double INTEGRAL_DECAY_FACTOR = 0.5;
      19             : 
      20             : private:
      21             :     double error_saturation_limit = 1;
      22             : 
      23             :     /** @brief Full 18-element state error [pose(6), twist(6), accel(6)] */
      24             :     Vector18d e = Vector18d::Zero();
      25             : 
      26             :     Vector6d body_integral_error = Vector6d::Zero();
      27             :     Vector6d body_pose_error     = Vector6d::Zero();
      28             :     Vector6d body_twist_error    = Vector6d::Zero();
      29             : 
      30             :     /** @brief Running integral of the pose error in the body frame */
      31             :     Vector6d integral = Vector6d::Zero();
      32             : 
      33             :     /**
      34             :      * @brief Snapshot of the integral taken at trajectory start
      35             :      *
      36             :      * Restored when the trajectory finishes so the stationkeep integrator is
      37             :      * not corrupted by in-flight trajectory tracking errors.
      38             :      */
      39             :     Vector6d saved_integral = Vector6d::Zero();
      40             : 
      41             :     Vector6d force_i = Vector6d::Zero();
      42             :     Vector6d force_p = Vector6d::Zero();
      43             :     Vector6d force_d = Vector6d::Zero();
      44             : 
      45             : public:
      46           0 :     using BaseControllerInterface::BaseControllerInterface;
      47             : 
      48             :     /** @brief True when the X, Y, or Yaw thruster axes are saturated */
      49             :     bool saturatedOnYawXYAxis = false;
      50             : 
      51             :     /** @brief True when the Z, Roll, or Pitch thruster axes are saturated */
      52             :     bool saturatedOnDepthRollPitchAxis = false;
      53             : 
      54             :     /**
      55             :      * @brief Resets all internal state including the integral term
      56             :      */
      57           1 :     void reset() override;
      58             : 
      59             :     /**
      60             :      * @brief Resets all internal state except the depth (Z) integral term
      61             :      */
      62           1 :     void resetAllExceptDepthIntegral();
      63             : 
      64             :     /**
      65             :      * @brief Computes the PID feedback force from the goal and current state
      66             :      *
      67             :      * @param goal [in] Desired vehicle state
      68             :      * @param curr [in] Current vehicle state
      69             :      * @param dt   [in] Time step in seconds used for integral accumulation
      70             :      *
      71             :      * @return 6-element PID force/torque vector in N and Nm
      72             :      */
      73           1 :     Vector6d getForce(const State& goal, const State& curr, double dt) override;
      74             : 
      75             :     /**
      76             :      * @brief Saves a snapshot of the current integral term for later restoration
      77             :      *
      78             :      * The snapshot is zeroed if either thruster axis group is saturated.
      79             :      */
      80           1 :     void save_integral();
      81             : 
      82             :     /**
      83             :      * @brief Restores the integral term from the most recent saved snapshot
      84             :      */
      85           1 :     void load_integral();
      86             : 
      87             :     /**
      88             :      * @brief Interpolates the integral term between its current value and the saved snapshot
      89             :      *
      90             :      * @param ratio [in] Blending ratio in [0, 1]; 1.0 fully loads the saved integral
      91             :      */
      92           1 :     void ramp_integral(double ratio);
      93             : 
      94             :     /**
      95             :      * @brief Returns the most recently computed body-frame integral error
      96             :      *
      97             :      * @return 6-element integral error vector
      98             :      */
      99           1 :     Vector6d getIntegralError() const;
     100             : 
     101             :     /**
     102             :      * @brief Returns the most recently computed body-frame pose error
     103             :      *
     104             :      * @return 6-element pose error vector
     105             :      */
     106           1 :     Vector6d getPoseError() const;
     107             : 
     108             :     /**
     109             :      * @brief Returns the most recently computed body-frame twist error
     110             :      *
     111             :      * @return 6-element twist error vector
     112             :      */
     113           1 :     Vector6d getTwistError() const;
     114             : 
     115             :     /**
     116             :      * @brief Returns the most recently computed integral force contribution
     117             :      *
     118             :      * @return 6-element integral force vector
     119             :      */
     120           1 :     Vector6d getIntegralForce() const;
     121             : 
     122             :     /**
     123             :      * @brief Directly sets the integral accumulator
     124             :      *
     125             :      * @param i [in] New integral value
     126             :      */
     127           1 :     void setIntegral(const Vector6d& i);
     128             : };
     129             : 
     130             : #endif  // CONTROLS_CONTROLLER_FEEDBACK_CONTROLLER_H