/** * 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); }