From 651c75558ea0df4348ad8ee346bdfbeaa01a898d Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 13 Jul 2020 14:06:28 -0400 Subject: [PATCH] Omni Pos-Ctrl: Added desired roll/pitch attitude setpoint generation --- .../PositionControl/ControlMath.cpp | 53 +++++++++++++++++-- .../PositionControl/ControlMath.hpp | 14 ++++- .../PositionControl/PositionControl.cpp | 3 +- .../PositionControl/PositionControl.hpp | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 3 +- 5 files changed, 67 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index 7f89ab58bb..be3be34988 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 matrix:: 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 > 3 || omni_att_mode < 0) { + if (omni_att_mode > 6 || omni_att_mode < 0) { PX4_ERR("OMNI_ATT_MODE parameter set to unknown value!"); } @@ -68,6 +68,13 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix:: break; } + case 4: { // Attitude is set to a fixed roll and pitch (used for omnidirectional vehicles) + float roll_angle = math::radians(5.F); + float pitch_angle = math::radians(5.F); + thrustToFixedRollPitch(thr_sp, yaw_sp, att, roll_angle, pitch_angle, 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(); @@ -332,9 +339,47 @@ void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const curr_z(i) = R_curr(i, 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); + att_sp.thrust_body[0] = thr_sp.dot(curr_x); + att_sp.thrust_body[1] = thr_sp.dot(curr_y); + att_sp.thrust_body[2] = thr_sp.dot(curr_z); +} + +void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, + const float roll_angle, + const float pitch_angle, vehicle_attitude_setpoint_s &att_sp) +{ + Eulerf euler_cmd(roll_angle, pitch_angle, yaw_sp); + + Quatf q_sp = euler_cmd; + q_sp.copyTo(att_sp.q_d); + + matrix::Dcmf R_body = q_sp; + Vector3f body_x, body_y, body_z; + + 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 euler angles, for logging only, must not be used for control + att_sp.roll_body = roll_angle; + att_sp.pitch_body = pitch_angle; + att_sp.yaw_body = yaw_sp; + + matrix::Dcmf R_curr = att; + + Vector3f curr_x, curr_y, curr_z; + + for (int i = 0; i < 3; i++) { + curr_x(i) = R_curr(i, 0); + curr_y(i) = R_curr(i, 1); + curr_z(i) = R_curr(i, 2); + } + + att_sp.thrust_body[0] = thr_sp.dot(curr_x); + att_sp.thrust_body[1] = thr_sp.dot(curr_y); + att_sp.thrust_body[2] = thr_sp.dot(curr_z); } Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index 12b29c1171..4dd9ecd5aa 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -83,7 +83,7 @@ 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. + * Converts thrust vector and yaw set-point to a desired-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 @@ -94,6 +94,18 @@ void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_s const float tilt_angle, const float tilt_dir, vehicle_attitude_setpoint_s &att_sp); +/** + * Converts thrust vector and yaw set-point to a desired given attitude for an omni-directional multirotor. + * @param thr_sp a 3D vector + * @param yaw_sp the desired yaw + * @param roll_angle the desired roll angle + * @param pitch_angle the desired pitch angle + * @param att_sp attitude setpoint to fill + */ +void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, + const float roll_angle, + const float pitch_angle, vehicle_attitude_setpoint_s &att_sp); + /** * Outputs the sum of two vectors but respecting the limits and priority. * The sum of two vectors are constraint such that v0 has priority over v1. diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 325bc09bef..1f0a6de67e 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -362,7 +362,8 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s } void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, - const float omni_dfc_max_thrust, + const float omni_dfc_max_thrust, float &omnni_att_tilt_angle, float &omnni_att_tilt_dir, float &omnni_att_roll, + float &omnni_att_pitch, vehicle_attitude_setpoint_s &attitude_setpoint) const { ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index b9e567441c..9b54aa0aff 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -174,6 +174,7 @@ public: * @param attitude_setpoint reference to struct to fill up */ void getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, + float &omnni_att_tilt_angle, float &omnni_att_tilt_dir, float &omnni_att_roll, float &omnni_att_pitch, vehicle_attitude_setpoint_s &attitude_setpoint) const; private: diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6d06d8f666..c0221bbe62 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -662,8 +662,9 @@ MulticopterPositionControl::Run() vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; + float omnni_att_tilt_angle, omnni_att_tilt_dir, omnni_att_roll, omnni_att_pitch; _control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), - attitude_setpoint); + omnni_att_tilt_angle, omnni_att_tilt_dir, omnni_att_roll, omnni_att_pitch, attitude_setpoint); // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.