Line data    Source code

       1             : /// @file   AC_P_2D.cpp
       2             : /// @brief  2D P controller with output limiting, derivative constraint, and EEPROM-backed gain storage.
       3             : 
       4             : #include <AP_Math/AP_Math.h>
       5             : #include "AC_P_2D.h"
       6             : 
       7             : const AP_Param::GroupInfo AC_P_2D::var_info[] = {
       8             :     // @Param: P
       9             :     // @DisplayName: Proportional Gain
      10             :     // @Description: Proportional gain that generates output based on the current error value
      11             :     AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P",    0, AC_P_2D, _kp, default_kp),
      12             :     AP_GROUPEND
      13             : };
      14             : 
      15             : // Constructor
      16        1570 : AC_P_2D::AC_P_2D(float initial_p) :
      17        1570 :     default_kp(initial_p)
      18             : {
      19             :     // load parameter values from eeprom
      20        1570 :     AP_Param::setup_object_defaults(this, var_info);
      21        1570 : }
      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    19442525 : Vector2f AC_P_2D::update_all(Vector2p &target, const Vector2p &measurement)
      27             : {
      28             :     // Compute vector error between target and measurement (NED frame)
      29    19442525 :     _error = (target - measurement).tofloat();
      30             : 
      31             :     // Limit error vector length to prevent exceeding output constraints
      32    19442525 :     if (is_positive(_error_max) && _error.limit_length(_error_max)) {
      33       87832 :         target = measurement + _error.topostype();
      34             :     }
      35             : 
      36             :     // Use sqrt_controller to limit output and/or its derivative
      37    19442525 :     return sqrt_controller(_error, _kp, _D1_max, 0.0);
      38             : }
      39             : 
      40             : // Sets limits on output, output slope (D1), and output acceleration (D2).
      41             : // For position controllers: output = velocity, D1 = acceleration, D2 = jerk.
      42     2739062 : void AC_P_2D::set_limits(float output_max, float D_Out_max, float D2_Out_max)
      43             : {
      44             :     // Reset all limits to zero before applying new ones
      45     2739062 :     _D1_max = 0.0f;
      46     2739062 :     _error_max = 0.0f;
      47             : 
      48             :     // Set first derivative (acceleration) limit if specified
      49     2739062 :     if (is_positive(D_Out_max)) {
      50     2739062 :         _D1_max = D_Out_max;
      51             :     }
      52             : 
      53             :     // If second derivative (jerk) limit is specified, constrain velocity limit accordingly
      54     2739062 :     if (is_positive(D2_Out_max) && is_positive(_kp)) {
      55             :         // limit the first derivative so as not to exceed the second derivative
      56         148 :         _D1_max = MIN(_D1_max, D2_Out_max / _kp);
      57             :     }
      58             : 
      59             :     // Compute min/max allowable error from output limits
      60     2739062 :     if (is_positive(output_max) && is_positive(_kp)) {
      61     2739062 :         _error_max = inv_sqrt_controller(output_max, _kp, _D1_max);
      62             :     }
      63     2739062 : }
      64             : 
      65             : // Reduces the maximum allowable error after limit initialization.
      66             : // Intended to be called after set_limits().
      67     1380446 : void AC_P_2D::set_error_max(float error_max)
      68             : {
      69             :     // Update _error_min if it's non-zero and the new value is more conservative
      70     1380446 :     if (is_positive(error_max)) {
      71     1380446 :         if (!is_zero(_error_max) ) {
      72     1380446 :             _error_max = MIN(_error_max, error_max);
      73             :         } else {
      74           0 :             _error_max = error_max;
      75             :         }
      76             :     }
      77     1380446 : }