From f18f92a505e9ae39ea4b4026ff11b6f75cc92f0e Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 13 Jul 2020 12:47:45 -0400 Subject: [PATCH] Omni Pos-Ctrl: Fixed-tilt attitude generation strategy implemented - Currently receiving hardcoded tilt angle and direction --- .../mc_att_control/mc_att_control_params.c | 6 +- .../PositionControl/ControlMath.cpp | 84 +++++++++++++++++-- .../PositionControl/ControlMath.hpp | 10 +++ 3 files changed, 92 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index cd1766b33f..a7dcd63a46 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -171,13 +171,15 @@ PARAM_DEFINE_FLOAT(OMNI_DFC_MAX_THR, 0.15f); * The "constant zero tilt" mode enforces zero roll and pitch and can only be * used for omnidirectional vehicles. The min-tilt mode enforces zero tilt * up to a maximum set acceleration (thrust) and tilts the vehicle as small - * as possible if the thrust vector is larger than the maximum. + * as possible if the thrust vector is larger than the maximum. The "constant + * tilt" mode enforces a given tilt angle and tilt direction for the vehicle. * * @min 0 - * @max 2 + * @max 3 * @value 0 tilted attitude * @value 1 min-tilt attitude * @value 2 constant zero tilt + * @value 3 constant tilt * @group Multicopter Attitude Control */ PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index 6523695307..38a8835a73 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -48,7 +48,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni const float omni_dfc_max_thrust, vehicle_attitude_setpoint_s &att_sp) { // Print an error if the omni_att_mode parameter is out of range - if (omni_att_mode > 2 || omni_att_mode < 0) { + if (omni_att_mode > 3 || omni_att_mode < 0) { PX4_ERR("OMNI_ATT_MODE parameter set to unknown value!"); } @@ -61,6 +61,13 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni thrustToZeroTiltAttitude(thr_sp, yaw_sp, att_sp); break; + case 3: { // Attitude is set to a fixed tilt at a fixed global direction (used for omnidirectional vehicles) + float tilt_angle = math::radians(10.F); + float tilt_dir = math::radians(270.F); + thrustToFixedTiltAttitude(thr_sp, yaw_sp, tilt_angle, tilt_dir, att_sp); + break; + } + default: // Attitude is calculated from the desired thrust direction bodyzToAttitude(-thr_sp, yaw_sp, att_sp); att_sp.thrust_body[2] = -thr_sp.length(); @@ -125,9 +132,6 @@ void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicl // set Z axis to upward direction Vector3f body_z = Vector3f(0.f, 0.f, 1.f); - // vector of desired yaw direction in XY plane, rotated by PI/2 - Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f); - // desired body_x and body_y axis Vector3f body_x = Vector3f(cos(yaw_sp), sin(yaw_sp), 0.0f); Vector3f body_y = Vector3f(-sinf(yaw_sp), cosf(yaw_sp), 0.0f); @@ -151,7 +155,6 @@ void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicl att_sp.pitch_body = 0; att_sp.yaw_body = yaw_sp; - att_sp.thrust_body[0] = thr_sp.dot(body_x); att_sp.thrust_body[1] = thr_sp.dot(body_y); att_sp.thrust_body[2] = thr_sp(2); @@ -161,7 +164,7 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f vehicle_attitude_setpoint_s &att_sp) { Vector3f body_z; - float lambda = 0.f; + float lambda = 0.f; // the minimum tilt angle // zero vector, no direction, set safe level value if (thr_sp.norm_squared() < FLT_EPSILON) { @@ -254,6 +257,75 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f att_sp.thrust_body[2] = -f_eff_z; } +void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const float tilt_angle, const float tilt_dir, + vehicle_attitude_setpoint_s &att_sp) +{ + Vector3f body_z; + + // zero vector, no direction, set safe level value + if (thr_sp.norm_squared() < FLT_EPSILON) { + body_z(2) = 1.f; + + } else { + // Calculate the direction of the body Z axis + Vector3f v_hat(0.f, 0.f, -1.f); // upward direction + // vector of desired yaw direction in XY plane, rotated by PI/2 + Vector3f kappa_C(cosf(tilt_dir), sinf(tilt_dir), 0.0f); + Vector3f p_hat = v_hat % kappa_C; // the axis of rotation for tilt_angle + p_hat.normalize(); + body_z = -(1 - cosf(tilt_angle)) * p_hat * (p_hat.dot(v_hat)) + cosf(tilt_angle) * v_hat - sinf(tilt_angle) * + (v_hat % p_hat); // Rodrigues' rotation formula + body_z = -body_z; + } + + // vector of desired yaw direction in XY plane, rotated by PI/2 + Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f); + + // desired body_x axis, orthogonal to body_z + Vector3f body_x = y_C % body_z; + + // keep nose to front while inverted upside down + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + if (fabsf(body_z(2)) < 0.000001f) { + // desired thrust is in XY plane, set X downside to construct correct matrix, + // but yaw component will not be used actually + body_x.zero(); + body_x(2) = 1.0f; + } + + body_x.normalize(); + + // desired body_y axis + Vector3f body_y = body_z % body_x; + + Dcmf R_sp; + + // fill rotation matrix + for (int i = 0; i < 3; i++) { + R_sp(i, 0) = body_x(i); + R_sp(i, 1) = body_y(i); + R_sp(i, 2) = body_z(i); + } + + // copy quaternion setpoint to attitude setpoint topic + Quatf q_sp = R_sp; + q_sp.copyTo(att_sp.q_d); + att_sp.q_d_valid = true; + + // calculate euler angles, for logging only, must not be used for control + Eulerf euler = R_sp; + att_sp.roll_body = euler(0); + att_sp.pitch_body = euler(1); + att_sp.yaw_body = euler(2); + + att_sp.thrust_body[0] = thr_sp.dot(body_x); + att_sp.thrust_body[1] = thr_sp.dot(body_y); + att_sp.thrust_body[2] = thr_sp.dot(body_z); +} + Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) { if (Vector2f(v0 + v1).norm() <= max) { diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index 970ac36d6b..bbec267a9e 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -81,6 +81,16 @@ void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp */ void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust, vehicle_attitude_setpoint_s &att_sp); +/** + * Converts thrust vector and yaw set-point to a zero-tilt attitude for an omni-directional multirotor. + * @param thr_sp a 3D vector + * @param yaw_sp the desired yaw + * @param tilt_angle the desired tilt angle + * @param tilt_dir the desired tilt direction + * @param att_sp attitude setpoint to fill + */ +void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float tilt_angle, + const float tilt_dir, vehicle_attitude_setpoint_s &att_sp); /** * Outputs the sum of two vectors but respecting the limits and priority.