mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-07 15:10:34 +08:00
PositionControl parameter renaming. Use the the name as stored in the metadata
This commit is contained in:
Dennis Mannhart
committed by
Lorenz Meier
Lorenz Meier
parent
cf59c2a2ac
commit
c01fab089a
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user