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

mc_pos_control: use MPC_XXX_FF to adjust setpoint on reset

Browse Source
This commit is contained in:
Anton Babushkin
2014-05-01 23:01:30 +02:00
parent f4c36f8c5c
commit 7232c0354b
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+5 -3
View File
@@ -388,9 +388,11 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.z_vel_max, &v);
_params.vel_max(2) = v;
param_get(_params_handles.xy_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(0) = v;
_params.vel_ff(1) = v;
param_get(_params_handles.z_ff, &v);
v = math::constrain(v, 0.0f, 1.0f);
_params.vel_ff(2) = v;
_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@@ -493,8 +495,8 @@ MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
_pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1);
_pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0));
_pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1));
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
@@ -504,7 +506,7 @@ MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2);
_pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2));
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
}
}