diff --git a/src/modules/mc_pos_control/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl.cpp index e25ad281b3..b5df85b7a8 100644 --- a/src/modules/mc_pos_control/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl.cpp @@ -45,21 +45,22 @@ using namespace matrix; PositionControl::PositionControl() { - _Pz_h = param_find("MPC_Z_P"); - _Pvz_h = param_find("MPC_Z_VEL_P"); - _Ivz_h = param_find("MPC_Z_VEL_I"); - _Dvz_h = param_find("MPC_Z_VEL_D"); - _Pxy_h = param_find("MPC_XY_P"); - _Pvxy_h = param_find("MPC_XY_VEL_P"); - _Ivxy_h = param_find("MPC_XY_VEL_I"); - _Dvxy_h = param_find("MPC_XY_VEL_D"); - _VelMaxXY_h = param_find("MPC_XY_VEL_MAX"); - _VelMaxZdown_h = param_find("MPC_Z_VEL_MAX_DN"); - _VelMaxZup_h = param_find("MPC_Z_VEL_MAX_UP"); - _ThrHover_h = param_find("MPC_THR_HOVER"); - _ThrMax_h = param_find("MPC_THR_MAX"); - _ThrMinPosition_h = param_find("MPC_THR_MIN"); - _ThrMinStab_h = param_find("MPC_MANTHR_MIN"); + MPC_Z_P_h = param_find("MPC_Z_P"); + MPC_Z_VEL_P_h = param_find("MPC_Z_VEL_P"); + MPC_Z_VEL_I_h = param_find("MPC_Z_VEL_I"); + MPC_Z_VEL_D_h = param_find("MPC_Z_VEL_D"); + MPC_XY_P_h = param_find("MPC_XY_P"); + MPC_XY_VEL_P_h = param_find("MPC_XY_VEL_P"); + MPC_XY_VEL_I_h = param_find("MPC_XY_VEL_I"); + MPC_XY_VEL_D_h = param_find("MPC_XY_VEL_D"); + MPC_XY_VEL_MAX_h = param_find("MPC_XY_VEL_MAX"); + MPC_Z_VEL_MAX_DN_h = param_find("MPC_Z_VEL_MAX_DN"); + MPC_Z_VEL_MAX_UP_h = param_find("MPC_Z_VEL_MAX_UP"); + MPC_THR_HOVER_h = param_find("MPC_THR_HOVER"); + MPC_THR_MAX_h = param_find("MPC_THR_MAX"); + MPC_THR_MIN_h = param_find("MPC_THR_MIN"); + MPC_THR_MIN_h = param_find("MPC_MANTHR_MIN"); + MPC_TILTMAX_AIR_h = param_find("MPC_TILTMAX_AIR"); _parameter_sub = orb_subscribe(ORB_ID(parameter_update)); // set parameter the very first time @@ -86,24 +87,33 @@ void PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &se // If full manual is required (thrust already generated), don't run position/velocity // controller and just return thrust. - _skipController = false; - _ThrustLimit.min = _ThrMinPosition; + _skip_controller = false; if (PX4_ISFINITE(setpoint.thrust[0]) && PX4_ISFINITE(setpoint.thrust[1]) && PX4_ISFINITE(setpoint.thrust[2])) { - _skipController = true; - _ThrustLimit.min = _ThrMinStab; - _pos_sp.zero(); - _vel_sp.zero(); - _acc_sp.zero(); + _skip_controller = true; } } void PositionControl::generateThrustYawSetpoint(const float &dt) { + if (_skip_controller) { + // Already received a valid thrust set-point. + // Limit the thrust vector. + float thr_mag = _thr_sp.length(); - // Only run position/velocity controller - // if thrust needs to be generated. - if (!_skipController) { + if (thr_mag > MPC_THR_MAX) { + _thr_sp = _thr_sp.normalized() * MPC_THR_MAX; + + } else if (thr_mag < MPC_THR_MIN && thr_mag > FLT_EPSILON) { + _thr_sp = _thr_sp.normalized() * MPC_THR_MIN; + } + + // Just set the set-points equal to the current vehicle state. + _pos_sp = _pos; + _vel_sp = _vel; + _acc_sp = _acc; + + } else { _positionController(); _velocityController(dt); } @@ -158,7 +168,7 @@ void PositionControl::_interfaceMapping() } if (!PX4_ISFINITE(_yaw_sp)) { - // Set the yaw-sp eaual the current yaw. + // Set the yaw-sp equal the current yaw. // That is the best we can do and it also // agrees with FlightTask-interface definition. _yaw_sp = _yaw; @@ -168,17 +178,17 @@ void PositionControl::_interfaceMapping() void PositionControl::_positionController() { // P-position controller - Vector3f vel_sp_position = (_pos_sp - _pos).emult(Pp); + Vector3f vel_sp_position = (_pos_sp - _pos).emult(_Pv); _vel_sp = vel_sp_position + _vel_sp; // Constrain horizontal velocity by prioritizing the velocity component along the // the desired position setpoint over the feed-forward term. - Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)), Vector2f(&(_vel_sp - vel_sp_position)(0)), - _VelMaxXY); + Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(&vel_sp_position(0)), + Vector2f(&(_vel_sp - vel_sp_position)(0)), MPC_XY_VEL_MAX); _vel_sp(0) = vel_sp_xy(0); _vel_sp(1) = vel_sp_xy(1); - // Constrain velocity in z-directio. - _vel_sp(2) = math::constrain(_vel_sp(2), -_VelMaxZ.up, _VelMaxZ.down); + // Constrain velocity in z-direction. + _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.vel_max_z_up, MPC_Z_VEL_MAX_DN); } void PositionControl::_velocityController(const float &dt) @@ -211,18 +221,18 @@ void PositionControl::_velocityController(const float &dt) Vector3f vel_err = _vel_sp - _vel; // Consider thrust in D-direction. - float thrust_desired_D = Pv(2) * vel_err(2) + Dv(2) * _vel_dot(2) + _thr_int(2) - _ThrHover; + float thrust_desired_D = _Pv(2) * vel_err(2) + _Dv(2) * _vel_dot(2) + _thr_int(2) - MPC_THR_HOVER; // The Thrust limits are negated and swapped due to NED-frame. - float uMax = -_ThrustLimit.min; - float uMin = -_ThrustLimit.max; + float uMax = -MPC_THR_MIN; + float uMin = -MPC_THR_MAX; // Apply Anti-Windup in D-direction. bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) || (thrust_desired_D <= uMin && vel_err(2) <= 0.0f); if (!stop_integral_D) { - _thr_int(2) += vel_err(2) * Iv(2) * dt; + _thr_int(2) += vel_err(2) * _Iv(2) * dt; } @@ -232,19 +242,19 @@ void PositionControl::_velocityController(const float &dt) if (fabsf(_thr_sp(0)) + fabsf(_thr_sp(1)) > FLT_EPSILON) { // Thrust set-point in NE-direction is already provided. Only // scaling by the maximum tilt is required. - float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_tilt_max); + float thr_xy_max = fabsf(_thr_sp(2)) * tanf(MPC_MAN_TILT_MAX); _thr_sp(0) *= thr_xy_max; _thr_sp(1) *= thr_xy_max; } else { // PID-velocity controller for NE-direction. Vector2f thrust_desired_NE; - thrust_desired_NE(0) = Pv(0) * vel_err(0) + Dv(0) * _vel_dot(0) + _thr_int(0); - thrust_desired_NE(1) = Pv(1) * vel_err(1) + Dv(1) * _vel_dot(1) + _thr_int(1); + thrust_desired_NE(0) = _Pv(0) * vel_err(0) + _Dv(0) * _vel_dot(0) + _thr_int(0); + thrust_desired_NE(1) = _Pv(1) * vel_err(1) + _Dv(1) * _vel_dot(1) + _thr_int(1); // Get maximum allowed thrust in NE based on tilt and excess thrust. - float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_tilt_max); - float thrust_max_NE = sqrtf(_ThrustLimit.max * _ThrustLimit.max - _thr_sp(2) * _thr_sp(2)); + float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt_max); + float thrust_max_NE = sqrtf(MPC_THR_MAX * MPC_THR_MAX - _thr_sp(2) * _thr_sp(2)); thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE); // Get the direction of (r-y) in NE-direction. @@ -255,8 +265,8 @@ void PositionControl::_velocityController(const float &dt) direction_NE >= 0.0f); if (!stop_integral_NE) { - _thr_int(0) += vel_err(0) * Iv(0) * dt; - _thr_int(1) += vel_err(1) * Iv(1) * dt; + _thr_int(0) += vel_err(0) * _Iv(0) * dt; + _thr_int(1) += vel_err(1) * _Iv(1) * dt; } // Saturate thrust in NE-direction. @@ -277,16 +287,16 @@ void PositionControl::updateConstraints(const Controller::Constraints &constrain // update all parameters since they might have changed _updateParams(); - // maximum tilt cannot exceed 90 degrees - _tilt_max = M_PI_2_F; + _constraints = constraints; - if (PX4_ISFINITE(constraints.tilt_max)) { - _tilt_max = math::min(constraints.tilt_max, _tilt_max); + // Check if adustable contraints are below global constraints. If they are not stricter than global + // constraints, then just use global constraints for the limits. + if (!PX4_ISFINITE(constraints.tilt_max) || !(constraints.tilt_max < MPC_TILTMAX_AIR)) { + _constraints.tilt_max = MPC_TILTMAX_AIR; } - // maximum velocity upwards cannot exceed global limit - if (PX4_ISFINITE(constraints.vel_max_z_up)) { - _VelMaxZ.up = math::min(constraints.vel_max_z_up, _VelMaxZ.up); + if (!PX4_ISFINITE(constraints.vel_max_z_up) || !(constraints.vel_max_z_up < MPC_Z_VEL_MAX_UP)) { + _constraints.vel_max_z_up = MPC_Z_VEL_MAX_UP; } } @@ -304,29 +314,25 @@ void PositionControl::_updateParams() void PositionControl::_setParams() { - param_get(_Pxy_h, &Pp(0)); - param_get(_Pxy_h, &Pp(1)); - param_get(_Pz_h, &Pp(2)); - - param_get(_Pvxy_h, &Pv(0)); - param_get(_Pvxy_h, &Pv(1)); - param_get(_Pvz_h, &Pv(2)); - - param_get(_Ivxy_h, &Iv(0)); - param_get(_Ivxy_h, &Iv(1)); - param_get(_Ivz_h, &Iv(2)); - - param_get(_Dvxy_h, &Dv(0)); - param_get(_Dvxy_h, &Dv(1)); - param_get(_Dvz_h, &Dv(2)); - - param_get(_VelMaxXY_h, &_VelMaxXY); - param_get(_VelMaxZup_h, &_VelMaxZ.up); - param_get(_VelMaxZdown_h, &_VelMaxZ.down); - - param_get(_ThrHover_h, &_ThrHover); - param_get(_ThrMax_h, &_ThrustLimit.max); - param_get(_ThrMinPosition_h, &_ThrMinPosition); - param_get(_ThrMinStab_h, &_ThrMinStab); - + param_get(MPC_XY_P_h, &_Pp(0)); + param_get(MPC_XY_P_h, &_Pp(1)); + param_get(MPC_Z_P_h, &_Pp(2)); + param_get(MPC_XY_VEL_P_h, &_Pv(0)); + param_get(MPC_XY_VEL_P_h, &_Pv(1)); + param_get(MPC_Z_VEL_P_h, &_Pv(2)); + param_get(MPC_XY_VEL_I_h, &_Iv(0)); + param_get(MPC_XY_VEL_I_h, &_Iv(1)); + param_get(MPC_Z_VEL_I_h, &_Iv(2)); + param_get(MPC_XY_VEL_D_h, &_Dv(0)); + param_get(MPC_XY_VEL_D_h, &_Dv(1)); + param_get(MPC_Z_VEL_D_h, &_Dv(2)); + param_get(MPC_XY_VEL_MAX_h, &MPC_XY_VEL_MAX); + param_get(MPC_Z_VEL_MAX_UP_h, &MPC_Z_VEL_MAX_UP); + param_get(MPC_Z_VEL_MAX_DN_h, &MPC_Z_VEL_MAX_DN); + param_get(MPC_THR_HOVER_h, &MPC_THR_HOVER); + param_get(MPC_THR_MAX_h, &MPC_THR_MAX); + param_get(MPC_THR_MIN_h, &MPC_THR_MIN); + param_get(MPC_MANTHR_MIN_h, &MPC_MANTHR_MIN); + param_get(MPC_TILTMAX_AIR_h, &MPC_TILTMAX_AIR); + param_get(MPC_MAN_TILT_MAX_h, &MPC_MAN_TILT_MAX); } diff --git a/src/modules/mc_pos_control/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl.hpp index bbf227c1b8..ee80c88ced 100644 --- a/src/modules/mc_pos_control/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl.hpp @@ -51,8 +51,8 @@ namespace Controller * than the global limits. * tilt_max: Cannot exceed PI/2 * vel_max_z_up: Cannot exceed maximum global velocity upwards - * @see _tilt_max - * @see _VelMaxZ + * @see MPC_TILTMAX_AIR + * @see MPC_Z_VEL_MAX_DN */ struct Constraints { float tilt_max; /**< maximum tilt always below Pi/2 */ @@ -161,22 +161,26 @@ public: matrix::Vector3f getPosSp() {return _pos_sp;} 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 */ matrix::Vector3f _vel_dot{}; /**< MC velocity derivative */ matrix::Vector3f _acc{}; /**< MC acceleration */ float _yaw{0.0f}; /**< MC yaw */ - matrix::Vector3f _pos_sp{}; /**< desired position */ matrix::Vector3f _vel_sp{}; /**< desired velocity */ matrix::Vector3f _acc_sp{}; /**< desired acceleration: not supported yet */ matrix::Vector3f _thr_sp{}; /**< desired thrust */ float _yaw_sp{}; /**< desired yaw */ float _yawspeed_sp{}; /** desired yaw-speed */ - matrix::Vector3f _thr_int{}; /**< thrust integral term */ Controller::Constraints _constraints{}; /**< variable constraints */ + bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */ /** * Position Gains. @@ -185,49 +189,35 @@ private: * 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}; + matrix::Vector3f _Pp, _Pv, _Iv, _Dv = matrix::Vector3f{0.0f, 0.0f, 0.0f}; - float _VelMaxXY{}; /**< maximum global limit for velocity in the horizontal direction */ - - struct DirectionD { - float up; - float down; - }; - DirectionD _VelMaxZ; /**< struct for velocity limits in the z-direction */ - - struct Limits { - float max; - float min; - }; - Limits _ThrustLimit; /**< struct for thrust-limits */ - - float _ThrHover{0.5f}; /** equilibrium point for the velocity controller */ - float _ThrMinPosition{0.0f}; /**< minimum throttle for any position controlled mode */ - float _ThrMinStab{0.0f}; /**< minimum throttle for stabilized mode */ - float _tilt_max{1.5f}; /**< maximum tilt for any velocity controlled mode. */ - bool _skipController{false}; /**< skips position/velocity controller. true for stabilized mode */ - - void _interfaceMapping(); /** maps setpoints to internal member setpoints */ - 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 */ + 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 */ + float MPC_MAN_TILT_MAX{3.1}; /**< maximum tilt for manual/altitude mode */ // Parameter handles int _parameter_sub { -1 }; - param_t _Pz_h { PARAM_INVALID }; - param_t _Pvz_h { PARAM_INVALID }; - param_t _Ivz_h { PARAM_INVALID }; - param_t _Dvz_h { PARAM_INVALID }; - param_t _Pxy_h { PARAM_INVALID }; - param_t _Pvxy_h { PARAM_INVALID }; - param_t _Ivxy_h { PARAM_INVALID }; - param_t _Dvxy_h { PARAM_INVALID }; - param_t _VelMaxXY_h { PARAM_INVALID }; - param_t _VelMaxZdown_h { PARAM_INVALID }; - param_t _VelMaxZup_h { PARAM_INVALID }; - param_t _ThrHover_h { PARAM_INVALID }; - param_t _ThrMax_h { PARAM_INVALID }; - param_t _ThrMinPosition_h { PARAM_INVALID }; - param_t _ThrMinStab_h { PARAM_INVALID }; + 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 }; };