aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/lib/mathlib/math/Vector3.hpp1
-rw-r--r--src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp56
-rw-r--r--src/modules/multirotor_pos_control/multirotor_pos_control.c52
3 files changed, 57 insertions, 52 deletions
diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
index 2bf00f26b..1656e184e 100644
--- a/src/lib/mathlib/math/Vector3.hpp
+++ b/src/lib/mathlib/math/Vector3.hpp
@@ -56,6 +56,7 @@ public:
Vector3 cross(const Vector3 &b) const;
Vector3 operator %(const Vector3 &v) const;
float operator *(const Vector3 &v) const;
+ using Vector::operator *;
/**
* accessors
diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
index 230fa15e4..fb09078fa 100644
--- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
@@ -564,33 +564,39 @@ MulticopterAttitudeControl::task_main()
math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed);
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
- /* sin(angle error) between current and desired thrust vectors (Z axes) */
math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2));
- math::Vector3 R_z_des(R_des(0, 2), R_des(1, 2), R_des(2, 2));
- math::Vector3 e_R = R.transpose() * R_z.cross(R_z_des);
+ math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2));
+
+ /* axis and sin(angle) of desired rotation */
+ math::Vector3 e_R = R.transpose() * R_z.cross(R_des_z);
/* calculate angle error */
- float e_R_sin = e_R.norm();
- float e_R_cos = R_z * R_z_des;
- float angle = atan2f(e_R_sin, e_R_cos);
+ float e_R_z_sin = e_R.norm();
+ float e_R_z_cos = R_z * R_des_z;
+ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+
+ /* calculate weight for yaw control */
+ float cos_z = cosf(R_z(2));
+ float yaw_w = cos_z * cos_z;
- /* e_R has zero Z component, set it according to yaw */
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix R_rp(3, 3);
- if (e_R_sin > 0.0f) {
+ if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
- math::Vector3 e_R_axis = e_R / e_R_sin;
+ math::Vector3 e_R_z_axis = e_R / e_R_z_sin;
+
+ e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix e_R_cp(3, 3);
e_R_cp.setAll(0.0f);
- e_R_cp(0, 1) = -e_R_axis(2);
- e_R_cp(0, 2) = e_R_axis(1);
- e_R_cp(1, 0) = e_R_axis(2);
- e_R_cp(1, 2) = -e_R_axis(0);
- e_R_cp(2, 0) = -e_R_axis(1);
- e_R_cp(2, 1) = e_R_axis(0);
+ e_R_cp(0, 1) = -e_R_z_axis(2);
+ e_R_cp(0, 2) = e_R_z_axis(1);
+ e_R_cp(1, 0) = e_R_z_axis(2);
+ e_R_cp(1, 2) = -e_R_z_axis(0);
+ e_R_cp(2, 0) = -e_R_z_axis(1);
+ e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
math::Matrix I(3, 3);
@@ -598,7 +604,7 @@ MulticopterAttitudeControl::task_main()
I(0, 0) = 1.0f;
I(1, 1) = 1.0f;
I(2, 2) = 1.0f;
- R_rp = R * (I + e_R_cp * e_R_sin + e_R_cp * e_R_cp * (1.0f - e_R_cos));
+ R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
@@ -606,13 +612,21 @@ MulticopterAttitudeControl::task_main()
}
/* R_rp and R_des has the same Z axis, calculate yaw error */
- math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2));
math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0));
math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
- e_R(2) = R_rp_x.cross(R_des_x) * R_des_z;
-
- /* don't try to control yaw when thrust vector has opposite direction to desired */
- e_R(2) *= (e_R_cos > 0.0f ? 1.0f : e_R_sin);
+ e_R(2) = atan2f(R_rp_x.cross(R_des_x) * R_des_z, R_rp_x * R_des_x) * yaw_w;
+
+ if (e_R_z_cos < 0.0) {
+ /* for large thrust vector rotations use another rotation method:
+ * calculate angle and axis for R -> R_des rotation directly */
+ math::Quaternion q(R.transpose() * R_des);
+ float angle_d = 2.0f * atan2f(sqrtf(q.getB() * q.getB() + q.getC() * q.getC() + q.getD() * q.getD()), q.getA());
+ math::Vector3 e_R_d(q.getB(), q.getC(), q.getD());
+
+ /* use fusion of Z axis based rotation and direct rotation */
+ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+ e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+ }
/* angular rates setpoint*/
math::Vector3 rates_sp = _K * e_R;
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 7e36872f9..54f9e52b8 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -71,6 +71,7 @@
#include "thrust_pid.h"
#define TILT_COS_MAX 0.7f
+#define SIGMA 0.000001f
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
@@ -439,7 +440,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
- float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
+ float xy_sp_ctl_dir = att_sp.yaw_body + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
@@ -771,7 +772,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
body_z[0] = 0.0f;
body_z[1] = 0.0f;
- body_z[2] = -1.0f;
+ body_z[2] = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
@@ -780,39 +781,28 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
y_C[1] = cosf(att_sp.yaw_body);
y_C[2] = 0;
- /* desired body_x axis = cross(y_C, body_z) */
- cross3(y_C, body_z, body_x);
- float body_x_norm = normalize3(body_x);
- static const float body_x_norm_max = 0.5f;
+ if (fabsf(body_z[2]) < SIGMA) {
+ /* desired thrust is in XY plane, set X downside to construct correct matrix,
+ * but yaw component will not be used actually */
+ body_x[0] = -body_x[0];
+ body_x[1] = -body_x[1];
+ body_x[2] = -body_x[2];
+ } else {
+ /* desired body_x axis = cross(y_C, body_z) */
+ cross3(y_C, body_z, body_x);
+
+ /* keep nose to front while inverted upside down */
+ if (body_z[2] < 0.0f) {
+ body_x[0] = -body_x[0];
+ body_x[1] = -body_x[1];
+ body_x[2] = -body_x[2];
+ }
+ normalize3(body_x);
+ }
/* desired body_y axis = cross(body_z, body_x) */
cross3(body_z, body_x, body_y);
- if (body_x_norm < body_x_norm_max) {
- /* roll is close to +/- PI/2, don't try to hold yaw exactly */
- float x_C[3];
- x_C[0] = cos(att_sp.yaw_body);
- x_C[1] = sinf(att_sp.yaw_body);
- x_C[2] = 0;
-
- float body_y_1[3];
- /* desired body_y axis for approximate yaw = cross(body_z, x_C) */
- cross3(body_z, x_C, body_y_1);
-
- float w = body_x_norm / body_x_norm_max;
- float w1 = 1.0f - w;
-
- /* mix two body_y axes */
- body_y[0] = body_y[0] * w + body_y_1[0] * w1;
- body_y[1] = body_y[1] * w + body_y_1[1] * w1;
- body_y[2] = body_y[2] * w + body_y_1[2] * w1;
-
- normalize3(body_y);
-
- /* desired body_x axis = cross(body_y, body_z) */
- cross3(body_y, body_z, body_x);
- }
-
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
att_sp.R_body[i][0] = body_x[i];
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-17 08:15:23 +0000