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

Merge branch 'master'

Browse Source
This commit is contained in:
TSC21
2015-05-23 14:36:09 +01:00
parent 6049365123 b9dc72f4cc
commit 2e40338b93
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+4 -1
View File
@@ -161,6 +161,7 @@ private:
perf_counter_t _controller_latency_perf;
math::Vector<3> _rates_prev; /**< angular rates on previous step */
math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */
math::Vector<3> _rates_sp; /**< angular rates setpoint */
math::Vector<3> _rates_int; /**< angular rates integral error */
float _thrust_sp; /**< thrust setpoint */
@@ -356,6 +357,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_rates_prev.zero();
_rates_sp.zero();
_rates_sp_prev.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
@@ -715,7 +717,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp);
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if not saturated on low limit and if motor commands are not saturated */