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.");
}
}
void initialize(UAS &uas_)
{
uas = &uas_;
// general params
gp_nh.param<std::string>("frame_id", frame_id, "fcu");
gp_nh.param("rot_covariance", rot_cov, 99999.0);
// tf subsection
gp_nh.param("tf/send", tf_send, true);
gp_nh.param<std::string>("tf/frame_id", tf_frame_id, "local_origin");
gp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "fcu_utm");
UAS_DIAG(uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run);
// gps data
raw_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("raw/fix", 10);
raw_vel_pub = gp_nh.advertise<geometry_msgs::TwistStamped>("raw/gps_vel", 10);
// fused global position
gp_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("global", 10);
gp_pos_pub = gp_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("local", 10);
gp_vel_pub = gp_nh.advertise<geometry_msgs::TwistStamped>("gp_vel", 10);
gp_rel_alt_pub = gp_nh.advertise<std_msgs::Float64>("rel_alt", 10);
gp_hdg_pub = gp_nh.advertise<std_msgs::Float64>("compass_hdg", 10);
}
void initialize(UAS &uas_)
{
uas = &uas_;
double conn_timeout_d;
double conn_heartbeat_d;
double min_voltage;
nh.param("conn/timeout", conn_timeout_d, 30.0);
nh.param("conn/heartbeat", conn_heartbeat_d, 0.0);
nh.param("sys/min_voltage", min_voltage, 6.0);
nh.param("sys/disable_diag", disable_diag, false);
// heartbeat diag always enabled
UAS_DIAG(uas).add(hb_diag);
if (!disable_diag) {
UAS_DIAG(uas).add(sys_diag);
UAS_DIAG(uas).add(batt_diag);
batt_diag.set_min_voltage(min_voltage);
}
// one-shot timeout timer
timeout_timer = nh.createTimer(ros::Duration(conn_timeout_d),
&SystemStatusPlugin::timeout_cb, this, true);
timeout_timer.start();
if (conn_heartbeat_d > 0.0) {
heartbeat_timer = nh.createTimer(ros::Duration(conn_heartbeat_d),
&SystemStatusPlugin::heartbeat_cb, this);
heartbeat_timer.start();
}
// version request timer
autopilot_version_timer = nh.createTimer(ros::Duration(1.0),
&SystemStatusPlugin::autopilot_version_cb, this);
autopilot_version_timer.stop();
// subscribe to connection event
uas->sig_connection_changed.connect(boost::bind(&SystemStatusPlugin::connection_cb, this, _1));
state_pub = nh.advertise<mavros::State>("state", 10);
batt_pub = nh.advertise<mavros::BatteryStatus>("battery", 10);
rate_srv = nh.advertiseService("set_stream_rate", &SystemStatusPlugin::set_rate_cb, this);
mode_srv = nh.advertiseService("set_mode", &SystemStatusPlugin::set_mode_cb, this);
}
void initialize(UAS &uas_)
{
double conn_system_time_d;
double conn_timesync_d;
ros::Duration conn_system_time;
ros::Duration conn_timesync;
uas = &uas_;
if (nh.getParam("conn/system_time_rate", conn_system_time_d) && conn_system_time_d != 0.0) {
conn_system_time = ros::Duration(ros::Rate(conn_system_time_d));
}
else if (nh.getParam("conn/system_time", conn_system_time_d)) {
// XXX deprecated parameter
ROS_WARN_NAMED("time", "TM: parameter `~conn/system_time` deprecated, "
"please use `~conn/system_time_rate` instead!");
conn_system_time = ros::Duration(conn_system_time_d);
}
if (nh.getParam("conn/timesync_rate", conn_timesync_d) && conn_timesync_d != 0.0) {
conn_timesync = ros::Duration(ros::Rate(conn_timesync_d));
}
else if (nh.getParam("conn/timesync", conn_timesync_d)) {
// XXX deprecated parameter
ROS_WARN_NAMED("time", "TM: parameter `~conn/timesync` deprecated, "
"please use `~conn/timesync_rate` instead!");
conn_timesync = ros::Duration(conn_timesync_d);
}
nh.param<std::string>("time/time_ref_source", time_ref_source, "fcu");
nh.param("time/timesync_avg_alpha", offset_avg_alpha, 0.6);
/*
* alpha for exponential moving average. The closer alpha is to 1.0,
* the faster the moving average updates in response to new offset samples (more jitter)
* We need a significant amount of smoothing , more so for lower message rates like 1Hz
*/
time_ref_pub = nh.advertise<sensor_msgs::TimeReference>("time_reference", 10);
// timer for sending system time messages
if (!conn_system_time.isZero()) {
sys_time_timer = nh.createTimer(conn_system_time,
&SystemTimePlugin::sys_time_cb, this);
sys_time_timer.start();
}
// timer for sending timesync messages
if (!conn_timesync.isZero()) {
// enable timesync diag only if that feature enabled
UAS_DIAG(uas).add(dt_diag);
timesync_timer = nh.createTimer(conn_timesync,
&SystemTimePlugin::timesync_cb, this);
timesync_timer.start();
}
}
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 connection_cb(bool connected) {
UAS_DIAG(uas).removeByName("3DR Radio");
diag_added = false;
}
void initialize(UAS &uas)
{
UAS_DIAG(&uas).add(tdr_diag);
status_pub = nh.advertise<mavros::RadioStatus>("radio_status", 10);
}
