/**
* Send angular velocity setpoint to FCU attitude controller
*
* ENU frame.
*/
void send_attitude_ang_velocity(const ros::Time &stamp, const float vx, const float vy, const float vz) {
// Q + Thrust, also bits noumbering started from 1 in docs
const uint8_t ignore_all_except_rpy = (1<<7)|(1<<6);
float q[4] = { 1.0, 0.0, 0.0, 0.0 };
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_rpy,
q,
vy, vx, -vz,
0.0);
}
/**
* Send throttle to FCU attitude controller
*/
void send_attitude_throttle(const float throttle) {
// Q + RPY
const uint8_t ignore_all_except_throttle = (1<<7)|(7<<0);
float q[4] = { 1.0, 0.0, 0.0, 0.0 };
set_attitude_target(ros::Time::now().toNSec() / 1000000,
ignore_all_except_throttle,
q,
0.0, 0.0, 0.0,
throttle);
}
/**
* @brief Send angular velocity setpoint to FCU attitude controller
*
* @note ENU frame.
*/
void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel) {
/* Q + Thrust, also bits noumbering started from 1 in docs
*/
const uint8_t ignore_all_except_rpy = (1 << 7) | (1 << 6);
float q[4] = { 1.0, 0.0, 0.0, 0.0 };
auto av = UAS::transform_frame_enu_ned(ang_vel);
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_rpy,
q,
av.x(), av.y(), av.z(),
0.0);
}
/**
* @brief Send angular velocity setpoint and thrust to FCU attitude controller
*/
void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel, const float thrust)
{
/**
* @note Q, also bits noumbering started from 1 in docs
*/
const uint8_t ignore_all_except_rpy = (1 << 7);
auto av = ftf::transform_frame_baselink_aircraft(ang_vel);
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_rpy,
Eigen::Quaterniond::Identity(),
av,
thrust);
}
/**
* Send attitude and thrust to FCU attitude controller
*/
void send_attitude_and_throttle(const ros::Time &stamp, const float qx, const float qy, const float qz, const float qw, const float throttle) {
// check mavlink_msg_set_attitude_target.h for type_mask definition
const uint8_t ignore_all_except_q_and_throttle = (7<<0);
float q[4];
q[0] = qw;
q[1] = qx;
q[2] = qy;
q[3] = qz;
set_attitude_target(ros::Time::now().toNSec() / 1000000,
ignore_all_except_q_and_throttle,
q,
0.0, 0.0, 0.0,
throttle);
}
/**
* @brief Send attitude setpoint to FCU attitude controller
*
* @note ENU frame.
*/
void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
/* Thrust + RPY, also bits numbering started from 1 in docs
*/
const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
float q[4];
// Eigen use same convention as mavlink: w x y z
Eigen::Map<Eigen::Quaternionf> q_out(q);
q_out = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())).cast<float>();
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_q,
q,
0.0, 0.0, 0.0,
0.0);
}
/**
* @brief Send attitude setpoint to FCU attitude controller
*
* @note ENU frame.
*/
void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
/* Thrust + RPY, also bits numbering started from 1 in docs
*/
const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
float q[4];
UAS::quaternion_to_mavlink(
UAS::transform_orientation_enu_ned(
UAS::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))),q);
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_q,
q,
0.0, 0.0, 0.0,
0.0);
}
/**
* @brief Send attitude setpoint and thrust to FCU attitude controller
*/
void send_attitude_quaternion(const ros::Time &stamp, const Eigen::Affine3d &tr, const float thrust)
{
/**
* @note RPY, also bits numbering started from 1 in docs
*/
const uint8_t ignore_all_except_q_and_thrust = (7 << 0);
auto q = ftf::transform_orientation_enu_ned(
ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))
);
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_q_and_thrust,
q,
Eigen::Vector3d::Zero(),
thrust);
}
/**
* Send attitude setpoint to FCU attitude controller
*
* ENU frame.
*/
void send_attitude_transform(const tf::Transform &transform, const ros::Time &stamp) {
// Thrust + RPY, also bits noumbering started from 1 in docs
const uint8_t ignore_all_except_q = (1<<6)|(7<<0);
float q[4];
// ENU->NED, description in #49.
tf::Quaternion tf_q = transform.getRotation();
q[0] = tf_q.w();
q[1] = tf_q.y();
q[2] = tf_q.x();
q[3] = -tf_q.z();
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_q,
q,
0.0, 0.0, 0.0,
0.0);
}
