fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 06:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: minor reorganizing

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-27 10:50:40 +04:00
parent a0355ccf76
commit 6a04d13e73
1 changed files with 10 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+10 -12
View File
@@ -179,10 +179,6 @@ private:
math::Vector<3> _vel_sp; math::Vector<3> _vel_sp;
math::Vector<3> _vel_err_prev; /**< velocity on previous step */ math::Vector<3> _vel_err_prev; /**< velocity on previous step */
hrt_abstime _time_prev;
bool _was_armed;
/** /**
* Update our local parameter cache. * Update our local parameter cache.
*/ */
@@ -241,10 +237,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
/* publications */ /* publications */
_local_pos_sp_pub(-1), _local_pos_sp_pub(-1),
_att_sp_pub(-1), _att_sp_pub(-1),
_global_vel_sp_pub(-1), _global_vel_sp_pub(-1)
_time_prev(0),
_was_armed(false)
{ {
memset(&_att, 0, sizeof(_att)); memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_att_sp, 0, sizeof(_att_sp));
@@ -469,6 +462,7 @@ MulticopterPositionControl::task_main()
bool reset_takeoff_sp = true; bool reset_takeoff_sp = true;
hrt_abstime t_prev = 0; hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f; const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f; const float pos_ctl_dz = 0.05f;
@@ -510,10 +504,10 @@ MulticopterPositionControl::task_main()
parameters_update(false); parameters_update(false);
hrt_abstime t = hrt_absolute_time(); hrt_abstime t = hrt_absolute_time();
float dt = _time_prev != 0 ? (t - _time_prev) * 0.000001f : 0.0f; float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
_time_prev = t; t_prev = t;
if (_control_mode.flag_armed && !_was_armed) { if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */ /* reset setpoints and integrals on arming */
reset_man_sp_z = true; reset_man_sp_z = true;
reset_man_sp_xy = true; reset_man_sp_xy = true;
@@ -523,7 +517,7 @@ MulticopterPositionControl::task_main()
reset_int_z = true; reset_int_z = true;
reset_int_xy = true; reset_int_xy = true;
} }
_was_armed = _control_mode.flag_armed; was_armed = _control_mode.flag_armed;
if (_control_mode.flag_control_altitude_enabled || if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled || _control_mode.flag_control_position_enabled ||
@@ -782,6 +776,10 @@ MulticopterPositionControl::task_main()
thrust_int += vel_err.emult(_params.vel_i) * dt; thrust_int += vel_err.emult(_params.vel_i) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f)
thrust_int(2) = 0.0f;
/* calculate attitude and thrust from thrust vector */ /* calculate attitude and thrust from thrust vector */
if (_control_mode.flag_control_velocity_enabled) { if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */ /* desired body_z axis = -normalize(thrust_vector) */