From 0e65bfffa4160a0cf108530b7a75fe3086e32894 Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Thu, 23 Jul 2020 06:29:57 -0400 Subject: [PATCH] Omni Pos-Ctrl: OMNI_PROJ_AXES works for OMNI_ATT_MODE = 1 --- .../mc_pos_control/PositionControl/ControlMath.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index b9f0fbacef..5d7d2f54be 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -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));