diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-28 00:16:10 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-28 00:16:10 +0400 |
| commit | 153114aec8571a9105541b1fef473d36c4099519 (patch) | |
| tree | 5a51e0273d656507c6b6ad036695a9407801d42e /src/modules/mc_pos_control/mc_pos_control_main.cpp | |
| parent | 7a34089dee9a3f96cdcdf0a3e94619cf22c2053c (diff) | |
| download | px4-firmware-153114aec8571a9105541b1fef473d36c4099519.tar.gz px4-firmware-153114aec8571a9105541b1fef473d36c4099519.tar.bz2 px4-firmware-153114aec8571a9105541b1fef473d36c4099519.zip | |
mc_pos_control: calculate velocity error derivative without setpoint acceleration to get more clean output
Diffstat (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 14 |
1 files changed, 9 insertions, 5 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 f7d50f45d..948459c43 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -177,7 +177,7 @@ private: math::Vector<3> _vel; math::Vector<3> _pos_sp; math::Vector<3> _vel_sp; - math::Vector<3> _vel_err_prev; /**< velocity on previous step */ + math::Vector<3> _vel_prev; /**< velocity on previous step */ /** * Update our local parameter cache. @@ -261,7 +261,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel.zero(); _pos_sp.zero(); _vel_sp.zero(); - _vel_err_prev.zero(); + _vel_prev.zero(); _params_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _params_handles.takeoff_gap = param_find("NAV_TAKEOFF_GAP"); @@ -693,11 +693,15 @@ MulticopterPositionControl::task_main() reset_int_xy = true; } - /* calculate desired thrust vector in NED frame */ + /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + (vel_err - _vel_err_prev).emult(_params.vel_d) / dt + thrust_int; - _vel_err_prev = vel_err; + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; |