PX4 Firmware
PX4 Autopilot Software http://px4.io
ulanding.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 ulanding.cpp
36  * @author Jessica Stockham <jessica@aerotenna.com>
37  * @author Roman Bapst <roman@uaventure.com>
38  *
39  * Driver for the uLanding radar from Aerotenna
40  */
41 
42 #include <errno.h>
43 #include <fcntl.h>
44 #include <math.h>
45 #include <poll.h>
46 #include <semaphore.h>
47 #include <stdbool.h>
48 #include <stdint.h>
49 #include <stdio.h>
50 #include <stdlib.h>
51 #include <string.h>
52 #include <sys/types.h>
53 #include <termios.h>
54 #include <unistd.h>
55 
56 #include <drivers/device/device.h>
58 #include <drivers/drv_hrt.h>
60 #include <mathlib/mathlib.h>
61 #include <perf/perf_counter.h>
62 #include <px4_platform_common/px4_config.h>
63 #include <px4_platform_common/defines.h>
64 #include <px4_platform_common/getopt.h>
65 #include <px4_platform_common/workqueue.h>
66 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
67 #include <systemlib/err.h>
69 #include <uORB/uORB.h>
70 
71 
72 using namespace time_literals;
73 
74 #define ULANDING_MEASURE_INTERVAL 10_ms
75 #define ULANDING_MAX_DISTANCE 50.0f
76 #define ULANDING_MIN_DISTANCE 0.315f
77 #define ULANDING_VERSION 1
78 
79 #if defined(__PX4_POSIX_OCPOC)
80 #define RADAR_DEFAULT_PORT "/dev/ttyS6" // Default uLanding port on OCPOC.
81 #else
82 #define RADAR_DEFAULT_PORT "/dev/ttyS2" // Default serial port on Pixhawk (TELEM2), baudrate 115200
83 #endif
84 
85 #if ULANDING_VERSION == 1
86 #define ULANDING_PACKET_HDR 254
87 #define ULANDING_BUFFER_LENGTH 18
88 #else
89 #define ULANDING_PACKET_HDR 72
90 #define ULANDING_BUFFER_LENGTH 9
91 #endif
92 
93 /**
94  * Assume standard deviation to be equal to sensor resolution.
95  * Static bench tests have shown that the sensor ouput does
96  * not vary if the unit is not moved.
97  */
98 #define SENS_VARIANCE 0.045f * 0.045f
99 
100 
101 class Radar : public cdev::CDev, public px4::ScheduledWorkItem
102 {
103 public:
104  /**
105  * Default Constructor
106  * @param port The serial port to open for communicating with the sensor.
107  * @param rotation The sensor rotation relative to the vehicle body.
108  */
109  Radar(const char *port = RADAR_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
110 
111  /** Virtual destructor */
112  virtual ~Radar();
113 
114  /**
115  * Method : init()
116  * This method initializes the general driver for a range finder sensor.
117  */
118  virtual int init() override;
119 
120  /**
121  * Diagnostics - print some basic information about the driver.
122  */
123  void print_info();
124 
125 private:
126 
127  /**
128  * Reads data from serial UART and places it into a buffer.
129  */
130  int collect();
131 
132  /**
133  * Opens and configures the UART serial communications port.
134  * @param speed The baudrate (speed) to configure the serial UART port.
135  */
136  int open_serial_port(const speed_t speed = B115200);
137 
138  /**
139  * Perform a reading cycle; collect from the previous measurement
140  * and start a new one.
141  */
142  void Run() override;
143 
144  /**
145  * Initialise the automatic measurement state machine and start it.
146  * @note This function is called at open and error time. It might make sense
147  * to make it more aggressive about resetting the bus in case of errors.
148  */
149  void start();
150 
151  /**
152  * Stops the automatic measurement state machine.
153  */
154  void stop();
155 
156  char _port[20];
157 
158  int _file_descriptor{-1};
159  int _orb_class_instance{-1};
160 
161  unsigned int _head{0};
162  unsigned int _tail{0};
163 
164  uint8_t _buffer[ULANDING_BUFFER_LENGTH] {};
165  uint8_t _rotation;
166 
167  perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "radar_com_err")};
168  perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "radar_read")};
169 
170  orb_advert_t _distance_sensor_topic{nullptr};
171 };
172 
173 Radar::Radar(const char *port, uint8_t rotation) :
175  ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
176  _rotation(rotation)
177 {
178  /* store port name */
179  strncpy(_port, port, sizeof(_port) - 1);
180  /* enforce null termination */
181  _port[sizeof(_port) - 1] = '\0';
182 }
183 
185 {
186  // Ensure we are truly inactive.
187  stop();
188 
191 }
192 
193 int
195 {
197 
198  int bytes_processed = 0;
199  int distance_cm = -1;
200  int index = 0;
201 
202  float distance_m = -1.0f;
203 
204  bool checksum_passed = false;
205 
206  // Read from the sensor UART buffer.
207  int bytes_read = ::read(_file_descriptor, &_buffer[0], sizeof(_buffer));
208 
209  if (bytes_read > 0) {
210  index = bytes_read - 6;
211 
212  while (index >= 0 && !checksum_passed) {
213  if (_buffer[index] == ULANDING_PACKET_HDR) {
214  bytes_processed = index;
215 
216  while (bytes_processed < bytes_read && !checksum_passed) {
217  if (ULANDING_VERSION == 1) {
218  uint8_t checksum_value = (_buffer[index + 1] + _buffer[index + 2] + _buffer[index + 3] + _buffer[index + 4]) & 0xFF;
219  uint8_t checksum_byte = _buffer[index + 5];
220 
221  if (checksum_value == checksum_byte) {
222  checksum_passed = true;
223  distance_cm = (_buffer[index + 3] << 8) | _buffer[index + 2];
224  distance_m = static_cast<float>(distance_cm) / 100.f;
225  }
226 
227  } else {
228  checksum_passed = true;
229  distance_cm = (_buffer[index + 1] & 0x7F);
230  distance_cm += ((_buffer[index + 2] & 0x7F) << 7);
231  distance_m = static_cast<float>(distance_cm) * 0.045f;
232  break;
233  }
234 
235  bytes_processed++;
236  }
237  }
238 
239  index--;
240  }
241  }
242 
243  if (!checksum_passed) {
244  return -EAGAIN;
245  }
246 
248 
249  distance_sensor_s report;
250  report.current_distance = distance_m;
251  report.id = 0; // TODO: set proper ID.
254  report.orientation = _rotation;
255  report.signal_quality = -1;
256  report.timestamp = hrt_absolute_time();
257  report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
258  report.variance = SENS_VARIANCE;
259 
260  // Publish the new report.
261  orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
262 
263  // Notify anyone waiting for data.
264  poll_notify(POLLIN);
266 
267  return PX4_OK;
268 }
269 
270 int
272 {
273  // Intitialize the character device.
274  if (CDev::init() != OK) {
275  return PX4_ERROR;
276  }
277 
278  // Get a publish handle on the range finder topic.
279  distance_sensor_s ds_report = {};
280  _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
282 
283  if (_distance_sensor_topic == nullptr) {
284  PX4_ERR("failed to create distance_sensor object");
285  }
286 
287  start();
288  return PX4_OK;
289 }
290 
291 int
292 Radar::open_serial_port(const speed_t speed)
293 {
294  // File descriptor initialized?
295  if (_file_descriptor > 0) {
296  // PX4_INFO("serial port already open");
297  return PX4_OK;
298  }
299 
300  // Configure port flags for read/write, non-controlling, non-blocking.
301  int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
302 
303  // Open the serial port.
304  _file_descriptor = ::open(_port, flags);
305 
306  if (_file_descriptor < 0) {
307  PX4_ERR("open failed (%i)", errno);
308  return PX4_ERROR;
309  }
310 
311  if (!isatty(_file_descriptor)) {
312  PX4_WARN("not a serial device");
313  return PX4_ERROR;
314  }
315 
316  termios uart_config = {};
317 
318  // Store the current port configuration. attributes.
319  tcgetattr(_file_descriptor, &uart_config);
320 
321  uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
322 
323  // Clear ONLCR flag (which appends a CR for every LF).
324  uart_config.c_oflag &= ~ONLCR;
325 
326  // No parity, one stop bit.
327  uart_config.c_cflag &= ~(CSTOPB | PARENB);
328 
329  // No line processing - echo off, echo newline off, canonical mode off, extended input processing off, signal chars off
330  uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
331 
332  // Set the input baud rate in the uart_config struct.
333  int termios_state = cfsetispeed(&uart_config, speed);
334 
335  if (termios_state < 0) {
336  PX4_ERR("CFG: %d ISPD", termios_state);
338  return PX4_ERROR;
339  }
340 
341  // Set the output baud rate in the uart_config struct.
342  termios_state = cfsetospeed(&uart_config, speed);
343 
344  if (termios_state < 0) {
345  PX4_ERR("CFG: %d OSPD", termios_state);
347  return PX4_ERROR;
348  }
349 
350  // Apply the modified port attributes.
351  termios_state = tcsetattr(_file_descriptor, TCSANOW, &uart_config);
352 
353  if (termios_state < 0) {
354  PX4_ERR("baud %d ATTR", termios_state);
356  return PX4_ERROR;
357  }
358 
359  PX4_INFO("successfully opened UART port %s", _port);
360  return PX4_OK;
361 }
362 
363 void
365 {
368  PX4_INFO("ulanding max distance %.2f m", static_cast<double>(ULANDING_MAX_DISTANCE));
369  PX4_INFO("ulanding min distance %.2f m", static_cast<double>(ULANDING_MIN_DISTANCE));
370  PX4_INFO("ulanding update rate: %.2f Hz", static_cast<double>(ULANDING_MEASURE_INTERVAL));
371 }
372 
373 void
375 {
376  // Ensure the serial port is open.
378 
379  collect();
380 }
381 
382 void
384 {
385  // Schedule the driver at regular intervals.
386  ScheduleOnInterval(ULANDING_MEASURE_INTERVAL, 0);
387  PX4_INFO("driver started");
388 }
389 
390 void
392 {
393  // Ensure the serial port is closed.
395 
396  // Clear the work queue schedule.
397  ScheduleClear();
398 }
399 
400 namespace radar
401 {
403 
404 int reset(const char *port = RADAR_DEFAULT_PORT);
405 int start(const char *port = RADAR_DEFAULT_PORT,
406  const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
407 int status();
408 int stop();
409 int test(const char *port = RADAR_DEFAULT_PORT);
410 int usage();
411 
412 /**
413  * Reset the driver.
414  */
415 int
416 reset(const char *port)
417 {
418  if (stop() == PX4_OK) {
419  return start(port);
420  }
421 
422  return PX4_ERROR;
423 }
424 
425 /**
426  * Start the driver.
427  */
428 int
429 start(const char *port, const uint8_t rotation)
430 {
431  if (g_dev != nullptr) {
432  PX4_INFO("already started");
433  return PX4_OK;
434  }
435 
436  // Instantiate the driver.
437  g_dev = new Radar(port, rotation);
438 
439  if (g_dev == nullptr) {
440  PX4_ERR("object instantiate failed");
441  return PX4_ERROR;
442  }
443 
444  if (g_dev->init() != PX4_OK) {
445  PX4_ERR("driver start failed");
446  delete g_dev;
447  g_dev = nullptr;
448  return PX4_ERROR;
449  }
450 
451  return PX4_OK;
452 }
453 
454 /**
455  * Print the driver status.
456  */
457 int
459 {
460  if (g_dev == nullptr) {
461  PX4_ERR("driver not running");
462  return PX4_ERROR;
463  }
464 
465  printf("state @ %p\n", g_dev);
466  g_dev->print_info();
467 
468  return PX4_OK;
469 }
470 
471 /**
472  * Stop the driver
473  */
474 int stop()
475 {
476  if (g_dev != nullptr) {
477  delete g_dev;
478  g_dev = nullptr;
479  }
480 
481  return PX4_ERROR;
482 }
483 
484 /**
485  * Perform some basic functional tests on the driver;
486  * make sure we can collect data from the sensor in polled
487  * and automatic modes.
488  */
489 int
490 test(const char *port)
491 {
492  int fd = open(port, O_RDONLY);
493 
494  if (fd < 0) {
495  PX4_ERR("%s open failed (try 'radar start' if the driver is not running", port);
496  return PX4_ERROR;
497  }
498 
499  // Perform a simple demand read.
500  distance_sensor_s report;
501  ssize_t bytes_read = read(fd, &report, sizeof(report));
502 
503  if (bytes_read != sizeof(report)) {
504  PX4_ERR("immediate read failed");
505  return PX4_ERROR;
506  }
507 
508  print_message(report);
509  return PX4_OK;
510 }
511 
512 int
514 {
515  PX4_INFO("usage: radar command [options]");
516  PX4_INFO("command:");
517  PX4_INFO("\treset|start|status|stop|test");
518  PX4_INFO("options:");
519  PX4_INFO("\t-R --rotation (%d)", distance_sensor_s::ROTATION_DOWNWARD_FACING);
520  PX4_INFO("\t-d --device_path");
521  return PX4_OK;
522 }
523 
524 } // namespace radar
525 
526 
527 /**
528  * Driver 'main' command.
529  */
530 extern "C" __EXPORT int ulanding_radar_main(int argc, char *argv[])
531 {
532  uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
533  const char *device_path = RADAR_DEFAULT_PORT;
534  int ch;
535  int myoptind = 1;
536  const char *myoptarg = nullptr;
537 
538  while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
539  switch (ch) {
540  case 'R':
541  rotation = (uint8_t)atoi(myoptarg);
542  break;
543 
544  case 'd':
545  device_path = myoptarg;
546  break;
547 
548  default:
549  PX4_WARN("Unknown option!");
550  return radar::usage();
551  }
552  }
553 
554  if (myoptind >= argc) {
555  return radar::usage();
556  }
557 
558  // Reset the driver.
559  if (!strcmp(argv[myoptind], "reset")) {
560  return radar::reset(device_path);
561  }
562 
563  // Start/load the driver.
564  if (!strcmp(argv[myoptind], "start")) {
565  return radar::start(device_path, rotation);
566  }
567 
568  // Print driver information.
569  if (!strcmp(argv[myoptind], "status")) {
570  return radar::status();
571  }
572 
573  // Stop the driver
574  if (!strcmp(argv[myoptind], "stop")) {
575  return radar::stop();
576  }
577 
578  // Test the driver/device.
579  if (!strcmp(argv[myoptind], "test")) {
580  return radar::test();
581  }
582 
583  return radar::usage();
584 }
#define ULANDING_PACKET_HDR
Definition: ulanding.cpp:86
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
void Run() override
Perform a reading cycle; collect from the previous measurement and start a new one.
Definition: ulanding.cpp:374
char _port[20]
Definition: ulanding.cpp:156
#define ULANDING_VERSION
Definition: ulanding.cpp:77
int _file_descriptor
Definition: ulanding.cpp:158
orb_advert_t _distance_sensor_topic
Definition: ulanding.cpp:170
virtual int open(file_t *filep)
Handle an open of the device.
Definition: CDev.cpp:141
measure the time elapsed performing an event
Definition: perf_counter.h:56
__EXPORT int ulanding_radar_main(int argc, char *argv[])
Driver &#39;main&#39; command.
Definition: ulanding.cpp:530
API for the uORB lightweight object broker.
perf_counter_t _sample_perf
Definition: ulanding.cpp:168
Radar(const char *port=RADAR_DEFAULT_PORT, uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Default Constructor.
Definition: ulanding.cpp:173
int test(const char *port=RADAR_DEFAULT_PORT)
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: ulanding.cpp:490
#define ULANDING_BUFFER_LENGTH
Definition: ulanding.cpp:87
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
int usage()
Prints info about the driver argument usage.
Definition: ulanding.cpp:513
static void stop()
Definition: dataman.cpp:1491
virtual int init() override
Method : init() This method initializes the general driver for a range finder sensor.
Definition: ulanding.cpp:271
count the number of times an event occurs
Definition: perf_counter.h:55
virtual ssize_t read(file_t *filep, char *buffer, size_t buflen)
Perform a read from the device.
Definition: CDev.hpp:111
int open_serial_port(const speed_t speed=B115200)
Opens and configures the UART serial communications port.
Definition: ulanding.cpp:292
High-resolution timer with callouts and timekeeping.
virtual int close(file_t *filep)
Handle a close of the device.
Definition: CDev.cpp:166
int _orb_class_instance
Definition: ulanding.cpp:159
virtual void poll_notify(pollevent_t events)
Report new poll events.
Definition: CDev.cpp:330
virtual ~Radar()
Virtual destructor.
Definition: ulanding.cpp:184
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Abstract class for any character device.
Definition: CDev.hpp:58
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
#define perf_alloc(a, b)
Definition: px4io.h:59
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
int stop()
Stop the driver.
Definition: ulanding.cpp:474
#define ULANDING_MIN_DISTANCE
Definition: ulanding.cpp:76
#define ULANDING_MAX_DISTANCE
Definition: ulanding.cpp:75
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
void perf_end(perf_counter_t handle)
End a performance event.
#define SENS_VARIANCE
Assume standard deviation to be equal to sensor resolution.
Definition: ulanding.cpp:98
void start()
Initialise the automatic measurement state machine and start it.
Definition: ulanding.cpp:383
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int fd
Definition: dataman.cpp:146
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
void print_info()
Diagnostics - print some basic information about the driver.
Definition: ulanding.cpp:364
int reset(const char *port=RADAR_DEFAULT_PORT)
Reset the driver.
Definition: ulanding.cpp:416
perf_counter_t _comms_errors
Definition: ulanding.cpp:167
static int start()
Definition: dataman.cpp:1452
int start(const char *port=RADAR_DEFAULT_PORT, const uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING)
Start the driver.
Definition: ulanding.cpp:429
#define RADAR_DEFAULT_PORT
Definition: ulanding.cpp:82
void stop()
Stops the automatic measurement state machine.
Definition: ulanding.cpp:391
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
Radar * g_dev
Definition: ulanding.cpp:402
Definition: bst.cpp:62
#define OK
Definition: uavcan_main.cpp:71
#define ULANDING_MEASURE_INTERVAL
Definition: ulanding.cpp:74
uint8_t _buffer[ULANDING_BUFFER_LENGTH]
Definition: ulanding.cpp:164
int status()
Print the driver status.
Definition: ulanding.cpp:458
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
#define RANGE_FINDER0_DEVICE_PATH
int collect()
Reads data from serial UART and places it into a buffer.
Definition: ulanding.cpp:194
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint8_t _rotation
Definition: ulanding.cpp:165
Performance measuring tools.