fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 15:28:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

PositionControl parameter renaming. Use the the name as stored in the metadata

Browse Source
This commit is contained in:
Dennis Mannhart
2018-07-20 09:26:18 +02:00
committed by Lorenz Meier
parent cf59c2a2ac
commit c01fab089a
2 changed files with 115 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+80 -74
View File
@@ -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);
}
src/modules/mc_pos_control/PositionControl.hpp
+35 -45
View File
@@ -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 };
};