PX4 Firmware
PX4 Autopilot Software http://px4.io
df_bebop_rangefinder_wrapper.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 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 df_bebop_rangefinder_wrapper.cpp
36  * Driver to access the Bebop rangefinder of the DriverFramework.
37  */
38 
39 #include <px4_platform_common/px4_config.h>
40 
41 #include <sys/types.h>
42 #include <sys/stat.h>
43 #include <stdint.h>
44 #include <stddef.h>
45 #include <stdlib.h>
46 #include <string.h>
47 #include <math.h>
48 #include <unistd.h>
49 #include <px4_platform_common/getopt.h>
50 #include <errno.h>
51 #include <string>
52 
53 #include <perf/perf_counter.h>
54 #include <systemlib/err.h>
55 
57 #include <drivers/drv_hrt.h>
58 
59 #include <uORB/uORB.h>
61 
62 #include <board_config.h>
63 
64 #include <bebop_rangefinder/BebopRangeFinder.hpp>
65 #include <DevMgr.hpp>
66 
67 
68 extern "C" { __EXPORT int df_bebop_rangefinder_wrapper_main(int argc, char *argv[]); }
69 
70 using namespace DriverFramework;
71 
72 
73 class DfBebopRangeFinderWrapper : public BebopRangeFinder
74 {
75 public:
77  ~DfBebopRangeFinderWrapper() = default;
78 
79 
80  /**
81  * Start automatic measurement.
82  *
83  * @return 0 on success
84  */
85  int start();
86 
87  /**
88  * Stop automatic measurement.
89  *
90  * @return 0 on success
91  */
92  int stop();
93 
94 private:
95  int _publish(struct bebop_range &data);
96 
98 
100 
101  // perf_counter_t _range_sample_perf;
102 
103 };
104 
106  BebopRangeFinder(BEBOP_RANGEFINDER_DEVICE_PATH),
107  _range_topic(nullptr),
108  _orb_class_instance(-1)
109 {
110 }
111 
113 {
114  /* Init device and start sensor. */
115  int ret = init();
116 
117  if (ret != 0) {
118  PX4_ERR("BebopRangeFinder init fail: %d", ret);
119  return ret;
120  }
121 
122  ret = BebopRangeFinder::start();
123 
124  if (ret != 0) {
125  PX4_ERR("BebopRangeFinder start fail: %d", ret);
126  return ret;
127  }
128 
129  return 0;
130 }
131 
133 {
134  /* Stop sensor. */
135  int ret = BebopRangeFinder::stop();
136 
137  if (ret != 0) {
138  PX4_ERR("BebopRangeFinder stop fail: %d", ret);
139  return ret;
140  }
141 
142  return 0;
143 }
144 
146 {
147  struct distance_sensor_s distance_data;
148 
149  memset(&distance_data, 0, sizeof(distance_sensor_s));
150 
151  distance_data.timestamp = hrt_absolute_time();
152  distance_data.min_distance = float(BEBOP_RANGEFINDER_MIN_DISTANCE_M); /* m */
153  distance_data.max_distance = float(BEBOP_RANGEFINDER_MAX_DISTANCE_M); /* m */
154 
155  distance_data.current_distance = float(data.height_m);
156 
157  distance_data.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
158 
159  distance_data.id = 0; // TODO set proper ID
160 
161  distance_data.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
162 
163  distance_data.variance = 1.0f; // TODO set correct value
164 
165  distance_data.signal_quality = -1;
166 
167  if (_range_topic == nullptr) {
168  _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &distance_data,
170 
171  } else {
172  orb_publish(ORB_ID(distance_sensor), _range_topic, &distance_data);
173  }
174 
175  return 0;
176 };
177 
178 
180 {
181 
183 
184 int start();
185 int stop();
186 int info();
187 void usage();
188 
189 int start()
190 {
191  g_dev = new DfBebopRangeFinderWrapper();
192 
193  if (g_dev == nullptr) {
194  PX4_ERR("failed instantiating DfBebopRangeFinderWrapper object");
195  return -1;
196  }
197 
198  int ret = g_dev->start();
199 
200  if (ret != 0) {
201  PX4_ERR("DfBebopRangeFinderWrapper start failed");
202  return ret;
203  }
204 
205  // Open the range sensor
206  DevHandle h;
207  DevMgr::getHandle(BEBOP_RANGEFINDER_DEVICE_PATH, h);
208 
209  if (!h.isValid()) {
210  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
211  BEBOP_RANGEFINDER_DEVICE_PATH, h.getError());
212  return -1;
213  }
214 
215  DevMgr::releaseHandle(h);
216 
217  return 0;
218 }
219 
220 int stop()
221 {
222  if (g_dev == nullptr) {
223  PX4_ERR("driver not running");
224  return 1;
225  }
226 
227  int ret = g_dev->stop();
228 
229  if (ret != 0) {
230  PX4_ERR("driver could not be stopped");
231  return ret;
232  }
233 
234  delete g_dev;
235  g_dev = nullptr;
236  return 0;
237 }
238 
239 /**
240  * Print a little info about the driver.
241  */
242 int
244 {
245  if (g_dev == nullptr) {
246  PX4_ERR("driver not running");
247  return 1;
248  }
249 
250  PX4_DEBUG("state @ %p", g_dev);
251 
252  return 0;
253 }
254 
255 void
257 {
258  PX4_WARN("Usage: df_bebop_rangefinder_wrapper 'start', 'info', 'stop'");
259 }
260 
261 } // namespace df_bebop_rangefinder_wrapper
262 
263 
264 int
265 df_bebop_rangefinder_wrapper_main(int argc, char *argv[])
266 {
267  int ch;
268  int ret = 0;
269  int myoptind = 1;
270  const char *myoptarg = NULL;
271 
272  /* jump over start/off/etc and look at options first */
273  while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
274  switch (ch) {
275  // Add rotation if necessary
276  default:
278  return 0;
279  }
280  }
281 
282  if (argc <= 1) {
284  return 1;
285  }
286 
287  const char *verb = argv[myoptind];
288 
289 
290  if (!strcmp(verb, "start")) {
291  ret = df_bebop_rangefinder_wrapper::start(/*rotation*/);
292  }
293 
294  else if (!strcmp(verb, "stop")) {
296  }
297 
298  else if (!strcmp(verb, "info")) {
300  }
301 
302  else {
304  return 1;
305  }
306 
307  return ret;
308 }
int start()
Start automatic measurement.
API for the uORB lightweight object broker.
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
static void stop()
Definition: dataman.cpp:1491
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void init()
Activates/configures the hardware registers.
uint8_t * data
Definition: dataman.cpp:149
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
int info()
Print a little info about the driver.
int start()
Attempt to start driver on all available I2C busses.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
void usage()
Prints info about the driver argument usage.
static int start()
Definition: dataman.cpp:1452
int _publish(struct bebop_range &data)
int stop()
Stop automatic measurement.
__EXPORT int df_bebop_rangefinder_wrapper_main(int argc, char *argv[])
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.