Line data    Source code

       1             : /// @file       AC_P_1D.cpp
       2             : /// @brief      Position-based P controller with optional limits on output and its first derivative.
       3             : 
       4             : #include <AP_Math/AP_Math.h>
       5             : #include "AC_P_1D.h"
       6             : 
       7             : const AP_Param::GroupInfo AC_P_1D::var_info[] = {
       8             :     // @Param: P
       9             :     // @DisplayName: P Proportional Gain
      10             :     // @Description: P Gain which produces an output value that is proportional to the current error value
      11             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P",    0, AC_P_1D, _kp, default_kp),
      12             :     AP_GROUPEND
      13             : };
      14             : 
      15             : // Constructor
      16        1285 : AC_P_1D::AC_P_1D(float initial_p) :
      17        1285 :     default_kp(initial_p)
      18             : {
      19             :     // load parameter values from eeprom
      20        1285 :     AP_Param::setup_object_defaults(this, var_info);
      21        1285 : }
      22             : 
      23             : // Computes the P controller output given a target and measurement.
      24             : // Applies position error clamping based on configured limits.
      25             : // Optionally constrains output slope using the sqrt_controller.
      26    25567102 : float AC_P_1D::update_all(postype_t &target, postype_t measurement)
      27             : {
      28             :     // Compute position error between target and measurement
      29    25567102 :     _error = (float)(target - measurement);
      30             : 
      31             :     // Clamp error to configured min/max bounds
      32    25567102 :     if (is_negative(_error_min) && (_error < _error_min)) {
      33       34268 :         _error = _error_min;
      34       34268 :         target = measurement + _error;
      35    25532834 :     } else if (is_positive(_error_max) && (_error > _error_max)) {
      36       79895 :         _error = _error_max;
      37       79895 :         target = measurement + _error;
      38             :     }
      39             : 
      40             :     // Use sqrt_controller to limit output and/or its derivative
      41    25567102 :     return sqrt_controller(_error, _kp, _D1_max, 0.0);
      42             : }
      43             : 
      44             : // Sets limits on output, output slope (D1), and output acceleration (D2).
      45             : // For position controllers: output = velocity, D1 = acceleration, D2 = jerk.
      46     3219594 : void AC_P_1D::set_limits(float output_min, float output_max, float D_Out_max, float D2_Out_max)
      47             : {
      48             :     // Reset all limits to zero before applying new ones
      49     3219594 :     _D1_max = 0.0f;
      50     3219594 :     _error_min = 0.0f;
      51     3219594 :     _error_max = 0.0f;
      52             : 
      53             :     // Set first derivative (acceleration) limit if specified
      54     3219594 :     if (is_positive(D_Out_max)) {
      55     3219594 :         _D1_max = D_Out_max;
      56             :     }
      57             : 
      58             :     // If second derivative (jerk) limit is specified, constrain velocity limit accordingly
      59     3219594 :     if (is_positive(D2_Out_max) && is_positive(_kp)) {
      60             :         // limit the first derivative so as not to exceed the second derivative
      61           0 :         _D1_max = MIN(_D1_max, D2_Out_max / _kp);
      62             :     }
      63             : 
      64             :     // Compute min/max allowable error from output limits
      65     3219594 :     if (is_negative(output_min) && is_positive(_kp)) {
      66     3219594 :         _error_min = inv_sqrt_controller(output_min, _kp, _D1_max);
      67             :     }
      68     3219594 :     if (is_positive(output_max) && is_positive(_kp)) {
      69     3219594 :         _error_max = inv_sqrt_controller(output_max, _kp, _D1_max);
      70             :     }
      71     3219594 : }
      72             : 
      73             : // Reduces error limits to user-specified bounds, respecting previously computed limits.
      74             : // Intended to be called after `set_limits()`.
      75           0 : void AC_P_1D::set_error_limits(float error_min, float error_max)
      76             : {
      77             :     // Update _error_min if it's non-zero and the new value is more conservative
      78           0 :     if (is_negative(error_min)) {
      79           0 :         if (!is_zero(_error_min)) {
      80           0 :             _error_min = MAX(_error_min, error_min);
      81             :         } else {
      82           0 :             _error_min = error_min;
      83             :         }
      84             :     }
      85             : 
      86             :     // Update _error_max if it's non-zero and the new value is more conservative
      87           0 :     if (is_positive(error_max)) {
      88           0 :         if (!is_zero(_error_max)) {
      89           0 :             _error_max = MIN(_error_max, error_max);
      90             :         } else {
      91           0 :             _error_max = error_max;
      92             :         }
      93             :     }
      94           0 : }