PX4 Firmware
PX4 Autopilot Software http://px4.io
listener_main.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file listener_main.cpp
36  *
37  * Tool for listening to topics.
38  */
39 
40 #include <px4_platform_common/module.h>
41 #include <px4_platform_common/getopt.h>
42 
43 #include <poll.h>
44 
45 #include "topic_listener.hpp"
47 
48 // Amount of time to wait when listening for a message, before giving up.
49 static constexpr float MESSAGE_TIMEOUT_S = 2.0f;
50 
51 extern "C" __EXPORT int listener_main(int argc, char *argv[]);
52 
53 static void usage();
54 
55 void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance,
56  unsigned topic_interval)
57 {
58 
59  if (topic_instance == -1 && num_msgs == 1) {
60  // first count the number of instances
61  int instances = 0;
62 
63  for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
64  if (orb_exists(id, i) == PX4_OK) {
65  instances++;
66  }
67  }
68 
69  if (instances == 1) {
70  PX4_INFO_RAW("\nTOPIC: %s\n", id->o_name);
71  int sub = orb_subscribe(id);
72  cb(id, sub);
73  orb_unsubscribe(sub);
74 
75  } else if (instances > 1) {
76  PX4_INFO_RAW("\nTOPIC: %s %d instances\n", id->o_name, instances);
77 
78  for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
79  if (orb_exists(id, i) == PX4_OK) {
80  PX4_INFO_RAW("\nInstance %d:\n", i);
81  int sub = orb_subscribe_multi(id, i);
82  cb(id, sub);
83  orb_unsubscribe(sub);
84  }
85  }
86  }
87 
88  if (instances == 0) {
89  PX4_INFO_RAW("never published\n");
90  }
91 
92  } else {
93  // default to the first instance if not specified
94  if (topic_instance == -1) {
95  topic_instance = 0;
96  }
97 
98  if (orb_exists(id, topic_instance) != 0) {
99  PX4_INFO_RAW("never published\n");
100  return;
101  }
102 
103  int sub = orb_subscribe_multi(id, topic_instance);
104  orb_set_interval(sub, topic_interval);
105 
106  unsigned msgs_received = 0;
107 
108  struct pollfd fds[2] {};
109  // Poll for user input (for q or escape)
110  fds[0].fd = 0; /* stdin */
111  fds[0].events = POLLIN;
112  // Poll the UOrb subscription
113  fds[1].fd = sub;
114  fds[1].events = POLLIN;
115 
116  while (msgs_received < num_msgs) {
117 
118  if (poll(&fds[0], 2, int(MESSAGE_TIMEOUT_S * 1000)) > 0) {
119 
120  // Received character from stdin
121  if (fds[0].revents & POLLIN) {
122  char c = 0;
123  int ret = read(0, &c, 1);
124 
125  if (ret) {
126  return;
127  }
128 
129  switch (c) {
130  case 0x03: // ctrl-c
131  case 0x1b: // esc
132  case 'q':
133  return;
134  /* not reached */
135  }
136  }
137 
138  // Received message from subscription
139  if (fds[1].revents & POLLIN) {
140  msgs_received++;
141 
142  PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received);
143 
144  int ret = cb(id, sub);
145 
146  if (ret != PX4_OK) {
147  PX4_ERR("listener callback failed (%i)", ret);
148  }
149  }
150 
151  } else {
152  PX4_INFO_RAW("Waited for %.1f seconds without a message. Giving up.\n", (double) MESSAGE_TIMEOUT_S);
153  break;
154  }
155  }
156 
157  orb_unsubscribe(sub);
158  }
159 
160 
161 }
162 
163 int listener_main(int argc, char *argv[])
164 {
165  if (argc <= 1) {
166  usage();
167  return 1;
168  }
169 
170  char *topic_name = argv[1];
171 
172  int topic_instance = -1;
173  unsigned topic_rate = 0;
174  unsigned num_msgs = 0;
175 
176  int myoptind = 1;
177  int ch;
178  const char *myoptarg = nullptr;
179 
180  while ((ch = px4_getopt(argc, argv, "i:r:n:", &myoptind, &myoptarg)) != EOF) {
181  switch (ch) {
182 
183  case 'i':
184  topic_instance = strtol(myoptarg, nullptr, 0);
185  break;
186 
187  case 'r':
188  topic_rate = strtol(myoptarg, nullptr, 0);
189  break;
190 
191  case 'n':
192  num_msgs = strtol(myoptarg, nullptr, 0);
193  break;
194 
195  default:
196  usage();
197  return -1;
198  break;
199  }
200  }
201 
202  if (num_msgs == 0) {
203  if (topic_rate != 0) {
204  num_msgs = 30 * topic_rate; // arbitrary limit (30 seconds at max rate)
205 
206  } else {
207  num_msgs = 1;
208  }
209  }
210 
211  listener_generated(topic_name, topic_instance, topic_rate, num_msgs);
212 
213  return 0;
214 }
215 
216 
217 static void
219 {
220  PRINT_MODULE_DESCRIPTION(
221  R"DESCR_STR(
222 Utility to listen on uORB topics and print the data to the console.
223 
224 The listener can be exited any time by pressing Ctrl+C, Esc, or Q.
225 )DESCR_STR");
226 
227  PRINT_MODULE_USAGE_NAME("listener", "command");
228  PRINT_MODULE_USAGE_ARG("<topic_name>", "uORB topic name", false);
229 
230  PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, ORB_MULTI_MAX_INSTANCES - 1, "Topic instance", true);
231  PRINT_MODULE_USAGE_PARAM_INT('n', 1, 0, 100, "Number of messages", true);
232  PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 1000, "Subscription rate (unlimited if 0)", true);
233 }
int(* listener_print_topic_cb)(const orb_id_t &orb_id, int subscription)
int orb_set_interval(int handle, unsigned interval)
Definition: uORB.cpp:126
void listener(listener_print_topic_cb cb, const orb_id_t &id, unsigned num_msgs, int topic_instance, unsigned topic_interval)
Definition: I2C.hpp:51
int orb_exists(const struct orb_metadata *meta, int instance)
Definition: uORB.cpp:105
const char * o_name
unique object name
Definition: uORB.h:51
__EXPORT int listener_main(int argc, char *argv[])
static void read(bootloader_app_shared_t *pshared)
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
static constexpr float MESSAGE_TIMEOUT_S
static void usage()
int orb_unsubscribe(int handle)
Definition: uORB.cpp:85
Object metadata.
Definition: uORB.h:50
void listener_generated(char *topic_name, int topic_instance, int topic_rate, int num_msgs)
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
Definition: uORB.cpp:80
#define ORB_MULTI_MAX_INSTANCES
Maximum number of multi topic instances.
Definition: uORB.h:62