/**
* Send transform to FCU position controller
*
* Note: send only XYZ, Yaw
*/
void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
// ENU frame
tf::Vector3 origin = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
/* Documentation start from bit 1 instead 0,
* Ignore velocity and accel vectors, yaw rate
*/
uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
if (uas->is_px4()) {
/**
* Current PX4 has bug: it cuts throttle if there no velocity sp
* Issue #273.
*
* @todo Revesit this quirk later. Should be fixed in firmware.
*/
ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
}
// ENU->NED. Issue #49.
set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_xyz_y,
origin.y(), origin.x(), -origin.z(),
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
tf::getYaw(q), 0.0);
}
/**
* Send velocity to FCU velocity controller
*
* Note: send only VX VY VZ. ENU frame.
*/
void send_setpoint_velocity(const ros::Time &stamp, float vx, float vy, float vz, float yaw_rate) {
/* Documentation start from bit 1 instead 0,
* Ignore position and accel vectors, yaw
*/
uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);
// ENU->NED. Issue #49.
set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_v_xyz_yr,
0.0, 0.0, 0.0,
vy, vx, -vz,
0.0, 0.0, 0.0,
0.0, yaw_rate);
}
/**
* Send velocity to FCU velocity controller
*
* Note: send only VX VY VZ. ENU frame.
*/
void send_setpoint_velocity(float vx, float vy, float vz) {
/* Documentation start from bit 1 instead 0,
* but implementation PX4 Firmware #1151 starts from 0
*/
uint16_t ignore_all_except_v_xyz = (7<<6)|(7<<0);
// ENU->NED. Issue #49.
set_position_target_local_ned(ros::Time::now().toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_v_xyz,
0.0, 0.0, 0.0,
vy, vx, -vz,
0.0, 0.0, 0.0);
}
/**
* Send transform to FCU position controller
*
* Note: send only XYZ, Yaw
*/
void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
// ENU frame
tf::Vector3 origin = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
/* Documentation start from bit 1 instead 0,
* Ignore velocity and accel vectors, yaw rate
*/
uint16_t ignore_all_except_xyzy = (1<<11)|(7<<6)|(7<<3);
// ENU->NED. Issue #49.
set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_xyzy,
origin.y(), origin.x(), -origin.z(),
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
tf::getYaw(q), 0.0);
}
/**
* @brief Send setpoint to FCU position controller.
*
* @warning Send only XYZ, Yaw. ENU frame.
*/
void send_position_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
/* Documentation start from bit 1 instead 0;
* Ignore velocity and accel vectors, yaw rate.
*
* In past versions on PX4 there been bug described in #273.
* If you got similar issue please try update firmware first.
*/
const uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);
auto p = UAS::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
auto q = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation()));
set_position_target_local_ned(stamp.toNSec() / 1000000,
MAV_FRAME_LOCAL_NED,
ignore_all_except_xyz_y,
p.x(), p.y(), p.z(),
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
UAS::quaternion_get_yaw(q), 0.0);
}
