yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: code style fix

Browse Source
This commit is contained in:
Dennis Mannhart
2017-02-02 19:54:12 +01:00
committed by Lorenz Meier
parent f0978fc9e9
commit 57b7c4fc3d
1 changed files with 9 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@@ -1302,30 +1302,21 @@ MulticopterPositionControl::control_offboard(float dt)
void
MulticopterPositionControl::vel_sp_slewrate(float dt)
{
/* limit total horizontal acceleration */
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
math::Vector<3> acc = (_vel_sp - _vel_sp_prev) / dt;
float acc_xy_mag = sqrtf(acc(0) * acc(0) + acc(1) * acc(1));
if (acc_hor.length() > _params.acc_hor_max) {
acc_hor = acc_hor.normalized() * _params.acc_hor_max;
_vel_sp(0) = acc_hor(0) * dt + _vel_sp_prev(0);
_vel_sp(1) = acc_hor(1) * dt + _vel_sp_prev(1);
/* limit total horizontal acceleration */
if (acc_xy_mag > _params.acc_hor_max) {
_vel_sp(0) = _params.acc_hor_max * acc(0) / acc_xy_mag * dt + _vel_sp_prev(0);
_vel_sp(1) = _params.acc_hor_max * acc(1) / acc_xy_mag * dt + _vel_sp_prev(1);
}
/* limit vertical acceleration */
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
float max_acc_z = acc(2) < 0.0f ? -_params.acc_up_max : _params.acc_down_max;
/* accelerate up */
if (acc_v < 0.0f && fabsf(acc_v) > _params.acc_up_max) {
_vel_sp(2) = -_params.acc_up_max * dt + _vel_sp_prev(2);
if (fabsf(acc(2)) > fabsf(max_acc_z)) {
_vel_sp(2) = max_acc_z * dt + _vel_sp_prev(2);
}
/*accelerate down */
if (acc_v >= 0.0f && fabsf(acc_v) > _params.acc_down_max) {
_vel_sp(2) = _params.acc_down_max * dt + _vel_sp_prev(2);
}
}
bool