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);
}
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 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 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 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 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);
}
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 safety_set_allowed_area(
uint8_t coordinate_frame,
float p1x, float p1y, float p1z,
float p2x, float p2y, float p2z) {
mavlink_message_t msg;
mavlink_msg_safety_set_allowed_area_pack_chan(UAS_PACK_CHAN(uas), &msg,
UAS_PACK_TGT(uas),
coordinate_frame,
p1x, p1y, p1z,
p2x, p2y, p2z);
uas->mav_link->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);
}
void optical_flow(uint64_t time_usec, uint8_t sensor_id,
uint16_t flow_x, uint16_t flow_y,
float flow_comp_m_x, float flow_comp_m_y,
uint8_t quality,
float ground_distance) {
mavlink_message_t msg;
mavlink_msg_optical_flow_pack_chan(UAS_PACK_CHAN(uas), &msg,
time_usec, sensor_id,
flow_x, flow_y,
flow_comp_m_x, flow_comp_m_y,
quality, ground_distance);
uas->mav_link->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;
}
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");
}
/* -*- 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);
}
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);
}
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->mav_link->send_message(&msg);
}
/* -*- 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);
}
void command_long(uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7) {
mavlink_message_t msg;
mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg,
UAS_PACK_TGT(uas),
command,
confirmation,
param1,
param2,
param3,
param4,
param5,
param6,
param7);
uas->mav_link->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 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);
}
