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

Omni Pos-Ctrl: OMNI_PROJ_AXES works for OMNI_ATT_MODE = 1

Browse Source
This commit is contained in:
Azarakhsh Keipour 2020-07-23 06:29:57 -04:00
parent d77f74bb53
commit 0e65bfffa4
1 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
View File

@ -259,6 +259,17 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f
att_sp.pitch_body = euler(1);
att_sp.yaw_body = euler(2);
if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
matrix::Dcmf R_body = att;
for (int i = 0; i < 3; i++) {
body_x(i) = R_body(i, 0);
body_y(i) = R_body(i, 1);
body_z(i) = R_body(i, 2);
}
}
// Calculate the direct force vector
float f_eff_z = -(omni_dfc_max_thrust * tanf(lambda) + thr_sp(2) / cosf(lambda));
Vector2f f_eff_h(thr_sp.dot(body_x), thr_sp.dot(body_y));