/**
* Common function for command service callbacks.
*
* NOTE: success is bool in messages, but has unsigned char type in C++
*/
bool send_command_long_and_wait(bool broadcast,
uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7,
unsigned char &success, uint8_t &result)
{
using mavlink::common::MAV_RESULT;
unique_lock lock(mutex);
L_CommandTransaction::iterator ack_it;
/* check transactions */
for (const auto &tr : ack_waiting_list) {
if (tr.expected_command == command) {
ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Command %u already in progress", command);
return false;
}
}
/**
* @note APM & PX4 master always send COMMAND_ACK. Old PX4 never.
* Don't expect any ACK in broadcast mode.
*/
bool is_ack_required = (confirmation != 0 || m_uas->is_ardupilotmega() || m_uas->is_px4()) && !broadcast;
if (is_ack_required)
ack_it = ack_waiting_list.emplace(ack_waiting_list.end(), command);
command_long(broadcast,
command, confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
if (is_ack_required) {
lock.unlock();
bool is_not_timeout = wait_ack_for(*ack_it);
lock.lock();
success = is_not_timeout && ack_it->result == enum_value(MAV_RESULT::ACCEPTED);
result = ack_it->result;
ack_waiting_list.erase(ack_it);
}
else {
success = true;
result = enum_value(MAV_RESULT::ACCEPTED);
}
return true;
}
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)
{
using mavlink::common::MAV_COMPONENT;
const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();
mavlink::common::msg::COMMAND_INT cmd;
cmd.target_system = tgt_sys_id;
cmd.target_component = tgt_comp_id;
cmd.frame = frame;
cmd.command = command;
cmd.current = current;
cmd.autocontinue = autocontinue;
cmd.param1 = param1;
cmd.param2 = param2;
cmd.param3 = param3;
cmd.param4 = param4;
cmd.x = x;
cmd.y = y;
cmd.z = z;
UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
}
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)
{
using mavlink::common::MAV_COMPONENT;
const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();
const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;
mavlink::common::msg::COMMAND_LONG cmd;
cmd.target_system = tgt_sys_id;
cmd.target_component = tgt_comp_id;
cmd.command = command;
cmd.confirmation = confirmation_fixed;
cmd.param1 = param1;
cmd.param2 = param2;
cmd.param3 = param3;
cmd.param4 = param4;
cmd.param5 = param5;
cmd.param6 = param6;
cmd.param7 = param7;
UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
}
bool set_home_cb(mavros_msgs::CommandHome::Request &req,
mavros_msgs::CommandHome::Response &res)
{
using mavlink::common::MAV_CMD;
return send_command_long_and_wait(false,
enum_value(MAV_CMD::DO_SET_HOME), 1,
(req.current_gps) ? 1.0 : 0.0,
0, 0, 0, req.latitude, req.longitude, req.altitude,
res.success, res.result);
}
bool arming_cb(mavros_msgs::CommandBool::Request &req,
mavros_msgs::CommandBool::Response &res)
{
using mavlink::common::MAV_CMD;
return send_command_long_and_wait(false,
enum_value(MAV_CMD::COMPONENT_ARM_DISARM), 1,
(req.value) ? 1.0 : 0.0,
0, 0, 0, 0, 0, 0,
res.success, res.result);
}
bool land_cb(mavros_msgs::CommandTOL::Request &req,
mavros_msgs::CommandTOL::Response &res)
{
using mavlink::common::MAV_CMD;
return send_command_long_and_wait(false,
enum_value(MAV_CMD::NAV_LAND), 1,
0, 0, 0,
req.yaw,
req.latitude, req.longitude, req.altitude,
res.success, res.result);
}
bool trigger_control_cb(mavros_msgs::CommandTriggerControl::Request &req,
mavros_msgs::CommandTriggerControl::Response &res)
{
using mavlink::common::MAV_CMD;
return send_command_long_and_wait(false,
enum_value(MAV_CMD::DO_TRIGGER_CONTROL), 1,
(req.trigger_enable)? 1.0 : 0.0,
req.integration_time,
0, 0, 0, 0, 0,
res.success, res.result);
}
void MavRos::mavlink_pub_cb(const mavlink_message_t *mmsg, Framing framing)
{
auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
if (mavlink_pub.getNumSubscribers() == 0)
return;
rmsg->header.stamp = ros::Time::now();
mavros_msgs::mavlink::convert(*mmsg, *rmsg, enum_value(framing));
mavlink_pub.publish(rmsg);
}
inline void set_target(MsgT &cmd, bool broadcast)
{
using mavlink::common::MAV_COMPONENT;
const uint8_t tgt_sys_id = (broadcast) ? 0 : m_uas->get_tgt_system();
const uint8_t tgt_comp_id = (broadcast) ? 0 :
(use_comp_id_system_control) ?
enum_value(MAV_COMPONENT::COMP_ID_SYSTEM_CONTROL) : m_uas->get_tgt_component();
cmd.target_system = tgt_sys_id;
cmd.target_component = tgt_comp_id;
}
bool trigger_interval_cb(mavros_msgs::CommandTriggerInterval::Request &req,
mavros_msgs::CommandTriggerInterval::Response &res)
{
using mavlink::common::MAV_CMD;
// trigger interval can only be set when triggering is disabled
return send_command_long_and_wait(false,
enum_value(MAV_CMD::DO_SET_CAM_TRIGG_INTERVAL), 1,
req.cycle_time,
req.integration_time,
0, 0, 0, 0, 0,
res.success, res.result);
}
explicit CommandTransaction(uint16_t command) :
ack(),
expected_command(command),
// Default result if wait ack timeout
result(enum_value(mavlink::common::MAV_RESULT::FAILED))
{ }
explicit CommandTransaction(uint16_t command) :
ack(),
expected_command(command),
result(enum_value(mavlink::common::MAV_RESULT::FAILED))
{ }
/**
* Common function for command service callbacks.
*
* NOTE: success is bool in messages, but has unsigned char type in C++
*/
bool send_command_long_and_wait(bool broadcast,
uint16_t command, uint8_t confirmation,
float param1, float param2,
float param3, float param4,
float param5, float param6,
float param7,
unsigned char &success, uint8_t &result)
{
using mavlink::common::MAV_RESULT;
unique_lock lock(mutex);
/* check transactions */
for (auto it = ack_waiting_list.cbegin();
it != ack_waiting_list.cend(); it++)
if ((*it)->expected_command == command) {
ROS_WARN_THROTTLE_NAMED(10, "cmd", "CMD: Command %u alredy in progress", command);
return false;
}
/**
* @note APM & PX4 master always send COMMAND_ACK. Old PX4 never.
* Don't expect any ACK in broadcast mode.
*/
bool is_ack_required = confirmation != 0 || m_uas->is_ardupilotmega() || m_uas->is_px4();
if (is_ack_required && !broadcast)
ack_waiting_list.push_back(new CommandTransaction(command));
command_long(broadcast,
command, confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
if (is_ack_required && !broadcast) {
auto it = ack_waiting_list.begin();
for (; it != ack_waiting_list.end(); it++)
if ((*it)->expected_command == command)
break;
if (it == ack_waiting_list.end()) {
ROS_ERROR_NAMED("cmd", "CMD: CommandTransaction not found for %u", command);
return false;
}
lock.unlock();
bool is_not_timeout = wait_ack_for(*it);
lock.lock();
success = is_not_timeout && (*it)->result == enum_value(MAV_RESULT::ACCEPTED);
result = (*it)->result;
delete *it;
ack_waiting_list.erase(it);
}
else {
success = true;
result = enum_value(MAV_RESULT::ACCEPTED);
}
return true;
}
