Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file   AC_P_2D.h
       4             : /// @brief  2D P controller with output limiting, derivative constraint, and EEPROM-backed gain storage.
       5             : 
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_Param/AP_Param.h>
       8             : 
       9             : /// @class  AC_P_2D
      10             : /// @brief  2-axis P controller
      11             : class AC_P_2D {
      12             : public:
      13             : 
      14             :     /// Constructor for 2D P controller with initial gain.
      15             :     AC_P_2D(float initial_p);
      16             : 
      17             :     CLASS_NO_COPY(AC_P_2D);
      18             : 
      19             :     // Computes the P controller output given a target and measurement.
      20             :     // Applies position error clamping based on configured limits.
      21             :     // Optionally constrains output slope using the sqrt_controller.
      22             :     Vector2f update_all(Vector2p &target, const Vector2p &measurement) WARN_IF_UNUSED;
      23             : 
      24             :     // Sets limits on output, output slope (D1), and output acceleration (D2).
      25             :     // For position controllers: output = velocity, D1 = acceleration, D2 = jerk.
      26             :     void set_limits(float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f);
      27             : 
      28             :     // Reduces the maximum allowable error after limit initialization.
      29             :     // Intended to be called after set_limits().
      30             :     void set_error_max(float error_max);
      31             : 
      32             :     // Returns the current error clamp limit in controller units.
      33             :     float get_error_max() const { return _error_max; }
      34             : 
      35             :     // Saves controller configuration from EEPROM. (not used)
      36             :     void save_gains() { _kp.save(); }
      37             : 
      38             :     // get accessors
      39     6431476 :     AP_Float &kP() WARN_IF_UNUSED { return _kp; }
      40       38565 :     const AP_Float &kP() const WARN_IF_UNUSED { return _kp; }
      41    35833620 :     const Vector2f& get_error() const { return _error; }
      42             : 
      43             :     // set accessors
      44           0 :     void set_kP(float v) { _kp.set(v); }
      45             : 
      46             :     // parameter var table
      47             :     static const struct AP_Param::GroupInfo var_info[];
      48             : 
      49             : private:
      50             : 
      51             :     // parameters
      52             :     AP_Float    _kp;
      53             : 
      54             :     // internal variables
      55             :     Vector2f _error;    // error between target and measured
      56             :     float _error_max;   // error limit in positive direction
      57             :     float _D1_max;      // maximum first derivative of output
      58             : 
      59             :     const float default_kp;
      60             : };