diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-27 10:50:40 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-27 10:50:40 +0400 |
commit | 6a04d13e733700e7e2e90c20399889a79eae293a (patch) | |
tree | 6ce53fa50b9110df69216a3cd4741648c0ec5e92 | |
parent | a0355ccf760208a02e1ba54fbc3b153752e9d191 (diff) | |
download | px4-firmware-6a04d13e733700e7e2e90c20399889a79eae293a.tar.gz px4-firmware-6a04d13e733700e7e2e90c20399889a79eae293a.tar.bz2 px4-firmware-6a04d13e733700e7e2e90c20399889a79eae293a.zip |
mc_pos_control: minor reorganizing
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 22 |
1 files changed, 10 insertions, 12 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c06caff1e..f7d50f45d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -179,10 +179,6 @@ private: math::Vector<3> _vel_sp; math::Vector<3> _vel_err_prev; /**< velocity on previous step */ - hrt_abstime _time_prev; - - bool _was_armed; - /** * Update our local parameter cache. */ @@ -241,10 +237,7 @@ MulticopterPositionControl::MulticopterPositionControl() : /* publications */ _local_pos_sp_pub(-1), _att_sp_pub(-1), - _global_vel_sp_pub(-1), - - _time_prev(0), - _was_armed(false) + _global_vel_sp_pub(-1) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -469,6 +462,7 @@ MulticopterPositionControl::task_main() bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; + const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; @@ -510,10 +504,10 @@ MulticopterPositionControl::task_main() parameters_update(false); hrt_abstime t = hrt_absolute_time(); - float dt = _time_prev != 0 ? (t - _time_prev) * 0.000001f : 0.0f; - _time_prev = t; + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; - if (_control_mode.flag_armed && !_was_armed) { + if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ reset_man_sp_z = true; reset_man_sp_xy = true; @@ -523,7 +517,7 @@ MulticopterPositionControl::task_main() reset_int_z = true; reset_int_xy = true; } - _was_armed = _control_mode.flag_armed; + was_armed = _control_mode.flag_armed; if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || @@ -782,6 +776,10 @@ MulticopterPositionControl::task_main() 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 */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ |