mirror of https://github.com/ArduPilot/ardupilot
42 lines
1.4 KiB
C++
42 lines
1.4 KiB
C++
|
/// @file AC_P_2D.cpp
|
||
|
|
/// @brief 2-axis P controller
|
||
|
|
|
||
|
|
#include <AP_Math/AP_Math.h>
|
||
|
|
#include "AC_P_2D.h"
|
||
|
|
|
||
|
|
const AP_Param::GroupInfo AC_P_2D::var_info[] = {
|
||
|
|
// @Param: P
|
||
|
|
// @DisplayName: Proportional Gain
|
||
|
|
// @Description: P Gain which produces an output value that is proportional to the current error value
|
||
|
|
AP_GROUPINFO("P", 0, AC_P_2D, _kp, 0),
|
||
|
|
AP_GROUPEND
|
||
|
|
};
|
||
|
|
|
||
|
|
// Constructor
|
||
|
|
AC_P_2D::AC_P_2D(float initial_p, float dt) :
|
||
|
|
_dt(dt)
|
||
|
|
{
|
||
|
|
// load parameter values from eeprom
|
||
|
|
AP_Param::setup_object_defaults(this, var_info);
|
||
|
|
|
||
|
|
_kp = initial_p;
|
||
|
|
}
|
||
|
|
|
||
|
|
// set target and measured inputs to P controller and calculate outputs
|
||
|
|
Vector2f AC_P_2D::update_all(float &target_x, float &target_y, const Vector2f &measurement, float error_max, float D2_max)
|
||
|
|
{
|
||
|
|
// calculate distance _error
|
||
|
|
Vector2f error = Vector2f(target_x, target_y) - measurement;
|
||
|
|
|
||
|
|
// Constrain _error and target position
|
||
|
|
// Constrain the maximum length of _vel_target to the maximum position correction velocity
|
||
|
|
if (error.limit_length(error_max)) {
|
||
|
|
target_x = measurement.x + error.x;
|
||
|
|
target_y = measurement.y + error.y;
|
||
|
|
}
|
||
|
|
|
||
|
|
// MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded
|
||
|
|
// return sqrt_controller(Vector2f(_error.x, _error.y), _kp, MIN(_D_max, _D2_max / _kp), _dt);
|
||
|
|
return sqrt_controller(error, _kp, D2_max, _dt);
|
||
|
|
}
|