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