|
|
|
@ -1655,11 +1655,11 @@ MulticopterPositionControl::task_main()
@@ -1655,11 +1655,11 @@ MulticopterPositionControl::task_main()
|
|
|
|
|
// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
|
|
|
|
|
// given by the last attitude setpoint
|
|
|
|
|
_vel_sp(0) = _vel(0) + (-Rb(0, |
|
|
|
|
2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0); |
|
|
|
|
2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0); |
|
|
|
|
_vel_sp(1) = _vel(1) + (-Rb(1, |
|
|
|
|
2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1); |
|
|
|
|
2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1); |
|
|
|
|
_vel_sp(2) = _vel(2) + (-Rb(2, |
|
|
|
|
2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2); |
|
|
|
|
2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2); |
|
|
|
|
_vel_sp_prev(0) = _vel_sp(0); |
|
|
|
|
_vel_sp_prev(1) = _vel_sp(1); |
|
|
|
|
_vel_sp_prev(2) = _vel_sp(2); |
|
|
|
|