PX4 Firmware
PX4 Autopilot Software http://px4.io
ll40ls.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2014-2019 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 ll40ls.cpp
36  * @author Allyson Kreft
37  * @author Johan Jansen <jnsn.johan@gmail.com>
38  * @author Ban Siesta <bansiesta@gmail.com>
39  * @author James Goppert <james.goppert@gmail.com>
40  *
41  * Interface for the PulsedLight Lidar-Lite range finders.
42  */
43 
44 #include <cstdlib>
45 #include <fcntl.h>
46 #include <string.h>
47 #include <stdio.h>
48 #include <systemlib/err.h>
49 
50 #include <board_config.h>
51 #include <drivers/device/i2c.h>
52 #include <px4_platform_common/getopt.h>
53 #include <px4_platform_common/module.h>
54 
55 #include "LidarLiteI2C.h"
56 #include "LidarLitePWM.h"
57 
58 
59 /**
60  * @brief Local functions in support of the shell command.
61  */
62 namespace ll40ls
63 {
64 
65 LidarLite *instance = nullptr;
66 
67 int print_regs();
68 int start(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
69 int start_bus(const int bus = PX4_I2C_BUS_EXPANSION,
70  const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
71 int start_pwm(const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
72 int status();
73 int stop();
74 int usage();
75 
76 /**
77  * @brief Prints register information to the console.
78  */
79 int
81 {
82  if (!instance) {
83  PX4_ERR("No ll40ls driver running");
84  return PX4_ERROR;
85  }
86 
87  instance->print_registers();
88  return PX4_OK;
89 }
90 
91 /**
92  * Attempt to start driver on all available I2C busses.
93  *
94  * This function will return as soon as the first sensor
95  * is detected on one of the available busses or if no
96  * sensors are detected.
97  */
98 int
99 start(const uint8_t rotation)
100 {
101  if (instance != nullptr) {
102  PX4_ERR("already started");
103  return PX4_ERROR;
104  }
105 
106  for (size_t i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
107  if (start_bus(i2c_bus_options[i], rotation) == PX4_OK) {
108  return PX4_OK;
109  }
110  }
111 
112  return PX4_ERROR;
113 }
114 
115 /**
116  * Start the driver on a specific bus.
117  *
118  * This function only returns if the sensor is up and running
119  * or could not be detected successfully.
120  */
121 int
122 start_bus(const int bus, const uint8_t rotation)
123 {
124  if (instance != nullptr) {
125  PX4_ERR("already started");
126  return PX4_OK;
127  }
128 
129  // Instantiate the driver.
130  instance = new LidarLiteI2C(bus, rotation);
131 
132  if (instance == nullptr) {
133  PX4_ERR("Failed to instantiate the driver");
134  delete instance;
135  return PX4_ERROR;
136  }
137 
138  // Initialize the sensor.
139  if (instance->init() != PX4_OK) {
140  PX4_ERR("Failed to initialize LidarLite on bus = %u", bus);
141  delete instance;
142  instance = nullptr;
143  return PX4_ERROR;
144  }
145 
146  // Start the driver.
147  instance->start();
148 
149  PX4_INFO("driver started");
150  return PX4_OK;
151 }
152 
153 /**
154  * Start the pwm driver.
155  *
156  * This function only returns if the sensor is up and running
157  * or could not be detected successfully.
158  */
159 int
160 start_pwm(const uint8_t rotation)
161 {
162  if (instance != nullptr) {
163  PX4_ERR("already started");
164  return PX4_OK;
165  }
166 
167  instance = new LidarLitePWM(rotation);
168 
169  if (instance == nullptr) {
170  PX4_ERR("Failed to instantiate the driver");
171  delete instance;
172  return PX4_ERROR;
173  }
174 
175  // Initialize the sensor.
176  if (instance->init() != PX4_OK) {
177  PX4_ERR("Failed to initialize LidarLite pwm.");
178  delete instance;
179  instance = nullptr;
180  return PX4_ERROR;
181  }
182 
183  // Start the driver.
184  instance->start();
185 
186  PX4_INFO("driver started");
187  return PX4_OK;
188 }
189 
190 /**
191  * @brief Prints status info about the driver.
192  */
193 int
195 {
196  if (instance == nullptr) {
197  PX4_ERR("driver not running");
198  return PX4_ERROR;
199  }
200 
201  instance->print_info();
202  return PX4_OK;
203 }
204 
205 /**
206  * @brief Stops the driver
207  */
208 int
210 {
211  if (instance != nullptr) {
212  delete instance;
213  instance = nullptr;
214  }
215 
216  PX4_INFO("driver stopped");
217  return PX4_OK;
218 }
219 
220 /**
221  * @brief Displays driver usage at the console.
222  */
223 int
225 {
226  PRINT_MODULE_DESCRIPTION(
227  R"DESCR_STR(
228 ### Description
229 
230 I2C bus driver for LidarLite rangefinders.
231 
232 The sensor/driver must be enabled using the parameter SENS_EN_LL40LS.
233 
234 Setup/usage information: https://docs.px4.io/en/sensor/lidar_lite.html
235 
236 ### Examples
237 
238 Start driver on any bus (start on bus where first sensor found).
239 $ ll40ls start i2c -a
240 Start driver on specified bus
241 $ ll40ls start i2c -b 1
242 Stop driver
243 $ ll40ls stop
244 )DESCR_STR");
245 
246  PRINT_MODULE_USAGE_NAME("ll40ls", "driver");
247  PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
248  PRINT_MODULE_USAGE_COMMAND_DESCR("print_regs","Print the register values");
249  PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
250  PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "PWM device");
251  PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "I2C device");
252  PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses (first one found)", true);
253  PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true);
254  PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
255  PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information");
256  PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
257  return PX4_OK;
258 }
259 
260 } // namespace ll40ls
261 
262 
263 /**
264  * @brief Driver 'main' command.
265  */
266 extern "C" __EXPORT int ll40ls_main(int argc, char *argv[])
267 {
268  const char *myoptarg = nullptr;
269 
270  int bus = PX4_I2C_BUS_EXPANSION;
271  int ch = 0;
272  int myoptind = 1;
273 
274  uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
275 
276  bool start_i2c_all = false;
277  bool start_pwm = false;
278 
279  while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
280  switch (ch) {
281  case 'a':
282  start_i2c_all = true;
283  break;
284 
285  case 'b':
286  bus = atoi(myoptarg);
287  break;
288 
289  case 'R':
290  rotation = (uint8_t)atoi(myoptarg);
291  PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
292  break;
293 
294  default:
295  return ll40ls::usage();
296  }
297  }
298 
299  // Determine protocol.
300  if (argc > myoptind + 1) {
301  const char *protocol = argv[myoptind + 1];
302 
303  if (!strcmp(protocol, "i2c")) {
304  PX4_INFO("protocol %s", protocol);
305 
306  } else if (!strcmp(protocol, "pwm")) {
307  PX4_INFO("protocol %s", protocol);
308  start_pwm = true;
309 
310  } else {
311  PX4_INFO("unknown protocol, choose pwm or i2c");
312  return ll40ls::usage();
313  }
314  }
315 
316  if (myoptind >= argc) {
317  return ll40ls::usage();
318  }
319 
320  // Print the sensor register values.
321  if (!strcmp(argv[myoptind], "print_regs")) {
322  return ll40ls::print_regs();
323  }
324 
325  // Start the driver.
326  if (!strcmp(argv[myoptind], "start")) {
327  if (start_i2c_all) {
328  PX4_INFO("starting all i2c busses");
329  return ll40ls::start(rotation);
330 
331  } else if (start_pwm) {
332  PX4_INFO("starting pwm");
333  return ll40ls::start_pwm(rotation);
334 
335  } else {
336  return ll40ls::start_bus(bus, rotation);
337  }
338  }
339 
340  // Print the driver status.
341  if (!strcmp(argv[myoptind], "status")) {
342  return ll40ls::status();
343  }
344 
345  // Stop the driver
346  if (!strcmp(argv[myoptind], "stop")) {
347  return ll40ls::stop();
348  }
349 
350  // Print driver usage information.
351  return ll40ls::usage();
352 }
int stop()
Stops the driver.
Definition: ll40ls.cpp:209
int start_pwm(const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Start the pwm driver.
Definition: ll40ls.cpp:160
Local functions in support of the shell command.
Definition: ll40ls.cpp:62
Definition: I2C.hpp:51
virtual void start()=0
int usage()
Displays driver usage at the console.
Definition: ll40ls.cpp:224
LidarLite * instance
Definition: ll40ls.cpp:65
int status()
Prints status info about the driver.
Definition: ll40ls.cpp:194
int start_bus(const int bus=PX4_I2C_BUS_EXPANSION, const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Start the driver on a specific bus.
Definition: ll40ls.cpp:122
virtual int init()=0
virtual void print_registers()
print registers to console.
Definition: LidarLite.h:78
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
static const int i2c_bus_options[]
Definition: i2c.h:46
int print_regs()
Prints register information to the console.
Definition: ll40ls.cpp:80
__EXPORT int ll40ls_main(int argc, char *argv[])
Driver &#39;main&#39; command.
Definition: ll40ls.cpp:266
void print_info()
Diagnostics - print some basic information about the driver.
Definition: LidarLite.cpp:61
#define NUM_I2C_BUS_OPTIONS
Definition: i2c.h:61
int start(const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Attempt to start driver on all available I2C busses.
Definition: ll40ls.cpp:99
Base class for devices connected via I2C.