diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index 74796d5862..89a0ec0970 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -41,8 +41,7 @@ #include #include -#include - +#include #pragma once namespace Controller @@ -77,12 +76,20 @@ struct Constraints { * * A setpoint that is NAN is considered as not set. */ -class PositionControl +class PositionControl : public ModuleParams { public: - PositionControl(); - ~PositionControl() {}; + PositionControl(ModuleParams *parent); + ~PositionControl() = default; + + /** + * Overwrites certain parameters. + * Overwrites are required for unit-conversion. + * This method should only be called if parameters + * have been updated. + */ + void overwriteParams(); /** * Update the current vehicle state. @@ -164,8 +171,6 @@ private: void _interfaceMapping(); /** maps set-points to internal member set-points */ void _positionController(); /** applies the P-position-controller */ void _velocityController(const float &dt); /** applies the PID-velocity-controller */ - void _updateParams(); /** updates parameters */ - void _setParams(); /** sets parameters to internal member */ matrix::Vector3f _pos{}; /**< MC position */ matrix::Vector3f _vel{}; /**< MC velocity */ @@ -182,42 +187,23 @@ private: Controller::Constraints _constraints{}; /**< variable constraints */ bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */ - /** - * Position Gains. - * Pp: P-controller gain for position-controller - * Pv: P-controller gain for velocity-controller - * Iv: I-controller gain for velocity-controller - * Dv: D-controller gain for velocity-controller - */ - matrix::Vector3f _Pp, _Pv, _Iv, _Dv = matrix::Vector3f{0.0f, 0.0f, 0.0f}; - - float MPC_THR_MAX{1.0f}; /**< maximum thrust */ - float MPC_THR_HOVER{0.5f}; /** equilibrium point for the velocity controller */ - float MPC_THR_MIN{0.0f}; /**< minimum throttle for any position controlled mode */ - float MPC_MANTHR_MIN{0.0f}; /**< minimum throttle for stabilized mode */ - float MPC_XY_VEL_MAX{1.0f}; /**< maximum speed in the horizontal direction */ - float MPC_Z_VEL_MAX_DN{1.0f}; /**< maximum speed in downwards direction */ - float MPC_Z_VEL_MAX_UP{1.0f}; /**< maximum speed in upwards direction */ - float MPC_TILTMAX_AIR{1.5}; /**< maximum tilt for any position/velocity controlled mode in radians */ - float MPC_MAN_TILT_MAX{3.1}; /**< maximum tilt for manual/altitude mode in radians */ - - // Parameter handles - int _parameter_sub { -1 }; - param_t MPC_Z_P_h { PARAM_INVALID }; - param_t MPC_Z_VEL_P_h { PARAM_INVALID }; - param_t MPC_Z_VEL_I_h { PARAM_INVALID }; - param_t MPC_Z_VEL_D_h { PARAM_INVALID }; - param_t MPC_XY_P_h { PARAM_INVALID }; - param_t MPC_XY_VEL_P_h { PARAM_INVALID }; - param_t MPC_XY_VEL_I_h { PARAM_INVALID }; - param_t MPC_XY_VEL_D_h { PARAM_INVALID }; - param_t MPC_XY_VEL_MAX_h { PARAM_INVALID }; - param_t MPC_Z_VEL_MAX_DN_h { PARAM_INVALID }; - param_t MPC_Z_VEL_MAX_UP_h { PARAM_INVALID }; - param_t MPC_THR_HOVER_h { PARAM_INVALID }; - param_t MPC_THR_MAX_h { PARAM_INVALID }; - param_t MPC_THR_MIN_h { PARAM_INVALID }; - param_t MPC_MANTHR_MIN_h { PARAM_INVALID }; - param_t MPC_TILTMAX_AIR_h { PARAM_INVALID }; - param_t MPC_MAN_TILT_MAX_h { PARAM_INVALID }; + DEFINE_PARAMETERS( + (ParamFloat) MPC_THR_MAX, + (ParamFloat) MPC_THR_HOVER, + (ParamFloat) MPC_THR_MIN, + (ParamFloat) MPC_MANTHR_MIN, + (ParamFloat) MPC_XY_VEL_MAX, + (ParamFloat) MPC_Z_VEL_MAX_DN, + (ParamFloat) MPC_Z_VEL_MAX_UP, + (ParamFloat) MPC_TILTMAX_AIR, + (ParamFloat) MPC_MAN_TILT_MAX, + (ParamFloat) MPC_Z_P, + (ParamFloat) MPC_Z_VEL_P, + (ParamFloat) MPC_Z_VEL_I, + (ParamFloat) MPC_Z_VEL_D, + (ParamFloat) MPC_XY_P, + (ParamFloat) MPC_XY_VEL_P, + (ParamFloat) MPC_XY_VEL_I, + (ParamFloat) MPC_XY_VEL_D + ) };