From b168ba92e0be91aec9db5aaccbcc427cfd067c7b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 29 Oct 2014 15:57:45 +0100 Subject: [PATCH] added setter functions into base class. used when integrating the base class into a gazebo environment --- .../mc_att_control/mc_att_control_base.cpp | 57 +++++++++++++++++++ .../mc_att_control/mc_att_control_base.h | 7 +++ 2 files changed, 64 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d4270b1530..d39bae4dc9 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -281,3 +281,60 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } } + +void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion& attitude) { + // check if this is consistent !!! + math::Vector<3> quat; + quat(0) = attitude(0); + quat(1) = attitude(1); + quat(2) = attitude(2); + quat(3) = attitude(3); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; + +} + +void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); + +} + +void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = control_attitude_thrust_reference(3); + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); + +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index d75088b85b..995838bb27 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -166,6 +166,13 @@ protected: void vehicle_attitude_setpoint_poll(); //provisional + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion& attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + + + };