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

mc_att_control_vector: attitude rate D component implemented

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-15 22:52:05 +04:00
parent 69c4f6f5e4
commit 72aa171ef9
1 changed files with 29 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+29 -7
View File
@@ -136,13 +136,18 @@ private:
bool _att_sp_valid; /**< flag if the attitude setpoint is valid */ bool _att_sp_valid; /**< flag if the attitude setpoint is valid */
math::Matrix _K; /**< diagonal gain matrix for position error */ math::Matrix _K; /**< diagonal gain matrix for position error */
math::Matrix _K_rate; /**< diagonal gain matrix for angular rate error */ math::Matrix _K_rate_p; /**< diagonal gain matrix for angular rate error */
math::Matrix _K_rate_d; /**< diagonal gain matrix for angular rate derivative */
math::Vector3 _rates_prev; /**< angular rates on previous step */
struct { struct {
param_t att_p; param_t att_p;
param_t att_rate_p; param_t att_rate_p;
param_t att_rate_d;
param_t yaw_p; param_t yaw_p;
param_t yaw_rate_p; param_t yaw_rate_p;
param_t yaw_rate_d;
} _parameter_handles; /**< handles for interesting parameters */ } _parameter_handles; /**< handles for interesting parameters */
/** /**
@@ -224,7 +229,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
/* gain matrices */ /* gain matrices */
_K(3, 3), _K(3, 3),
_K_rate(3, 3) _K_rate_p(3, 3),
_K_rate_d(3, 3),
_rates_prev(0.0f, 0.0f, 0.0f)
{ {
memset(&_att, 0, sizeof(_att)); memset(&_att, 0, sizeof(_att));
@@ -235,8 +243,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_parameter_handles.att_p = param_find("MC_ATT_P"); _parameter_handles.att_p = param_find("MC_ATT_P");
_parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P");
_parameter_handles.att_rate_d = param_find("MC_ATTRATE_D");
_parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_p = param_find("MC_YAWPOS_P");
_parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_parameter_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); parameters_update();
@@ -271,22 +281,32 @@ MulticopterAttitudeControl::parameters_update()
{ {
float att_p; float att_p;
float att_rate_p; float att_rate_p;
float att_rate_d;
float yaw_p; float yaw_p;
float yaw_rate_p; float yaw_rate_p;
float yaw_rate_d;
param_get(_parameter_handles.att_p, &att_p); param_get(_parameter_handles.att_p, &att_p);
param_get(_parameter_handles.att_rate_p, &att_rate_p); param_get(_parameter_handles.att_rate_p, &att_rate_p);
param_get(_parameter_handles.att_rate_d, &att_rate_d);
param_get(_parameter_handles.yaw_p, &yaw_p); param_get(_parameter_handles.yaw_p, &yaw_p);
param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p);
param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d);
_K.setAll(0.0f); _K.setAll(0.0f);
_K(0, 0) = att_p; _K(0, 0) = att_p;
_K(1, 1) = att_p; _K(1, 1) = att_p;
_K(2, 2) = yaw_p; _K(2, 2) = yaw_p;
_K_rate.setAll(0.0f);
_K_rate(0, 0) = att_rate_p; _K_rate_p.setAll(0.0f);
_K_rate(1, 1) = att_rate_p; _K_rate_p(0, 0) = att_rate_p;
_K_rate(2, 2) = yaw_rate_p; _K_rate_p(1, 1) = att_rate_p;
_K_rate_p(2, 2) = yaw_rate_p;
_K_rate_d.setAll(0.0f);
_K_rate_d(0, 0) = att_rate_d;
_K_rate_d(1, 1) = att_rate_d;
_K_rate_d(2, 2) = yaw_rate_d;
return OK; return OK;
} }
@@ -596,9 +616,11 @@ MulticopterAttitudeControl::task_main()
/* angular rates setpoint*/ /* angular rates setpoint*/
math::Vector3 rates_sp = _K * e_R; math::Vector3 rates_sp = _K * e_R;
/* feed forward yaw setpoint rate */ /* feed forward yaw setpoint rate */
rates_sp(2) += yaw_sp_move_rate; rates_sp(2) += yaw_sp_move_rate;
math::Vector3 control = _K_rate * (rates_sp - rates); math::Vector3 control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f);
_rates_prev = rates;
/* publish the attitude rates setpoint */ /* publish the attitude rates setpoint */
_rates_sp.roll = rates_sp(0); _rates_sp.roll = rates_sp(0);