Line data    Source code

       1             : #pragma once
       2             : 
       3             : /// @file       AC_P_1D.h
       4             : /// @brief      Position-based P controller with optional limits on output and its first derivative.
       5             : 
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_Param/AP_Param.h>
       8             : 
       9             : /// @class      AC_P_1D
      10             : /// @brief      Object managing one P controller
      11             : class AC_P_1D {
      12             : public:
      13             : 
      14             :     /// Constructor for 1D P controller with initial gain.
      15             :     AC_P_1D(float initial_p);
      16             : 
      17             :     CLASS_NO_COPY(AC_P_1D);
      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             :     float update_all(postype_t &target, postype_t 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_min, float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f);
      27             : 
      28             :     // Reduces error limits to user-specified bounds, respecting previously computed limits.
      29             :     // Intended to be called after `set_limits()`.
      30             :     void set_error_limits(float error_min, float error_max);
      31             : 
      32             :     // Returns the current minimum error clamp, in controller units.
      33             :     float get_error_min() const { return _error_min; }
      34             : 
      35             :     // Returns the current maximum error clamp, in controller units.
      36             :     float get_error_max() const { return _error_max; }
      37             : 
      38             :     // Saves controller configuration from EEPROM. (not used)
      39             :     void save_gains() { _kp.save(); }
      40             : 
      41             :     // accessors
      42    13474461 :     AP_Float &kP() WARN_IF_UNUSED { return _kp; }
      43       65872 :     const AP_Float &kP() const WARN_IF_UNUSED { return _kp; }
      44    72504970 :     float get_error() const { return _error; }
      45             : 
      46             :     // set accessors
      47           0 :     void set_kP(float v) { _kp.set(v); }
      48             : 
      49             :     // parameter var table
      50             :     static const struct AP_Param::GroupInfo var_info[];
      51             : 
      52             : private:
      53             : 
      54             :     // parameters
      55             :     AP_Float _kp;
      56             : 
      57             :     // internal variables
      58             :     float _error;       // time step in seconds
      59             :     float _error_min; // error limit in negative direction
      60             :     float _error_max; // error limit in positive direction
      61             :     float _D1_max;      // maximum first derivative of output
      62             : 
      63             :     const float default_kp;
      64             : };