From 725fff3052b695a792e9a762b01b0a819ed23c95 Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 13 Jul 2020 13:37:21 -0400 Subject: [PATCH] Omni Pos-Ctrl: Current attitude passed to attitude generation methods --- .../PositionControl/ControlMath.cpp | 19 +++++++++++++++---- .../PositionControl/ControlMath.hpp | 6 ++++-- .../PositionControl/PositionControl.cpp | 5 +++-- .../PositionControl/PositionControl.hpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 5 +++-- 5 files changed, 26 insertions(+), 11 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index 38a8835a73..7f89ab58bb 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -44,7 +44,7 @@ using namespace matrix; namespace ControlMath { -void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode, +void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode, 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 @@ -64,7 +64,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni 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); + thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, tilt_angle, tilt_dir, att_sp); break; } @@ -175,7 +175,7 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f Vector2f thr_sp_h(thr_sp(0), thr_sp(1)); if (thr_sp_h.norm() <= omni_dfc_max_thrust) { - thrustToAttitude(thr_sp, yaw_sp, 2, omni_dfc_max_thrust, att_sp); + thrustToAttitude(thr_sp, yaw_sp, matrix::Quatf(), 2, omni_dfc_max_thrust, att_sp); return; } @@ -257,7 +257,8 @@ 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, +void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, + const float tilt_angle, const float tilt_dir, vehicle_attitude_setpoint_s &att_sp) { Vector3f body_z; @@ -321,6 +322,16 @@ void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const att_sp.pitch_body = euler(1); att_sp.yaw_body = euler(2); + 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(body_x); att_sp.thrust_body[1] = thr_sp.dot(body_y); att_sp.thrust_body[2] = thr_sp.dot(body_z); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index bbec267a9e..12b29c1171 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -53,7 +53,8 @@ namespace ControlMath * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles */ -void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode, +void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, + const int omni_att_mode, const float omni_dfc_max_thrust, vehicle_attitude_setpoint_s &att_sp); /** @@ -89,7 +90,8 @@ void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, * @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, +void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, + const float tilt_angle, const float tilt_dir, vehicle_attitude_setpoint_s &att_sp); /** diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 34873b93e6..325bc09bef 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -361,9 +361,10 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s _thr_sp.copyTo(local_position_setpoint.thrust); } -void PositionControl::getAttitudeSetpoint(const int omni_att_mode, const float omni_dfc_max_thrust, +void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, + const float omni_dfc_max_thrust, vehicle_attitude_setpoint_s &attitude_setpoint) const { - ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint); + ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint); attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index a41715260b..b9e567441c 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -173,7 +173,7 @@ public: * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles * @param attitude_setpoint reference to struct to fill up */ - void getAttitudeSetpoint(const int omni_att_mode, const float omni_dfc_max_thrust, + void getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, 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 07fe595f0d..b5640eabc1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -83,6 +83,7 @@ class MulticopterPositionControl : public ModuleBase public ModuleParams, public px4::WorkItem { public: + vehicle_attitude_s att; MulticopterPositionControl(bool vtol = false); ~MulticopterPositionControl() override; @@ -400,7 +401,6 @@ MulticopterPositionControl::poll_subscriptions() _home_pos_sub.update(&_home_pos); if (_att_sub.updated()) { - vehicle_attitude_s att; if (_att_sub.copy(&att) && PX4_ISFINITE(att.q[0])) { _states.yaw = Eulerf(Quatf(att.q)).psi(); @@ -657,7 +657,8 @@ MulticopterPositionControl::Run() vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; - _control.getAttitudeSetpoint(_param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), attitude_setpoint); + _control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), + 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.