ardupilot/libraries/AC_PID/AC_P_2D.h

66 lines
2.1 KiB
C
Raw Normal View History

#pragma once
/// @file AC_P_2D.h
/// @brief 2-axis P controller with EEPROM-backed storage of constants.
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
/// @class AC_P_2D
/// @brief 2-axis P controller
class AC_P_2D {
public:
// constructor
AC_P_2D(float initial_p, float dt);
2021-06-06 05:33:35 -03:00
CLASS_NO_COPY(AC_P_2D);
// set time step in seconds
void set_dt(float dt) { _dt = dt; }
// set target and measured inputs to P controller and calculate outputs
Vector2f update_all(float &target_x, float &target_y, const Vector2f &measurement, bool &limit) WARN_IF_UNUSED;
// set target and measured inputs to P controller and calculate outputs
// measurement is provided as 3-axis vector but only x and y are used
Vector2f update_all(float &target_x, float &target_y, const Vector3f &measurement, bool &limit) WARN_IF_UNUSED {
return update_all(target_x, target_y, Vector2f{measurement.x, measurement.y}, limit);
}
// set_limits - sets the maximum error to limit output and first and second derivative of output
void set_limits(float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f);
// set_error_max - reduce maximum position error to error_max
// to be called after setting limits
void set_error_max(float error_max);
// get_error_max - return maximum position error
float get_error_max() { return _error_max; }
// save gain to eeprom
void save_gains() { _kp.save(); }
// get accessors
AP_Float &kP() WARN_IF_UNUSED { return _kp; }
const AP_Float &kP() const WARN_IF_UNUSED { return _kp; }
const Vector2f& get_error() const { return _error; }
// set accessors
void kP(float v) { _kp.set(v); }
// parameter var table
static const struct AP_Param::GroupInfo var_info[];
private:
// parameters
AP_Float _kp;
// internal variables
float _dt; // time step in seconds
Vector2f _error; // time step in seconds
float _error_max; // error limit in positive direction
float _D1_max; // maximum first derivative of output
};