fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 06:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC: Split yaw speed limiting between manual and velocity control modes

Browse Source
This commit is contained in:
Lorenz Meier
2016-01-30 15:03:20 +01:00
parent 56cd54ab71
commit 7455a833d0
2 changed files with 32 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+18 -2
View File
@@ -190,6 +190,7 @@ private:
param_t roll_rate_max;
param_t pitch_rate_max;
param_t yaw_rate_max;
param_t yaw_auto_max;
param_t acro_roll_max;
param_t acro_pitch_max;
@@ -214,8 +215,9 @@ private:
float roll_rate_max;
float pitch_rate_max;
float yaw_rate_max;
float yaw_auto_max;
math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */
math::Vector<3> auto_rate_max; /**< attitude rate limits in auto modes */
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
float rattitude_thres;
int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
@@ -347,6 +349,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.pitch_rate_max = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.mc_rate_max.zero();
_params.auto_rate_max.zero();
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_params.vtol_opt_recovery_enabled = false;
@@ -379,6 +382,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
@@ -482,6 +486,14 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* auto angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.auto_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.auto_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max);
_params.auto_rate_max(2) = math::radians(_params.yaw_auto_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
@@ -705,7 +717,11 @@ MulticopterAttitudeControl::control_attitude(float dt)
/* limit rates */
for (int i = 0; i < 3; i++) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
if (_v_control_mode.flag_control_velocity_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
} else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
}
/* feed forward yaw setpoint rate */
src/modules/mc_att_control/mc_att_control_params.c
+14 -2
View File
@@ -251,6 +251,18 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f);
/**
* Max yaw rate
*
* A value of significantly over 120 degrees per second can already lead to mixer saturation.
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
/**
* Max yaw rate in auto mode
*
* Limit for yaw rate, has effect for large rotations in autonomous mode,
* to avoid large control output and mixer saturation. A value of significantly
* over 60 degrees per second can already lead to mixer saturation.
@@ -258,10 +270,10 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f);
*
* @unit deg/s
* @min 0.0
* @max 360.0
* @max 120.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 45.0f);
PARAM_DEFINE_FLOAT(MC_YAWRAUTO_MAX, 45.0f);
/**
* Max acro roll rate