void connection_cb(bool connected) {
// if connection changes, start delayed version request
version_retries = RETRIES_COUNT;
if (connected)
autopilot_version_timer.start();
else
autopilot_version_timer.stop();
// add/remove APM diag tasks
if (connected && disable_diag && uas->is_ardupilotmega()) {
#ifdef MAVLINK_MSG_ID_MEMINFO
UAS_DIAG(uas).add(mem_diag);
#endif
#ifdef MAVLINK_MSG_ID_HWSTATUS
UAS_DIAG(uas).add(hwst_diag);
#endif
#if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
"Extra diagnostic disabled.");
#else
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
#endif
}
else {
UAS_DIAG(uas).removeByName(mem_diag.getName());
UAS_DIAG(uas).removeByName(hwst_diag.getName());
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
}
}
/**
* 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(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) {
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", "Command %u alredy in progress", command);
return false;
}
//! @note APM always send COMMAND_ACK, while PX4 never.
bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4();
if (is_ack_required)
ack_waiting_list.push_back(new CommandTransaction(command));
command_long(command, confirmation,
param1, param2,
param3, param4,
param5, param6,
param7);
if (is_ack_required) {
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", "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 == MAV_RESULT_ACCEPTED;
result = (*it)->result;
delete *it;
ack_waiting_list.erase(it);
}
else {
success = true;
result = MAV_RESULT_ACCEPTED;
}
return true;
}
void handle_statustext(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_statustext_t textm;
mavlink_msg_statustext_decode(msg, &textm);
std::string text(textm.text,
strnlen(textm.text, sizeof(textm.text)));
if (uas->is_ardupilotmega())
process_statustext_apm_quirk(textm.severity, text);
else
process_statustext_normal(textm.severity, text);
}
void handle_raw_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (has_hr_imu || has_scaled_imu)
return;
sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
mavlink_raw_imu_t imu_raw;
mavlink_msg_raw_imu_decode(msg, &imu_raw);
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id;
/* NOTE: APM send SCALED_IMU data as RAW_IMU */
fill_imu_msg_raw(imu_msg,
imu_raw.xgyro * MILLIRS_TO_RADSEC,
-imu_raw.ygyro * MILLIRS_TO_RADSEC,
-imu_raw.zgyro * MILLIRS_TO_RADSEC,
imu_raw.xacc * MILLIG_TO_MS2,
-imu_raw.yacc * MILLIG_TO_MS2,
-imu_raw.zacc * MILLIG_TO_MS2);
if (!uas->is_ardupilotmega()) {
ROS_WARN_THROTTLE_NAMED(60, "imu", "RAW_IMU: linear acceleration known on APM only");
linear_accel_vec.x = 0.0;
linear_accel_vec.y = 0.0;
linear_accel_vec.z = 0.0;
}
imu_msg->header = header;
imu_raw_pub.publish(imu_msg);
/* -*- magnetic vector -*- */
sensor_msgs::MagneticFieldPtr magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
// Convert from local NED plane to ENU
magn_msg->magnetic_field.x = imu_raw.ymag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.y = imu_raw.xmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.z = -imu_raw.zmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field_covariance = magnetic_cov;
magn_msg->header = header;
magn_pub.publish(magn_msg);
}