void command_long(bool broadcast,
uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7)
{
mavlink_message_t msg;
const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg,
tgt_sys_id,
tgt_comp_id,
command,
confirmation_fixed,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg);
}
/**
* @brief Send a safety zone (volume), which is defined by two corners of a cube,
* to the FCU.
*
* @note ENU frame.
*/
void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2)
{
ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safty area: P1 " << p1 << " P2 " << p2);
p1 = ftf::transform_frame_enu_ned(p1);
p2 = ftf::transform_frame_enu_ned(p2);
mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s;
m_uas->msg_set_target(s);
// TODO: use enum from lib
s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED);
// [[[cog:
// for p in range(1, 3):
// for v in ('x', 'y', 'z'):
// cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v))
// ]]]
s.p1x = p1.x();
s.p1y = p1.y();
s.p1z = p1.z();
s.p2x = p2.x();
s.p2y = p2.y();
s.p2z = p2.z();
// [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0)
UAS_FCU(m_uas)->send_message_ignore_drop(s);
}
void override_cb(const mavros_msgs::OverrideRCIn::ConstPtr req)
{
if (!m_uas->is_ardupilotmega() && !m_uas->is_px4())
ROS_WARN_THROTTLE_NAMED(30, "rc", "RC override not supported by this FCU!");
mavlink::common::msg::RC_CHANNELS_OVERRIDE ovr;
ovr.target_system = m_uas->get_tgt_system();
ovr.target_component = m_uas->get_tgt_component();
// [[[cog:
// for i in range(1, 9):
// cog.outl("ovr.chan%d_raw = req->channels[%d];" % (i, i - 1))
// ]]]
ovr.chan1_raw = req->channels[0];
ovr.chan2_raw = req->channels[1];
ovr.chan3_raw = req->channels[2];
ovr.chan4_raw = req->channels[3];
ovr.chan5_raw = req->channels[4];
ovr.chan6_raw = req->channels[5];
ovr.chan7_raw = req->channels[6];
ovr.chan8_raw = req->channels[7];
// [[[end]]] (checksum: bd27f3e85f5ab614ce1332ae3f4c6ebd)
UAS_FCU(m_uas)->send_message_ignore_drop(ovr);
}
void command_int(bool broadcast,
uint8_t frame, uint16_t command,
uint8_t current, uint8_t autocontinue,
float param1, float param2,
float param3, float param4,
int32_t x, int32_t y,
float z)
{
mavlink_message_t msg;
const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg,
tgt_sys_id,
tgt_comp_id,
frame,
command,
current,
autocontinue,
param1, param2,
param3, param4,
x, y, z);
UAS_FCU(uas)->send_message(&msg);
}
/*
* Arming/disarming the UAV
*/
void arming(const mms::Arm::ConstPtr msg){
mavlink_message_t msg_mav;
if (msg->arm_disarm){
enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
float param1 = 1; //1-->arm 0-->disarm
float param2 = 0; //not used
float param3 = 0; //not used
float param4 = 0; //not used
float param5 = 0; //not used
float param6 = 0; //not used
float param7 = 0; //not used
uint8_t confirmation = 1;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
uas->get_tgt_system(),
uas->get_tgt_component(),
command,
confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg_mav);
ROS_INFO("Arming UAV");
} else {
enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
float param1 = 0; //1-->arm 0-->disarm
float param2 = 0; //not used
float param3 = 0; //not used
float param4 = 0; //not used
float param5 = 0; //not used
float param6 = 0; //not used
float param7 = 0; //not used
uint8_t confirmation = 1;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
uas->get_tgt_system(),
uas->get_tgt_component(),
command,
confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
UAS_FCU(uas)->send_message(&msg_mav); //TODO decide if send or not disarm by software
ROS_INFO("Disarming UAV");
}
}
void vision_speed_estimate(uint64_t usec,
float x, float y, float z) {
mavlink_message_t msg;
mavlink_msg_vision_speed_estimate_pack_chan(UAS_PACK_CHAN(uas), &msg,
usec,
x, y, z);
UAS_FCU(uas)->send_message(&msg);
}
void laser_distance_send(float a, float b, float c, float d) {
mavlink_message_t laser_distance_msg;
mavlink_msg_laser_distance_pack_chan(UAS_PACK_CHAN(uas),&laser_distance_msg,a,b,c,d); //pack
UAS_FCU(uas)->send_message(&laser_distance_msg); //send
ROS_INFO("float_b %d %d", laser_distance_msg.seq,laser_distance_msg.len);
}
void pump_controller_send(float a, float b){
mavlink_message_t pump_controller_msg;
mavlink_msg_pump_controller_pack_chan(UAS_PACK_CHAN(uas),&pump_controller_msg,a,b); //pack
UAS_FCU(uas)->send_message(&pump_controller_msg); //send
//ROS_INFO("size %d %f", field_size_msg.seq, a);
}
void send_timesync_msg(uint64_t tc1, uint64_t ts1) {
mavlink_message_t msg;
mavlink_msg_timesync_pack_chan(UAS_PACK_CHAN(uas), &msg,
tc1,
ts1
);
UAS_FCU(uas)->send_message(&msg);
}
void sonar_distance_send(float a, float b, float c, float d, float e, float f, float g){
mavlink_message_t sonar_distance_msg;
mavlink_msg_sonar_distance_pack_chan(UAS_PACK_CHAN(uas),&sonar_distance_msg,a,b,c,d,e,f,g); //pack
UAS_FCU(uas)->send_message(&sonar_distance_msg); //send
//mavlink_msg_sonar_distance_send(MAVLINK_COMM_1,a,b,c,d,e,f);
ROS_INFO("float_a %d %d", sonar_distance_msg.seq,sonar_distance_msg.len);
//uas->mav_link->send_message(&msg);
}
void heartbeat_cb(const ros::TimerEvent &event) {
mavlink_message_t msg;
mavlink_msg_heartbeat_pack_chan(UAS_PACK_CHAN(uas), &msg,
MAV_TYPE_ONBOARD_CONTROLLER,
MAV_AUTOPILOT_INVALID,
MAV_MODE_MANUAL_ARMED,
0,
MAV_STATE_ACTIVE
);
UAS_FCU(uas)->send_message(&msg);
}
void sys_time_cb(const ros::TimerEvent &event) {
// For filesystem only
mavlink_message_t msg;
uint64_t time_unix_usec = ros::Time::now().toNSec() / 1000; // nano -> micro
mavlink_msg_system_time_pack_chan(UAS_PACK_CHAN(uas), &msg,
time_unix_usec,
0
);
UAS_FCU(uas)->send_message(&msg);
}
/*
* From directive to RC
*/
void directive_cb(const guidance_node_amsl::Directive::ConstPtr msg){
/*
* Initializing values, if safety is on, these values will remain
*/
velocity_.channels[0]=0;
velocity_.channels[1]=0;
velocity_.channels[2]=0;
velocity_.channels[3]=0;
velocity_.channels[4]=0;
velocity_.channels[5]=0;
velocity_.channels[6]=0;
velocity_.channels[7]=0;
if(!safetyOn){
/*
* If safety is off, I translate velocity in RC values
*/
uint16_t vx_RC= (uint16_t)400.0f*(-msg->vxBody)/v_xy_max + 1520; //New: 400 + 1520; Old: 500 + 1500
uint16_t vy_RC=(uint16_t)400.0f*(msg->vyBody)/v_xy_max + 1520; //New: 400 + 1520; Old: 500 + 1500
/*
* it seems it loiters with 1420 instead of 1500...
*/
uint16_t vz_RC= 1420;
if (msg->vzBody > 0){ //going down, mapped only in 420us
vz_RC = vz_RC + (uint16_t)320.0f*(-msg->vzBody)/v_z_max; //New: 320; Old: 420
} else { //going up, mapped in 580us
vz_RC = vz_RC + (uint16_t)480.0f*(-msg->vzBody)/v_z_max; //New: 480 Old: 580
}
uint16_t v_psi_RC = (uint16_t)400.0f*(msg->yawRate)/v_psi_max + 1520; //New: 400 + 1520; Old: 500 + 1500
velocity_.channels[0]=vy_RC;
velocity_.channels[1]=vx_RC;
velocity_.channels[2]=vz_RC;
velocity_.channels[3]=v_psi_RC;
}
//DEBUG
/*ROS_INFO("RC_OVERRIDE: [CH1:%u, CH2:%u, CH3:%u, CH4:%u, CH5:%u, CH6:%u, CH7:%u, CH8:%u]",
velocity_.channels[0], velocity_.channels[1],
velocity_.channels[2], velocity_.channels[3],
velocity_.channels[4], velocity_.channels[5],
velocity_.channels[6], velocity_.channels[7]);*/
//velocity_publisher_.publish(velocity_);
mavlink_message_t msg_mav;
mavlink_msg_rc_channels_raw_pack_chan(UAS_PACK_CHAN(uas),&msg_mav,0,1,velocity_.channels[0],velocity_.channels[1],velocity_.channels[2],velocity_.channels[3],0,0,0,0,100); //1 is the sequence that we are not considering right now
UAS_FCU(uas)->send_message(&msg_mav);
}
//! message definiton here: @p https://pixhawk.ethz.ch/mavlink/#SET_ACTUATOR_CONTROL_TARGET
void set_actuator_control_target(const uint64_t time_usec,
const uint8_t group_mix,
const float controls[8])
{
mavlink_message_t msg;
mavlink_msg_set_actuator_control_target_pack_chan(UAS_PACK_CHAN(uas), &msg,
time_usec,
group_mix,
UAS_PACK_TGT(uas),
controls);
UAS_FCU(uas)->send_message(&msg);
}
bool set_rate_cb(mavros::StreamRate::Request &req,
mavros::StreamRate::Response &res) {
mavlink_message_t msg;
mavlink_msg_request_data_stream_pack_chan(UAS_PACK_CHAN(uas), &msg,
UAS_PACK_TGT(uas),
req.stream_id,
req.message_rate,
(req.on_off) ? 1 : 0
);
UAS_FCU(uas)->send_message(&msg);
return true;
}
// perhaps better add send_ prefix, naming: message name in lower case
void vision_position_estimate(float x, float y, float z, float roll, float pitch, float yaw) {
mavlink_message_t msg;
mavlink_msg_vision_position_estimate_pack(0, 0, &msg,
0,
x,
y,
z,
roll,
pitch,
yaw);
//uas->mav_link->send_message(&msg);
UAS_FCU(uas)->send_message(&msg);
}
void vicon_send(
float x, float y, float z,
float vx, float vy, float vz) {
mavlink_message_t vicon_msg;
mavlink_msg_vicon_pack_chan(UAS_PACK_CHAN(uas), &vicon_msg,
x,
y,
z,
vx,
vy,
vz);
UAS_FCU(uas)->send_message(&vicon_msg);
ROS_INFO("send");
}
void vision_position_estimate(uint64_t usec,
float x, float y, float z,
float roll, float pitch, float yaw) {
mavlink_message_t msg;
mavlink_msg_vision_position_estimate_pack_chan(UAS_PACK_CHAN(uas), &msg,
usec,
x,
y,
z,
roll,
pitch,
yaw);
UAS_FCU(uas)->send_message(&msg);
}
/* -*- low-level send -*- */
void mocap_pose_send
(uint64_t usec,
float q[4],
float x, float y, float z)
{
mavlink_message_t msg;
mavlink_msg_att_pos_mocap_pack_chan(UAS_PACK_CHAN(uas), &msg,
usec,
q,
x,
y,
z);
UAS_FCU(uas)->send_message(&msg);
}
/**
* @brief Handle mavros_msgs::RTCM message
* It converts the message to the MAVLink GPS_RTCM_DATA message for GPS injection.
* Message specification: https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA
* @param msg Received ROS msg
*/
void rtcm_cb(const mavros_msgs::RTCM::ConstPtr &msg)
{
mavlink::common::msg::GPS_RTCM_DATA rtcm_data;
const size_t max_frag_len = rtcm_data.data.size();
uint8_t seq_u5 = uint8_t(msg->header.seq & 0x1F) << 3;
if (msg->data.size() > 4 * max_frag_len) {
ROS_FATAL("gps_rtk: RTCM message received is bigger than the maximal possible size.");
return;
}
auto data_it = msg->data.begin();
auto end_it = msg->data.end();
if (msg->data.size() <= max_frag_len) {
rtcm_data.len = msg->data.size();
rtcm_data.flags = seq_u5;
std::copy(data_it, end_it, rtcm_data.data.begin());
std::fill(rtcm_data.data.begin() + rtcm_data.len, rtcm_data.data.end(), 0);
UAS_FCU(m_uas)->send_message(rtcm_data);
} else {
for (uint8_t fragment_id = 0; fragment_id < 4 && data_it < end_it; fragment_id++) {
uint8_t len = std::min((size_t) std::distance(data_it, end_it), max_frag_len);
rtcm_data.flags = 1; // LSB set indicates message is fragmented
rtcm_data.flags |= fragment_id << 1; // Next 2 bits are fragment id
rtcm_data.flags |= seq_u5; // Next 5 bits are sequence id
rtcm_data.len = len;
std::copy(data_it, data_it + len, rtcm_data.data.begin());
std::fill(rtcm_data.data.begin() + len, rtcm_data.data.end(), 0);
UAS_FCU(m_uas)->send_message(rtcm_data);
std::advance(data_it, len);
}
}
}
void set_attitude_target(uint32_t time_boot_ms,
uint8_t type_mask,
float q[4],
float roll_rate, float pitch_rate, float yaw_rate,
float thrust) {
mavlink_message_t msg;
mavlink_msg_set_attitude_target_pack_chan(UAS_PACK_CHAN(uas), &msg,
time_boot_ms,
UAS_PACK_TGT(uas),
type_mask,
q,
roll_rate, pitch_rate, yaw_rate,
thrust);
UAS_FCU(uas)->send_message(&msg);
}
void vision_speed_estimate(uint64_t usec, Eigen::Vector3d &v)
{
mavlink::common::msg::VISION_SPEED_ESTIMATE vs{};
vs.usec = usec;
// [[[cog:
// for f in "xyz":
// cog.outl("vs.%s = v.%s();" % (f, f))
// ]]]
vs.x = v.x();
vs.y = v.y();
vs.z = v.z();
// [[[end]]] (checksum: aee3cc9a73a2e736b7bc6c83ea93abdb)
UAS_FCU(m_uas)->send_message_ignore_drop(vs);
}
/* -*- low-level send -*- */
void distance_sensor(uint32_t time_boot_ms,
uint32_t min_distance,
uint32_t max_distance,
uint32_t current_distance,
uint8_t type, uint8_t id,
uint8_t orientation, uint8_t covariance) {
mavlink_message_t msg;
mavlink_msg_distance_sensor_pack_chan(UAS_PACK_CHAN(uas), &msg,
time_boot_ms,
min_distance,
max_distance,
current_distance,
type,
id,
orientation,
covariance);
UAS_FCU(uas)->send_message(&msg);
}
bool set_mode_cb(mavros::SetMode::Request &req,
mavros::SetMode::Response &res) {
mavlink_message_t msg;
uint8_t base_mode = req.base_mode;
uint32_t custom_mode = 0;
if (req.custom_mode != "") {
if (!uas->cmode_from_str(req.custom_mode, custom_mode)) {
res.success = false;
return true;
}
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
}
mavlink_msg_set_mode_pack_chan(UAS_PACK_CHAN(uas), &msg,
uas->get_tgt_system(),
base_mode,
custom_mode);
UAS_FCU(uas)->send_message(&msg);
res.success = true;
return true;
}
void vision_position_estimate(uint64_t usec,
Eigen::Vector3d &position,
Eigen::Vector3d &rpy)
{
mavlink::common::msg::VISION_POSITION_ESTIMATE vp{};
vp.usec = usec;
// [[[cog:
// for f in "xyz":
// cog.outl("vp.%s = position.%s();" % (f, f))
// for a, b in zip("xyz", ('roll', 'pitch', 'yaw')):
// cog.outl("vp.%s = rpy.%s();" % (b, a))
// ]]]
vp.x = position.x();
vp.y = position.y();
vp.z = position.z();
vp.roll = rpy.x();
vp.pitch = rpy.y();
vp.yaw = rpy.z();
// [[[end]]] (checksum: 2048daf411780847e77f08fe5a0b9dd3)
UAS_FCU(m_uas)->send_message_ignore_drop(vp);
}
void send(bool a, float b, float c, float d, float e){
mavlink_message_t msg;
mavlink_msg_ardrone_position_pack_chan(UAS_PACK_CHAN(uas),&msg,a,b,c,d,e); //pack
UAS_FCU(uas)->send_message(&msg); //send
ROS_INFO("Send!\nrobot_x:%f\nrobot_y:%f\ncatch_x:%f\ncatch_y:%f\n", b, c, d, e);
}
