void set(msgT &rst) {
lock_guard lock(mutex);
data_received = true;
#define RST_COPY(field) last_rst.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
}
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);
}
