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