diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 9e0dba63b4..90fb0cb845 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -28,6 +28,26 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // @User: Advanced AP_GROUPINFO("SPEED", 2, AC_PrecLand, _speed_xy, AC_PRECLAND_SPEED_XY_DEFAULT), + // @Param: VEL_P + // @DisplayName: Precision landing velocity controller P gain + // @Description: Precision landing velocity controller P gain + // @Range: 0.100 5.000 + // @User: Advanced + + // @Param: VEL_I + // @DisplayName: Precision landing velocity controller I gain + // @Description: Precision landing velocity controller I gain + // @Range: 0.100 5.000 + // @User: Advanced + + // @Param: VEL_IMAX + // @DisplayName: Precision landing velocity controller I gain maximum + // @Description: Precision landing velocity controller I gain maximum + // @Range: 0 1000 + // @Units: cm/s + // @User: Standard + AP_SUBGROUPINFO(_pi_vel_xy, "VEL_", 3, AC_PrecLand, AC_PI_2D), + AP_GROUPEND }; @@ -35,11 +55,10 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav, - AC_PI_2D& pi_precland_xy, float dt) : +AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav, float dt) : _ahrs(ahrs), _inav(inav), - _pi_precland_xy(pi_precland_xy), + _pi_vel_xy(PRECLAND_P, PRECLAND_I, PRECLAND_IMAX, PRECLAND_FILT_HZ, dt), _dt(dt), _have_estimate(false), _backend(NULL) diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 4650928831..1b3d627a31 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -12,6 +12,7 @@ #define PRECLAND_P 2.0f // velocity controller P gain default #define PRECLAND_I 1.0f // velocity controller I gain default #define PRECLAND_IMAX 500.0f // velocity controller IMAX default +#define PRECLAND_FILT_HZ 5.0f // velocity controller filter hz #define PRECLAND_UPDATE_TIME 0.02f // precland runs at 50hz // declare backend classes @@ -43,7 +44,7 @@ public: }; // Constructor - AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav, AC_PI_2D& pi_precland_xy, float dt); + AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav, float dt); // init - perform any required initialisation of landing controllers void init(); @@ -83,7 +84,7 @@ private: // references to inertial nav and ahrs libraries const AP_AHRS& _ahrs; const AP_InertialNav& _inav; - AC_PI_2D& _pi_precland_xy; // horizontal velocity PI controller + AC_PI_2D _pi_vel_xy; // horizontal velocity PI controller // parameters AP_Int8 _enabled; // enabled/disabled and behaviour