rc_io.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/RCIn.h>
20 #include <mavros_msgs/RCOut.h>
21 #include <mavros_msgs/OverrideRCIn.h>
22 
23 namespace mavros {
24 namespace std_plugins {
29 public:
31  rc_nh("~rc"),
32  raw_rc_in(0),
33  raw_rc_out(0),
34  has_rc_channels_msg(false)
35  { }
36 
37  void initialize(UAS &uas_) override
38  {
39  PluginBase::initialize(uas_);
40 
41  rc_in_pub = rc_nh.advertise<mavros_msgs::RCIn>("in", 10);
42  rc_out_pub = rc_nh.advertise<mavros_msgs::RCOut>("out", 10);
43  override_sub = rc_nh.subscribe("override", 10, &RCIOPlugin::override_cb, this);
44 
46  };
47 
49  return {
53  };
54  }
55 
56 private:
57  using lock_guard = std::lock_guard<std::mutex>;
60 
61  std::vector<uint16_t> raw_rc_in;
62  std::vector<uint16_t> raw_rc_out;
63  std::atomic<bool> has_rc_channels_msg;
64 
68 
69  /* -*- rx handlers -*- */
70 
71  void handle_rc_channels_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS_RAW &port)
72  {
73  /* if we receive RC_CHANNELS, drop RC_CHANNELS_RAW */
74  if (has_rc_channels_msg)
75  return;
76 
77  lock_guard lock(mutex);
78 
79  size_t offset = port.port * 8;
80  if (raw_rc_in.size() < offset + 8)
81  raw_rc_in.resize(offset + 8);
82 
83  // [[[cog:
84  // import cog
85  // for i in range(1, 9):
86  // cog.outl("raw_rc_in[offset + %d] = port.chan%d_raw;" % (i - 1, i))
87  // ]]]
88  raw_rc_in[offset + 0] = port.chan1_raw;
89  raw_rc_in[offset + 1] = port.chan2_raw;
90  raw_rc_in[offset + 2] = port.chan3_raw;
91  raw_rc_in[offset + 3] = port.chan4_raw;
92  raw_rc_in[offset + 4] = port.chan5_raw;
93  raw_rc_in[offset + 5] = port.chan6_raw;
94  raw_rc_in[offset + 6] = port.chan7_raw;
95  raw_rc_in[offset + 7] = port.chan8_raw;
96  // [[[end]]] (checksum: fcb14b1ddfff9ce7dd02f5bd03825cff)
97 
98  auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
99 
100  rcin_msg->header.stamp = m_uas->synchronise_stamp(port.time_boot_ms);
101  rcin_msg->rssi = port.rssi;
102  rcin_msg->channels = raw_rc_in;
103 
104  rc_in_pub.publish(rcin_msg);
105  }
106 
107  void handle_rc_channels(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS &channels)
108  {
109  constexpr size_t MAX_CHANCNT = 18;
110  lock_guard lock(mutex);
111 
112  ROS_INFO_COND_NAMED(!has_rc_channels_msg, "rc", "RC_CHANNELS message detected!");
113  has_rc_channels_msg = true;
114 
115  if (channels.chancount > MAX_CHANCNT) {
116  ROS_WARN_THROTTLE_NAMED(60, "rc",
117  "FCU receives %u RC channels, but RC_CHANNELS can store %zu",
118  channels.chancount, MAX_CHANCNT);
119 
120  channels.chancount = MAX_CHANCNT;
121  }
122 
123  raw_rc_in.resize(channels.chancount);
124 
125  // switch works as start point selector.
126  switch (channels.chancount) {
127  // [[[cog:
128  // for i in range(18, 0, -1):
129  // cog.outl("case %2d: raw_rc_in[%2d] = channels.chan%d_raw;" % (i, i - 1, i))
130  // ]]]
131  case 18: raw_rc_in[17] = channels.chan18_raw;
132  case 17: raw_rc_in[16] = channels.chan17_raw;
133  case 16: raw_rc_in[15] = channels.chan16_raw;
134  case 15: raw_rc_in[14] = channels.chan15_raw;
135  case 14: raw_rc_in[13] = channels.chan14_raw;
136  case 13: raw_rc_in[12] = channels.chan13_raw;
137  case 12: raw_rc_in[11] = channels.chan12_raw;
138  case 11: raw_rc_in[10] = channels.chan11_raw;
139  case 10: raw_rc_in[ 9] = channels.chan10_raw;
140  case 9: raw_rc_in[ 8] = channels.chan9_raw;
141  case 8: raw_rc_in[ 7] = channels.chan8_raw;
142  case 7: raw_rc_in[ 6] = channels.chan7_raw;
143  case 6: raw_rc_in[ 5] = channels.chan6_raw;
144  case 5: raw_rc_in[ 4] = channels.chan5_raw;
145  case 4: raw_rc_in[ 3] = channels.chan4_raw;
146  case 3: raw_rc_in[ 2] = channels.chan3_raw;
147  case 2: raw_rc_in[ 1] = channels.chan2_raw;
148  case 1: raw_rc_in[ 0] = channels.chan1_raw;
149  // [[[end]]] (checksum: 56e9ab5407bd2c864abde230a6cf3fed)
150  case 0: break;
151  }
152 
153  auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
154 
155  rcin_msg->header.stamp = m_uas->synchronise_stamp(channels.time_boot_ms);
156  rcin_msg->rssi = channels.rssi;
157  rcin_msg->channels = raw_rc_in;
158 
159  rc_in_pub.publish(rcin_msg);
160  }
161 
162  void handle_servo_output_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SERVO_OUTPUT_RAW &port)
163  {
164  lock_guard lock(mutex);
165 
166  uint8_t num_channels;
167 
168  // If using Mavlink protocol v2, number of available servo channels is 16;
169  // otherwise, 8
170  if (msg->magic == MAVLINK_STX)
171  num_channels = 16;
172  else
173  num_channels = 8;
174 
175  size_t offset = port.port * num_channels;
176  if (raw_rc_out.size() < offset + num_channels)
177  raw_rc_out.resize(offset + num_channels);
178 
179  // [[[cog:
180  // for i in range(1, 9):
181  // cog.outl("raw_rc_out[offset + %d] = port.servo%d_raw;" % (i - 1, i))
182  // ]]]
183  raw_rc_out[offset + 0] = port.servo1_raw;
184  raw_rc_out[offset + 1] = port.servo2_raw;
185  raw_rc_out[offset + 2] = port.servo3_raw;
186  raw_rc_out[offset + 3] = port.servo4_raw;
187  raw_rc_out[offset + 4] = port.servo5_raw;
188  raw_rc_out[offset + 5] = port.servo6_raw;
189  raw_rc_out[offset + 6] = port.servo7_raw;
190  raw_rc_out[offset + 7] = port.servo8_raw;
191  // [[[end]]] (checksum: 946d524fe9fbaa3e52fbdf8a905fbf0f)
192  if (msg->magic == MAVLINK_STX) {
193  // [[[cog:
194  // for i in range(9, 17):
195  // cog.outl("raw_rc_out[offset + %d] = port.servo%d_raw;" % (i - 1, i))
196  // ]]]
197  raw_rc_out[offset + 8] = port.servo9_raw;
198  raw_rc_out[offset + 9] = port.servo10_raw;
199  raw_rc_out[offset + 10] = port.servo11_raw;
200  raw_rc_out[offset + 11] = port.servo12_raw;
201  raw_rc_out[offset + 12] = port.servo13_raw;
202  raw_rc_out[offset + 13] = port.servo14_raw;
203  raw_rc_out[offset + 14] = port.servo15_raw;
204  raw_rc_out[offset + 15] = port.servo16_raw;
205  // [[[end]]] (checksum: 60a386cba6faa126ee7dfe1b22f50398)
206  }
207 
208  auto rcout_msg = boost::make_shared<mavros_msgs::RCOut>();
209 
210  // XXX: Why time_usec is 32 bit? We should test that.
211  uint64_t time_usec = port.time_usec;
212 
213  rcout_msg->header.stamp = m_uas->synchronise_stamp(time_usec);
214  rcout_msg->channels = raw_rc_out;
215 
216  rc_out_pub.publish(rcout_msg);
217  }
218 
219  /* -*- callbacks -*- */
220 
221  void connection_cb(bool connected) override
222  {
223  lock_guard lock(mutex);
224  raw_rc_in.clear();
225  raw_rc_out.clear();
226  has_rc_channels_msg = false;
227  }
228 
230  {
231  if (!m_uas->is_ardupilotmega() && !m_uas->is_px4())
232  ROS_WARN_THROTTLE_NAMED(30, "rc", "RC override not supported by this FCU!");
233 
234  mavlink::common::msg::RC_CHANNELS_OVERRIDE ovr = {};
235  ovr.target_system = m_uas->get_tgt_system();
236  ovr.target_component = m_uas->get_tgt_component();
237 
238  // [[[cog:
239  // for i in range(1, 9):
240  // cog.outl("ovr.chan%d_raw = req->channels[%d];" % (i, i - 1))
241  // ]]]
242  ovr.chan1_raw = req->channels[0];
243  ovr.chan2_raw = req->channels[1];
244  ovr.chan3_raw = req->channels[2];
245  ovr.chan4_raw = req->channels[3];
246  ovr.chan5_raw = req->channels[4];
247  ovr.chan6_raw = req->channels[5];
248  ovr.chan7_raw = req->channels[6];
249  ovr.chan8_raw = req->channels[7];
250  // [[[end]]] (checksum: bd27f3e85f5ab614ce1332ae3f4c6ebd)
251 
252  UAS_FCU(m_uas)->send_message_ignore_drop(ovr);
253  }
254 };
255 } // namespace std_plugins
256 } // namespace mavros
257 
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std::shared_ptr< MAVConnInterface const > ConstPtr
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
#define ROS_INFO_COND_NAMED(cond, name,...)
std::vector< uint16_t > raw_rc_in
Definition: rc_io.cpp:61
void initialize(UAS &uas_) override
Plugin initializer.
Definition: rc_io.cpp:37
void publish(const boost::shared_ptr< M > &message) const
std::atomic< bool > has_rc_channels_msg
Definition: rc_io.cpp:63
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: rc_io.cpp:48
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
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
ros::Publisher rc_in_pub
Definition: rc_io.cpp:65
void connection_cb(bool connected) override
Definition: rc_io.cpp:221
void override_cb(const mavros_msgs::OverrideRCIn::ConstPtr req)
Definition: rc_io.cpp:229
void handle_rc_channels_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS_RAW &port)
Definition: rc_io.cpp:71
void handle_rc_channels(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS &channels)
Definition: rc_io.cpp:107
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
uint8_t get_tgt_system()
Return communication target system.
Definition: mavros_uas.h:159
void handle_servo_output_raw(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SERVO_OUTPUT_RAW &port)
Definition: rc_io.cpp:162
UAS for plugins.
Definition: mavros_uas.h:67
ros::Subscriber override_sub
Definition: rc_io.cpp:67
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
bool is_ardupilotmega()
Check that FCU is APM.
Definition: mavros_uas.h:410
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Compute FCU message time from time_boot_ms or time_usec field.
ros::Publisher rc_out_pub
Definition: rc_io.cpp:66
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::lock_guard< std::mutex > lock_guard
Definition: rc_io.cpp:57
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
std::vector< uint16_t > raw_rc_out
Definition: rc_io.cpp:62
bool is_px4()
Check that FCU is PX4.
Definition: mavros_uas.h:417
uint8_t get_tgt_component()
Return communication target component.
Definition: mavros_uas.h:166
std::recursive_mutex mutex


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