void override_cb(const mavros_msgs::OverrideRCIn::ConstPtr req)
{
if (!m_uas->is_ardupilotmega() && !m_uas->is_px4())
ROS_WARN_THROTTLE_NAMED(30, "rc", "RC override not supported by this FCU!");
mavlink::common::msg::RC_CHANNELS_OVERRIDE ovr;
ovr.target_system = m_uas->get_tgt_system();
ovr.target_component = m_uas->get_tgt_component();
// [[[cog:
// for i in range(1, 9):
// cog.outl("ovr.chan%d_raw = req->channels[%d];" % (i, i - 1))
// ]]]
ovr.chan1_raw = req->channels[0];
ovr.chan2_raw = req->channels[1];
ovr.chan3_raw = req->channels[2];
ovr.chan4_raw = req->channels[3];
ovr.chan5_raw = req->channels[4];
ovr.chan6_raw = req->channels[5];
ovr.chan7_raw = req->channels[6];
ovr.chan8_raw = req->channels[7];
// [[[end]]] (checksum: bd27f3e85f5ab614ce1332ae3f4c6ebd)
UAS_FCU(m_uas)->send_message_ignore_drop(ovr);
}
void handle_timesync(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_timesync_t tsync;
mavlink_msg_timesync_decode(msg, &tsync);
uint64_t now_ns = ros::Time::now().toNSec();
if (tsync.tc1 == 0) {
send_timesync_msg(now_ns, tsync.ts1);
return;
}
else if (tsync.tc1 > 0) {
int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1 * 2) / 2;
int64_t dt = time_offset_ns - offset_ns;
if (std::abs(dt) > 10000000) { // 10 millisecond skew
time_offset_ns = offset_ns; // hard-set it.
uas->set_time_offset(time_offset_ns);
dt_diag.clear();
dt_diag.set_timestamp(tsync.tc1);
ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Clock skew detected (%0.9f s). "
"Hard syncing clocks.", dt / 1e9);
}
else {
average_offset(offset_ns);
dt_diag.tick(dt, tsync.tc1, time_offset_ns);
uas->set_time_offset(time_offset_ns);
}
}
}
/**
* @brief Handle RAW_IMU MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html/#RAW_IMU
* @param msg Received Mavlink msg
* @param imu_raw RAW_IMU msg
*/
void handle_raw_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RAW_IMU &imu_raw)
{
ROS_INFO_COND_NAMED(!has_raw_imu, "imu", "IMU: Raw IMU message used.");
has_raw_imu = true;
if (has_hr_imu || has_scaled_imu)
return;
auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
auto header = m_uas->synchronized_header(frame_id, imu_raw.time_usec);
/** @note APM send SCALED_IMU data as RAW_IMU
*/
auto gyro_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
auto accel_frd = Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
auto accel_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(accel_frd);
if (m_uas->is_ardupilotmega()) {
accel_frd *= MILLIG_TO_MS2;
accel_flu *= MILLIG_TO_MS2;
} else if (m_uas->is_px4()) {
accel_frd *= MILLIMS2_TO_MS2;
accel_flu *= MILLIMS2_TO_MS2;
}
publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
if (!m_uas->is_ardupilotmega()) {
ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: linear acceleration on RAW_IMU known on APM only.");
ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: ~imu/data_raw stores unscaled raw acceleration report.");
linear_accel_vec_flu.setZero();
linear_accel_vec_frd.setZero();
}
/** Magnetic field data:
* @snippet src/plugins/imu.cpp mag_field
*/
// [mag_field]
auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
// [mag_field]
publish_mag(header, mag_field);
}
void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_gps_raw_int_t raw_gps;
mavlink_msg_gps_raw_int_decode(msg, &raw_gps);
auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec);
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
if (raw_gps.fix_type > 2)
fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
else {
ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
}
fill_lla(raw_gps, fix);
float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;
if (!isnan(eph)) {
const double hdop = eph;
// From nmea_navsat_driver
fix->position_covariance[0 + 0] = \
fix->position_covariance[3 + 1] = std::pow(hdop, 2);
fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fill_unknown_cov(fix);
}
// store & publish
uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
raw_fix_pub.publish(fix);
if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = angles::from_degrees(raw_gps.cog / 1E2); // rad
auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
vel->header.frame_id = frame_id;
vel->header.stamp = fix->header.stamp;
// From nmea_navsat_driver
vel->twist.linear.x = speed * std::sin(course);
vel->twist.linear.y = speed * std::cos(course);
raw_vel_pub.publish(vel);
}
}
/**
* 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_rc_channels(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS &channels)
{
constexpr size_t MAX_CHANCNT = 18;
lock_guard lock(mutex);
ROS_INFO_COND_NAMED(!has_rc_channels_msg, "rc", "RC_CHANNELS message detected!");
has_rc_channels_msg = true;
if (channels.chancount > MAX_CHANCNT) {
ROS_WARN_THROTTLE_NAMED(60, "rc",
"FCU receives %u RC channels, but RC_CHANNELS can store %zu",
channels.chancount, MAX_CHANCNT);
channels.chancount = MAX_CHANCNT;
}
raw_rc_in.resize(channels.chancount);
// switch works as start point selector.
switch (channels.chancount) {
// [[[cog:
// for i in range(18, 0, -1):
// cog.outl("case %2d: raw_rc_in[%2d] = channels.chan%d_raw;" % (i, i - 1, i))
// ]]]
case 18: raw_rc_in[17] = channels.chan18_raw;
case 17: raw_rc_in[16] = channels.chan17_raw;
case 16: raw_rc_in[15] = channels.chan16_raw;
case 15: raw_rc_in[14] = channels.chan15_raw;
case 14: raw_rc_in[13] = channels.chan14_raw;
case 13: raw_rc_in[12] = channels.chan13_raw;
case 12: raw_rc_in[11] = channels.chan12_raw;
case 11: raw_rc_in[10] = channels.chan11_raw;
case 10: raw_rc_in[ 9] = channels.chan10_raw;
case 9: raw_rc_in[ 8] = channels.chan9_raw;
case 8: raw_rc_in[ 7] = channels.chan8_raw;
case 7: raw_rc_in[ 6] = channels.chan7_raw;
case 6: raw_rc_in[ 5] = channels.chan6_raw;
case 5: raw_rc_in[ 4] = channels.chan5_raw;
case 4: raw_rc_in[ 3] = channels.chan4_raw;
case 3: raw_rc_in[ 2] = channels.chan3_raw;
case 2: raw_rc_in[ 1] = channels.chan2_raw;
case 1: raw_rc_in[ 0] = channels.chan1_raw;
// [[[end]]] (checksum: 56e9ab5407bd2c864abde230a6cf3fed)
case 0: break;
}
auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
rcin_msg->header.stamp = m_uas->synchronise_stamp(channels.time_boot_ms);
rcin_msg->rssi = channels.rssi;
rcin_msg->channels = raw_rc_in;
rc_in_pub.publish(rcin_msg);
}
void message_rx_cb(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(msg, &ack);
lock_guard lock(mutex);
for (auto it = ack_waiting_list.cbegin();
it != ack_waiting_list.cend(); it++)
if ((*it)->expected_command == ack.command) {
(*it)->result = ack.result;
(*it)->ack.notify_all();
return;
}
ROS_WARN_THROTTLE_NAMED(10, "cmd", "Unexpected command %u, result %u",
ack.command, ack.result);
}
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);
}
void handle_message(msgT &rst, uint8_t sysid, uint8_t compid) {
if (sysid != '3' || compid != 'D')
ROS_WARN_THROTTLE_NAMED(30, "radio", "RADIO_STATUS not from 3DR modem?");
tdr_diag.set(rst);
mavros::RadioStatusPtr msg = boost::make_shared<mavros::RadioStatus>();
#define RST_COPY(field) msg->field = rst.field
RST_COPY(rssi);
RST_COPY(remrssi);
RST_COPY(txbuf);
RST_COPY(noise);
RST_COPY(remnoise);
RST_COPY(rxerrors);
RST_COPY(fixed);
#undef RST_COPY
msg->header.stamp = ros::Time::now();
status_pub.publish(msg);
}
void handle_message(msgT &rst, uint8_t sysid, uint8_t compid) {
if (sysid != '3' || compid != 'D')
ROS_WARN_THROTTLE_NAMED(30, "radio", "RADIO_STATUS not from 3DR modem?");
auto msg = boost::make_shared<mavros::RadioStatus>();
msg->header.stamp = ros::Time::now();
#define RST_COPY(field) msg->field = rst.field
RST_COPY(rssi);
RST_COPY(remrssi);
RST_COPY(txbuf);
RST_COPY(noise);
RST_COPY(remnoise);
RST_COPY(rxerrors);
RST_COPY(fixed);
#undef RST_COPY
// valid for 3DR modem
msg->rssi_dbm = (rst.rssi / 1.9) - 127;
msg->remrssi_dbm = (rst.remrssi / 1.9) - 127;
// add diag at first event
if (!diag_added) {
UAS_DIAG(uas).add("3DR Radio", this, &TDRRadioPlugin::diag_run);
diag_added = true;
}
// store last status for diag
{
lock_guard lock(diag_mutex);
last_status = msg;
}
status_pub.publish(msg);
}
void handle_system_time(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_system_time_t mtime;
mavlink_msg_system_time_decode(msg, &mtime);
// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
const bool fcu_time_valid = mtime.time_unix_usec > 1234567890ULL * 1000000;
if (fcu_time_valid) {
// continious publish for ntpd
auto time_unix = boost::make_shared<sensor_msgs::TimeReference>();
ros::Time time_ref(
mtime.time_unix_usec / 1000000, // t_sec
(mtime.time_unix_usec % 1000000) * 1000); // t_nsec
time_unix->header.stamp = ros::Time::now();
time_unix->time_ref = time_ref;
time_unix->source = time_ref_source;
time_ref_pub.publish(time_unix);
}
else {
ROS_WARN_THROTTLE_NAMED(60, "time", "TM: Wrong FCU time.");
}
}
void message_rx_cb(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
switch (msg->msgid) {
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_gps_raw_int_t raw_gps;
mavlink_msg_gps_raw_int_decode(msg, &raw_gps);
sensor_msgs::NavSatFixPtr fix(new sensor_msgs::NavSatFix);
geometry_msgs::TwistStampedPtr vel(new geometry_msgs::TwistStamped);
gps_diag.set_gps_raw(raw_gps);
if (raw_gps.fix_type < 2) {
ROS_WARN_THROTTLE_NAMED(60, "gps", "GPS: no fix");
return;
}
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
if (raw_gps.fix_type == 2 || raw_gps.fix_type == 3)
fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
else
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
fix->latitude = raw_gps.lat / 1E7; // deg
fix->longitude = raw_gps.lon / 1E7; // deg
fix->altitude = raw_gps.alt / 1E3; // m
if (raw_gps.eph != UINT16_MAX) {
double hdop = raw_gps.eph / 1E2;
double hdop2 = std::pow(hdop, 2);
// TODO: Check
// From nmea_navsat_driver
fix->position_covariance[0] = hdop2;
fix->position_covariance[4] = hdop2;
fix->position_covariance[8] = pow(2 * hdop, 2);
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
}
fix->header.frame_id = frame_id;
fix->header.stamp = ros::Time::now();
fix_pub.publish(fix);
if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = raw_gps.cog / 1E2; // deg
// From nmea_navsat_driver
vel->twist.linear.x = speed * std::sin(course);
vel->twist.linear.y = speed * std::cos(course);
vel->header.frame_id = frame_id;
vel->header.stamp = fix->header.stamp;
vel_pub.publish(vel);
}
}
break;
case MAVLINK_MSG_ID_GPS_STATUS:
{
// TODO: not supported by APM:Plane,
// no standard ROS messages
mavlink_gps_status_t gps_stat;
mavlink_msg_gps_status_decode(msg, &gps_stat);
ROS_DEBUG_NAMED("gps", "GPS stat sat visible: %d", gps_stat.satellites_visible);
}
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
{
mavlink_system_time_t mtime;
mavlink_msg_system_time_decode(msg, &mtime);
if (mtime.time_unix_usec == 0) {
ROS_WARN_THROTTLE_NAMED(60, "gps", "Wrong system time. Is GPS Ok? (boot_ms: %u)",
mtime.time_boot_ms);
return;
}
sensor_msgs::TimeReferencePtr time(new sensor_msgs::TimeReference);
ros::Time time_ref(
mtime.time_unix_usec / 1000000, // t_sec
(mtime.time_unix_usec % 1000000) * 1000); // t_nsec
time->source = time_ref_source;
time->time_ref = time_ref;
time->header.frame_id = time_ref_source;
time->header.stamp = ros::Time::now();
time_ref_pub.publish(time);
}
break;
};
}
