PX4 Firmware
PX4 Autopilot Software http://px4.io
df_trone_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_trone_wrapper.cpp
36  * Driver to access the TROne of the DriverFramework.
37  *
38  * @author Nicolas de Palezieux <ndepal@gmail.com>
39  */
40 
41 #include <px4_platform_common/px4_config.h>
42 
43 #include <sys/types.h>
44 #include <sys/stat.h>
45 #include <stdint.h>
46 #include <stddef.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <math.h>
50 #include <unistd.h>
51 #include <px4_platform_common/getopt.h>
52 #include <errno.h>
53 #include <string>
54 
55 #include <perf/perf_counter.h>
56 #include <systemlib/err.h>
57 
59 #include <drivers/drv_hrt.h>
60 
61 #include <uORB/uORB.h>
63 
64 #include <board_config.h>
65 
66 #include <trone/TROne.hpp>
67 #include <DevMgr.hpp>
68 
69 
70 extern "C" { __EXPORT int df_trone_wrapper_main(int argc, char *argv[]); }
71 
72 using namespace DriverFramework;
73 
74 
75 class DfTROneWrapper : public TROne
76 {
77 public:
78  DfTROneWrapper(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
79  ~DfTROneWrapper();
80 
81 
82  /**
83  * Start automatic measurement.
84  *
85  * @return 0 on success
86  */
87  int start();
88 
89  /**
90  * Stop automatic measurement.
91  *
92  * @return 0 on success
93  */
94  int stop();
95 
96 private:
97  int _publish(struct range_sensor_data &data);
98 
99  uint8_t _rotation;
100 
102 
104 
105  // perf_counter_t _range_sample_perf;
106 
107 };
108 
110  TROne(TRONE_DEVICE_PATH),
111  _rotation(rotation),
112  _range_topic(nullptr),
113  _orb_class_instance(-1)
114 {
115 }
116 
118 {
119 }
120 
122 {
123  int ret;
124 
125  /* Init device and start sensor. */
126  ret = init();
127 
128  if (ret != 0) {
129  PX4_ERR("TROne init fail: %d", ret);
130  return ret;
131  }
132 
133  ret = TROne::start();
134 
135  if (ret != 0) {
136  PX4_ERR("TROne start fail: %d", ret);
137  return ret;
138  }
139 
140  return 0;
141 }
142 
144 {
145  /* Stop sensor. */
146  int ret = TROne::stop();
147 
148  if (ret != 0) {
149  PX4_ERR("TROne stop fail: %d", ret);
150  return ret;
151  }
152 
153  return 0;
154 }
155 
156 int DfTROneWrapper::_publish(struct range_sensor_data &data)
157 {
158  struct distance_sensor_s d;
159 
160  memset(&d, 0, sizeof(d));
161 
163 
164  d.min_distance = float(TRONE_MIN_DISTANCE); /* m */
165 
166  d.max_distance = float(TRONE_MAX_DISTANCE); /* m */
167 
168  d.current_distance = float(data.dist);
169 
170  d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
171 
172  d.id = 0; // TODO set proper ID
173 
175 
176  d.variance = 0.0f;
177 
178  d.signal_quality = -1;
179 
180  if (_range_topic == nullptr) {
181  _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
183 
184  } else {
185  orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
186  }
187 
188  return 0;
189 };
190 
191 
193 {
194 
196 
197 int start(uint8_t rotation);
198 int stop();
199 int info();
200 int probe();
201 void usage();
202 
203 int start(uint8_t rotation)
204 {
205  PX4_ERR("start");
206  g_dev = new DfTROneWrapper(rotation);
207 
208  if (g_dev == nullptr) {
209  PX4_ERR("failed instantiating DfTROneWrapper object");
210  return -1;
211  }
212 
213  int ret = g_dev->start();
214 
215  if (ret != 0) {
216  PX4_ERR("DfTROneWrapper start failed");
217  return ret;
218  }
219 
220  // Open the range sensor
221  DevHandle h;
222  DevMgr::getHandle(TRONE_DEVICE_PATH, h);
223 
224  if (!h.isValid()) {
225  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
226  TRONE_DEVICE_PATH, h.getError());
227  return -1;
228  }
229 
230  DevMgr::releaseHandle(h);
231 
232  return 0;
233 }
234 
235 int stop()
236 {
237  if (g_dev == nullptr) {
238  PX4_ERR("driver not running");
239  return 1;
240  }
241 
242  int ret = g_dev->stop();
243 
244  if (ret != 0) {
245  PX4_ERR("driver could not be stopped");
246  return ret;
247  }
248 
249  delete g_dev;
250  g_dev = nullptr;
251  return 0;
252 }
253 
254 /**
255  * Print a little info about the driver.
256  */
257 int
259 {
260  if (g_dev == nullptr) {
261  PX4_ERR("driver not running");
262  return 1;
263  }
264 
265  PX4_DEBUG("state @ %p", g_dev);
266 
267  return 0;
268 }
269 
270 /**
271  * Who am i
272  */
273 int
275 {
276  int ret;
277 
278  if (g_dev == nullptr) {
279  ret = start(distance_sensor_s::ROTATION_DOWNWARD_FACING);
280 
281  if (ret) {
282  PX4_ERR("Failed to start");
283  return ret;
284  }
285  }
286 
287  ret = g_dev->probe();
288 
289  if (ret) {
290  PX4_ERR("Failed to probe");
291  return ret;
292  }
293 
294  PX4_DEBUG("state @ %p", g_dev);
295 
296  return 0;
297 }
298 
299 
300 void
302 {
303  PX4_WARN("Usage: df_trone_wrapper 'start', 'info', 'stop'");
304 }
305 
306 } // namespace df_trone_wrapper
307 
308 
309 int
310 df_trone_wrapper_main(int argc, char *argv[])
311 {
312  int ch;
313  int ret = 0;
314  int myoptind = 1;
315  const char *myoptarg = NULL;
316  uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
317 
318  /* jump over start/off/etc and look at options first */
319  while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
320  switch (ch) {
321  case 'R':
322  rotation = (uint8_t)atoi(myoptarg);
323  PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
324  break;
325 
326  default:
328  return 0;
329  }
330  }
331 
332  if (argc <= 1) {
334  return 1;
335  }
336 
337  const char *verb = argv[myoptind];
338 
339 
340  if (!strcmp(verb, "start")) {
341  ret = df_trone_wrapper::start(rotation);
342  }
343 
344  else if (!strcmp(verb, "stop")) {
345  ret = df_trone_wrapper::stop();
346  }
347 
348  else if (!strcmp(verb, "info")) {
349  ret = df_trone_wrapper::info();
350  }
351 
352  else if (!strcmp(verb, "probe")) {
353  ret = df_trone_wrapper::probe();
354  }
355 
356  else {
358  return 1;
359  }
360 
361  return ret;
362 }
int stop()
Stop the driver.
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.
int start()
Start automatic measurement.
int probe()
Who am i.
void usage()
Prints info about the driver argument usage.
#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
int _publish(struct range_sensor_data &data)
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
__EXPORT int df_trone_wrapper_main(int argc, char *argv[])
orb_advert_t _range_topic
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
DfTROneWrapper(uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
static int start()
Definition: dataman.cpp:1452
int stop()
Stop automatic measurement.
int info()
Print a little info about the driver.
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
DfTROneWrapper * g_dev
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
int start(uint8_t rotation)
Performance measuring tools.