PX4 Firmware
PX4 Autopilot Software http://px4.io
df_isl29501_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_isl29501_wrapper.cpp
36  * Driver to access the ISL29501 of the DriverFramework.
37  *
38  * @author Zach Lovett <zach.lovett@3drobotics.com>
39  * @author Siddharth B Purohit <sid@3drobotics.com>
40  */
41 
42 #include <px4_platform_common/px4_config.h>
43 
44 #include <sys/types.h>
45 #include <sys/stat.h>
46 #include <stdint.h>
47 #include <stddef.h>
48 #include <stdlib.h>
49 #include <string.h>
50 #include <math.h>
51 #include <unistd.h>
52 #include <px4_platform_common/getopt.h>
53 #include <errno.h>
54 #include <string>
55 
56 #include <perf/perf_counter.h>
57 #include <systemlib/err.h>
58 
60 #include <drivers/drv_hrt.h>
61 
62 #include <uORB/uORB.h>
64 
65 #include <board_config.h>
66 
67 #include <isl29501/isl29501.hpp>
68 #include <DevMgr.hpp>
69 
70 
71 extern "C" { __EXPORT int df_isl29501_wrapper_main(int argc, char *argv[]); }
72 
73 using namespace DriverFramework;
74 
75 
76 class DfISL29501Wrapper : public ISL29501
77 {
78 public:
81 
82 
83  /**
84  * Start automatic measurement.
85  *
86  * @return 0 on success
87  */
88  int start();
89 
90  /**
91  * Stop automatic measurement.
92  *
93  * @return 0 on success
94  */
95  int stop();
96 
97 private:
98  int _publish(struct range_sensor_data &data);
99 
101 
103 
104  // perf_counter_t _range_sample_perf;
105 
106 };
107 
108 DfISL29501Wrapper::DfISL29501Wrapper(/*enum Rotation rotation*/) :
109  ISL29501(ISL_DEVICE_PATH),
110  _range_topic(nullptr),
111  _orb_class_instance(-1)
112 {
113 }
114 
116 {
117 }
118 
120 {
121  int ret;
122  ret = ISL29501::init();
123 
124  if (ret != 0) {
125  PX4_ERR("ISL init fail: %d", ret);
126  return ret;
127  }
128 
129  ret = ISL29501::start();
130 
131  if (ret != 0) {
132  PX4_ERR("ISL start fail: %d", ret);
133  return ret;
134  }
135 
136  return 0;
137 }
138 
140 {
141  /* Stop sensor. */
142  int ret = ISL29501::stop();
143 
144  if (ret != 0) {
145  PX4_ERR("ISL stop fail: %d", ret);
146  return ret;
147  }
148 
149  return 0;
150 }
151 
152 int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
153 {
154  struct distance_sensor_s d;
155 
156  memset(&d, 0, sizeof(d));
157 
159 
160  d.min_distance = float(ISL_MIN_DISTANCE); /* m */
161 
162  d.max_distance = float(ISL_MAX_DISTANCE); /* m */
163 
164  d.current_distance = float(data.dist);
165 
166  d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
167 
168  d.id = 0; // TODO set proper ID
169 
170  d.variance = 0.0f;
171 
172  d.signal_quality = -1;
173 
174  if (_range_topic == nullptr) {
175  _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,
177 
178  } else {
179  orb_publish(ORB_ID(distance_sensor), _range_topic, &d);
180  }
181 
182  return 0;
183 };
184 
185 
187 {
188 
190 
191 int start();
192 int stop();
193 int info();
194 int probe();
195 int calibration();
196 void usage();
197 
198 int start()
199 {
200  PX4_ERR("start");
201  g_dev = new DfISL29501Wrapper();
202 
203  if (g_dev == nullptr) {
204  PX4_ERR("failed instantiating DfISL29501Wrapper object");
205  return -1;
206  }
207 
208  int ret = g_dev->start();
209 
210  if (ret != 0) {
211  PX4_ERR("DfISL29501Wrapper start failed");
212  return ret;
213  }
214 
215  // Open the range sensor
216  DevHandle h;
217  DevMgr::getHandle(ISL_DEVICE_PATH, h);
218 
219  if (!h.isValid()) {
220  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
221  ISL_DEVICE_PATH, h.getError());
222  return -1;
223  }
224 
225  DevMgr::releaseHandle(h);
226 
227  return 0;
228 }
229 
230 int stop()
231 {
232  if (g_dev == nullptr) {
233  PX4_ERR("driver not running");
234  return 1;
235  }
236 
237  int ret = g_dev->stop();
238 
239  if (ret != 0) {
240  PX4_ERR("driver could not be stopped");
241  return ret;
242  }
243 
244  delete g_dev;
245  g_dev = nullptr;
246  return 0;
247 }
248 
249 /**
250  * Print a little info about the driver.
251  */
252 int
254 {
255  if (g_dev == nullptr) {
256  PX4_ERR("driver not running");
257  return 1;
258  }
259 
260  PX4_DEBUG("state @ %p", g_dev);
261 
262  return 0;
263 }
264 
265 /**
266  * Who am i
267  */
268 int
270 {
271  int ret;
272 
273  if (g_dev == nullptr) {
274  ret = start();
275 
276  if (ret) {
277  PX4_ERR("Failed to start");
278  return ret;
279  }
280  }
281 
282  ret = g_dev->probe();
283 
284  if (ret) {
285  PX4_ERR("Failed to probe");
286  return ret;
287  }
288 
289  PX4_DEBUG("state @ %p", g_dev);
290 
291  return 0;
292 }
293 
294 /**
295  * Calibration
296  * runs calibration routine for ISL29501
297  * TODO: implement calibration user interface and parameter system to store calib
298  * Note: Currently only serves debugging purpose, user is required to manually
299  * set offset inside code.
300  */
301 int
303 {
304  int ret;
305 
306  if (g_dev == nullptr) {
307  ret = start();
308 
309  if (ret) {
310  PX4_ERR("Failed to start");
311  return ret;
312  }
313  }
314 
315  ret = g_dev->calibration();
316 
317  if (ret) {
318  PX4_ERR("Failed to calibrate");
319  return ret;
320  }
321 
322  PX4_DEBUG("state @ %p", g_dev);
323 
324  return 0;
325 }
326 
327 
328 void
330 {
331  PX4_WARN("Usage: df_isl_wrapper 'start', 'info', 'stop', 'calib', 'probe'");
332 }
333 
334 } // namespace df_isl_wrapper
335 
336 
337 int
338 df_isl29501_wrapper_main(int argc, char *argv[])
339 {
340  int ret = 0;
341  int myoptind = 1;
342 
343  if (argc <= 1) {
345  return 1;
346  }
347 
348  const char *verb = argv[myoptind];
349 
350 
351  if (!strcmp(verb, "start")) {
352  ret = df_isl29501_wrapper::start(/*rotation*/);
353  }
354 
355  else if (!strcmp(verb, "stop")) {
357  }
358 
359  else if (!strcmp(verb, "info")) {
361  }
362 
363  else if (!strcmp(verb, "probe")) {
365  }
366 
367  else if (!strcmp(verb, "calib")) {
369  return 1;
370  }
371 
372  else {
374  return 1;
375  }
376 
377  return ret;
378 }
API for the uORB lightweight object broker.
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
int _publish(struct range_sensor_data &data)
int stop()
Stop the driver.
static void stop()
Definition: dataman.cpp:1491
High-resolution timer with callouts and timekeeping.
__EXPORT int df_isl29501_wrapper_main(int argc, char *argv[])
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
int calibration()
Calibration runs calibration routine for ISL29501 TODO: implement calibration user interface and para...
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 stop()
Stop automatic measurement.
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
DfISL29501Wrapper * g_dev
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 start()
Start 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
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.