3dr_radio.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014,2015,2016 Vladimir Ermakov.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
18 
19 #include <mavros_msgs/RadioStatus.h>
20 
21 namespace mavros {
22 namespace std_plugins {
27 public:
29  nh("~"),
30  has_radio_status(false),
31  diag_added(false),
32  low_rssi(0)
33  { }
34 
35  void initialize(UAS &uas_) override
36  {
37  PluginBase::initialize(uas_);
38 
39  nh.param("tdr_radio/low_rssi", low_rssi, 40);
40 
41  status_pub = nh.advertise<mavros_msgs::RadioStatus>("radio_status", 10);
42 
44  }
45 
47  {
48  return {
51  };
52  }
53 
54 private:
56 
58  bool diag_added;
59  int low_rssi;
60 
62 
65 
66  /* -*- message handlers -*- */
67 
68  void handle_radio_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RADIO_STATUS &rst)
69  {
70  has_radio_status = true;
71  handle_message(msg, rst);
72  }
73 
74  void handle_radio(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RADIO &rst)
75  {
76  if (has_radio_status)
77  return;
78 
79  // actually the same data, but from earlier modems
80  handle_message(msg, rst);
81  }
82 
83  template<typename msgT>
84  void handle_message(const mavlink::mavlink_message_t *mmsg, msgT &rst)
85  {
86  if (mmsg->sysid != '3' || mmsg->compid != 'D')
87  ROS_WARN_THROTTLE_NAMED(30, "radio", "RADIO_STATUS not from 3DR modem?");
88 
89  auto msg = boost::make_shared<mavros_msgs::RadioStatus>();
90 
91  msg->header.stamp = ros::Time::now();
92 
93 #define RST_COPY(field) msg->field = rst.field
94  RST_COPY(rssi);
95  RST_COPY(remrssi);
96  RST_COPY(txbuf);
97  RST_COPY(noise);
98  RST_COPY(remnoise);
99  RST_COPY(rxerrors);
100  RST_COPY(fixed);
101 #undef RST_COPY
102 
103  // valid for 3DR modem
104  msg->rssi_dbm = (rst.rssi / 1.9) - 127;
105  msg->remrssi_dbm = (rst.remrssi / 1.9) - 127;
106 
107  // add diag at first event
108  if (!diag_added) {
109  UAS_DIAG(m_uas).add("3DR Radio", this, &TDRRadioPlugin::diag_run);
110  diag_added = true;
111  }
112 
113  // store last status for diag
114  {
115  std::lock_guard<std::mutex> lock(diag_mutex);
116  last_status = msg;
117  }
118 
119  status_pub.publish(msg);
120  }
121 
122 
124  {
125  std::lock_guard<std::mutex> lock(diag_mutex);
126 
127  if (!last_status) {
128  stat.summary(2, "No data");
129  return;
130  }
131  else if (last_status->rssi < low_rssi)
132  stat.summary(1, "Low RSSI");
133  else if (last_status->remrssi < low_rssi)
134  stat.summary(1, "Low remote RSSI");
135  else
136  stat.summary(0, "Normal");
137 
138  stat.addf("RSSI", "%u", last_status->rssi);
139  stat.addf("RSSI (dBm)", "%.1f", last_status->rssi_dbm);
140  stat.addf("Remote RSSI", "%u", last_status->remrssi);
141  stat.addf("Remote RSSI (dBm)", "%.1f", last_status->remrssi_dbm);
142  stat.addf("Tx buffer (%)", "%u", last_status->txbuf);
143  stat.addf("Noice level", "%u", last_status->noise);
144  stat.addf("Remote noice level", "%u", last_status->remnoise);
145  stat.addf("Rx errors", "%u", last_status->rxerrors);
146  stat.addf("Fixed", "%u", last_status->fixed);
147  }
148 
149  void connection_cb(bool connected) override
150  {
151  UAS_DIAG(m_uas).removeByName("3DR Radio");
152  diag_added = false;
153  }
154 
155 };
156 } // namespace std_plugins
157 } // namespace mavros
158 
msg
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: 3dr_radio.cpp:46
void handle_radio_status(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RADIO_STATUS &rst)
Definition: 3dr_radio.cpp:68
void handle_radio(const mavlink::mavlink_message_t *msg, mavlink::ardupilotmega::msg::RADIO &rst)
Definition: 3dr_radio.cpp:74
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
void publish(const boost::shared_ptr< M > &message) const
void summary(unsigned char lvl, const std::string s)
MAVROS Plugin base.
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
Definition: mavros_plugin.h:87
#define UAS_DIAG(uasobjptr)
helper accessor to diagnostic updater
Definition: mavros_uas.h:48
void handle_message(const mavlink::mavlink_message_t *mmsg, msgT &rst)
Definition: 3dr_radio.cpp:84
void addf(const std::string &key, const char *format,...)
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
UAS for plugins.
Definition: mavros_uas.h:67
void initialize(UAS &uas_) override
Plugin initializer.
Definition: 3dr_radio.cpp:35
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
mavros_msgs::RadioStatus::Ptr last_status
Definition: 3dr_radio.cpp:64
void connection_cb(bool connected) override
Definition: 3dr_radio.cpp:149
#define RST_COPY(field)
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
void diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat)
Definition: 3dr_radio.cpp:123
static Time now()
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::recursive_mutex mutex
std::shared_ptr< MAVConnInterface > Ptr


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26