PX4 Firmware
PX4 Autopilot Software http://px4.io
sf1xx.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-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 sf1xx.cpp
36  *
37  * @author ecmnet <ecm@gmx.de>
38  * @author Vasily Evseenko <svpcom@gmail.com>
39  *
40  * Driver for the Lightware SF1xx lidar range finder series.
41  * Default I2C address 0x66 is used.
42  */
43 
44 #include <px4_platform_common/px4_config.h>
45 #include <px4_platform_common/defines.h>
46 #include <px4_platform_common/getopt.h>
47 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
48 #include <px4_platform_common/module.h>
49 
50 #include <drivers/device/i2c.h>
51 
52 #include <sys/types.h>
53 #include <stdint.h>
54 #include <stdlib.h>
55 #include <stdbool.h>
56 #include <semaphore.h>
57 #include <string.h>
58 #include <fcntl.h>
59 #include <poll.h>
60 #include <stdio.h>
61 #include <math.h>
62 #include <unistd.h>
63 
64 #include <lib/parameters/param.h>
65 #include <perf/perf_counter.h>
66 
67 #include <drivers/drv_hrt.h>
70 
71 #include <uORB/uORB.h>
73 
74 #include <board_config.h>
75 
76 /* Configuration Constants */
77 #define SF1XX_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
78 #define SF1XX_BASEADDR 0x66
79 #define SF1XX_DEVICE_PATH "/dev/sf1xx"
80 
81 
82 class SF1XX : public device::I2C, public px4::ScheduledWorkItem
83 {
84 public:
85  SF1XX(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SF1XX_BUS_DEFAULT,
86  int address = SF1XX_BASEADDR);
87 
88  virtual ~SF1XX() override;
89 
90  int init() override;
91 
92  ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override;
93  int ioctl(device::file_t *filp, int cmd, unsigned long arg) override;
94 
95  /**
96  * Diagnostics - print some basic information about the driver.
97  */
98  void print_info();
99 
100 protected:
101  int probe() override;
102 
103 private:
104  /**
105  * Test whether the device supported by the driver is present at a
106  * specific address.
107  *
108  * @param address The I2C bus address to probe.
109  * @return True if the device is present.
110  */
111  int probe_address(uint8_t address);
112 
113  /**
114  * Initialise the automatic measurement state machine and start it.
115  *
116  * @note This function is called at open and error time. It might make sense
117  * to make it more aggressive about resetting the bus in case of errors.
118  */
119  void start();
120 
121  /**
122  * Stop the automatic measurement state machine.
123  */
124  void stop();
125 
126  /**
127  * Set the min and max distance thresholds if you want the end points of the sensors
128  * range to be brought in at all, otherwise it will use the defaults SF1XX_MIN_DISTANCE
129  * and SF1XX_MAX_DISTANCE
130  */
131  void set_minimum_distance(float min);
132  void set_maximum_distance(float max);
133  float get_minimum_distance();
134  float get_maximum_distance();
135 
136  /**
137  * Perform a poll cycle; collect from the previous measurement
138  * and start a new one.
139  */
140  void Run() override;
141  int measure();
142  int collect();
143 
144  bool _sensor_ok{false};
145 
150 
151  float _max_distance{-1.0f};
152  float _min_distance{-1.0f};
153 
154  uint8_t _rotation{0};
155 
156  ringbuffer::RingBuffer *_reports{nullptr};
157 
159 
162 };
163 
164 /*
165  * Driver 'main' command.
166  */
167 extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]);
168 
169 SF1XX::SF1XX(uint8_t rotation, int bus, int address) :
170  I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000),
171  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
172  _rotation(rotation)
173 {
174 }
175 
177 {
178  /* make sure we are truly inactive */
179  stop();
180 
181  /* free any existing reports */
182  if (_reports != nullptr) {
183  delete _reports;
184  }
185 
186  if (_distance_sensor_topic != nullptr) {
188  }
189 
190  if (_class_instance != -1) {
191  unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance);
192  }
193 
194  /* free perf counters */
197 }
198 
199 int
201 {
202  int ret = PX4_ERROR;
203  int hw_model;
204  param_get(param_find("SENS_EN_SF1XX"), &hw_model);
205 
206  switch (hw_model) {
207  case 0:
208  PX4_WARN("disabled.");
209  return ret;
210 
211  case 1: /* SF10/a (25m 32Hz) */
212  _min_distance = 0.01f;
213  _max_distance = 25.0f;
214  _conversion_interval = 31250;
215  break;
216 
217  case 2: /* SF10/b (50m 32Hz) */
218  _min_distance = 0.01f;
219  _max_distance = 50.0f;
220  _conversion_interval = 31250;
221  break;
222 
223  case 3: /* SF10/c (100m 16Hz) */
224  _min_distance = 0.01f;
225  _max_distance = 100.0f;
226  _conversion_interval = 62500;
227  break;
228 
229  case 4:
230  /* SF11/c (120m 20Hz) */
231  _min_distance = 0.01f;
232  _max_distance = 120.0f;
233  _conversion_interval = 50000;
234  break;
235 
236  case 5:
237  /* SF/LW20/b (50m 48-388Hz) */
238  _min_distance = 0.001f;
239  _max_distance = 50.0f;
240  _conversion_interval = 20834;
241  break;
242 
243  case 6:
244  /* SF/LW20/c (100m 48-388Hz) */
245  _min_distance = 0.001f;
246  _max_distance = 100.0f;
247  _conversion_interval = 20834;
248  break;
249 
250  default:
251  PX4_ERR("invalid HW model %d.", hw_model);
252  return ret;
253  }
254 
255  /* do I2C init (and probe) first */
256  if (I2C::init() != OK) {
257  return ret;
258  }
259 
260  /* allocate basic report buffers */
261  _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
262 
263  set_device_address(SF1XX_BASEADDR);
264 
265  if (_reports == nullptr) {
266  return ret;
267  }
268 
269  _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
270 
271  /* get a publish handle on the range finder topic */
272  struct distance_sensor_s ds_report = {};
273 
274  _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
276 
277  if (_distance_sensor_topic == nullptr) {
278  PX4_ERR("failed to create distance_sensor object");
279  }
280 
281  // Select altitude register
282  int ret2 = measure();
283 
284  if (ret2 == 0) {
285  ret = OK;
286  _sensor_ok = true;
287  PX4_INFO("(%dm %dHz) with address %d found", (int)_max_distance,
288  (int)(1e6f / _conversion_interval), SF1XX_BASEADDR);
289  }
290 
291  return ret;
292 }
293 
294 int
296 {
297  return measure();
298 }
299 
300 void
302 {
303  _min_distance = min;
304 }
305 
306 void
308 {
309  _max_distance = max;
310 }
311 
312 float
314 {
315  return _min_distance;
316 }
317 
318 float
320 {
321  return _max_distance;
322 }
323 
324 int
325 SF1XX::ioctl(device::file_t *filp, int cmd, unsigned long arg)
326 {
327  switch (cmd) {
328 
329  case SENSORIOCSPOLLRATE: {
330  switch (arg) {
331 
332  /* zero would be bad */
333  case 0:
334  return -EINVAL;
335 
336  /* set default polling rate */
338  /* do we need to start internal polling? */
339  bool want_start = (_measure_interval == 0);
340 
341  /* set interval for next measurement to minimum legal value */
343 
344  /* if we need to start the poll state machine, do it */
345  if (want_start) {
346  start();
347 
348  }
349 
350  return OK;
351  }
352 
353  /* adjust to a legal polling interval in Hz */
354  default: {
355  /* do we need to start internal polling? */
356  bool want_start = (_measure_interval == 0);
357 
358  /* convert hz to tick interval via microseconds */
359  int interval = (1000000 / arg);
360 
361  /* check against maximum rate */
362  if (interval < _conversion_interval) {
363  return -EINVAL;
364  }
365 
366  /* update interval for next measurement */
367  _measure_interval = interval;
368 
369  /* if we need to start the poll state machine, do it */
370  if (want_start) {
371  start();
372  }
373 
374  return OK;
375  }
376  }
377  }
378 
379  default:
380  /* give it to the superclass */
381  return I2C::ioctl(filp, cmd, arg);
382  }
383 }
384 
385 ssize_t
386 SF1XX::read(device::file_t *filp, char *buffer, size_t buflen)
387 {
388  unsigned count = buflen / sizeof(struct distance_sensor_s);
389  struct distance_sensor_s *rbuf = reinterpret_cast<struct distance_sensor_s *>(buffer);
390  int ret = 0;
391 
392  /* buffer must be large enough */
393  if (count < 1) {
394  return -ENOSPC;
395  }
396 
397  /* if automatic measurement is enabled */
398  if (_measure_interval > 0) {
399 
400  /*
401  * While there is space in the caller's buffer, and reports, copy them.
402  * Note that we may be pre-empted by the workq thread while we are doing this;
403  * we are careful to avoid racing with them.
404  */
405  while (count--) {
406  if (_reports->get(rbuf)) {
407  ret += sizeof(*rbuf);
408  rbuf++;
409  }
410  }
411 
412  /* if there was no data, warn the caller */
413  return ret ? ret : -EAGAIN;
414  }
415 
416  /* manual measurement - run one conversion */
417  do {
418  _reports->flush();
419 
420  /* trigger a measurement */
421  if (OK != measure()) {
422  ret = -EIO;
423  break;
424  }
425 
426  /* wait for it to complete */
427  px4_usleep(_conversion_interval);
428 
429  /* run the collection phase */
430  if (OK != collect()) {
431  ret = -EIO;
432  break;
433  }
434 
435  /* state machine will have generated a report, copy it out */
436  if (_reports->get(rbuf)) {
437  ret = sizeof(*rbuf);
438  }
439 
440  } while (0);
441 
442  return ret;
443 }
444 
445 int
447 {
448  int ret;
449 
450  /*
451  * Send the command '0' -- read altitude
452  */
453 
454  uint8_t cmd = 0;
455  ret = transfer(&cmd, 1, nullptr, 0);
456 
457  if (OK != ret) {
459  PX4_DEBUG("i2c::transfer returned %d", ret);
460  return ret;
461  }
462 
463  ret = OK;
464 
465  return ret;
466 }
467 
468 int
470 {
471  int ret = -EIO;
472 
473  /* read from the sensor */
474  uint8_t val[2] = {0, 0};
476 
477  ret = transfer(nullptr, 0, &val[0], 2);
478 
479  if (ret < 0) {
480  PX4_DEBUG("error reading from sensor: %d", ret);
483  return ret;
484  }
485 
486  uint16_t distance_cm = val[0] << 8 | val[1];
487  float distance_m = float(distance_cm) * 1e-2f;
488 
489  struct distance_sensor_s report;
490  report.timestamp = hrt_absolute_time();
491  report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
492  report.orientation = _rotation;
493  report.current_distance = distance_m;
496  report.variance = 0.0f;
497  report.signal_quality = -1;
498  /* TODO: set proper ID */
499  report.id = 0;
500 
501  /* publish it, if we are the primary */
502  if (_distance_sensor_topic != nullptr) {
503  orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
504  }
505 
506  _reports->force(&report);
507 
508  /* notify anyone waiting for data */
509  poll_notify(POLLIN);
510 
511  ret = OK;
512 
514  return ret;
515 }
516 
517 void
519 {
520  /* reset the report ring and state machine */
521  _reports->flush();
522 
523  /* set register to '0' */
524  measure();
525 
526  /* schedule a cycle to start things */
527  ScheduleDelayed(_conversion_interval);
528 }
529 
530 void
532 {
533  ScheduleClear();
534 }
535 
536 void
538 {
539  /* Collect results */
540  if (OK != collect()) {
541  PX4_DEBUG("collection error");
542  /* if error restart the measurement state machine */
543  start();
544  return;
545  }
546 
547  /* schedule a fresh cycle call when the measurement is done */
548  ScheduleDelayed(_conversion_interval);
549 }
550 
551 void
553 {
556  printf("poll interval: %u\n", _measure_interval);
557  _reports->print_info("report queue");
558 }
559 
560 /**
561  * Local functions in support of the shell command.
562  */
563 namespace sf1xx
564 {
565 
567 
568 int start(uint8_t rotation);
569 int start_bus(uint8_t rotation, int i2c_bus);
570 int stop();
571 int test();
572 int reset();
573 int info();
574 
575 /**
576  *
577  * Attempt to start driver on all available I2C busses.
578  *
579  * This function will return as soon as the first sensor
580  * is detected on one of the available busses or if no
581  * sensors are detected.
582  *
583  */
584 int
585 start(uint8_t rotation)
586 {
587  if (g_dev != nullptr) {
588  PX4_ERR("already started");
589  return PX4_ERROR;
590  }
591 
592  for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
593  if (start_bus(rotation, i2c_bus_options[i]) == PX4_OK) {
594  return PX4_OK;
595  }
596  }
597 
598  return PX4_ERROR;
599 }
600 
601 /**
602  * Start the driver on a specific bus.
603  *
604  * This function only returns if the sensor is up and running
605  * or could not be detected successfully.
606  */
607 int
608 start_bus(uint8_t rotation, int i2c_bus)
609 {
610  int fd = -1;
611 
612  if (g_dev != nullptr) {
613  PX4_ERR("already started");
614  return PX4_ERROR;
615  }
616 
617  /* create the driver */
618  g_dev = new SF1XX(rotation, i2c_bus);
619 
620  if (g_dev == nullptr) {
621  goto fail;
622  }
623 
624  if (OK != g_dev->init()) {
625  goto fail;
626  }
627 
628  /* set the poll rate to default, starts automatic data collection */
629  fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY);
630 
631  if (fd < 0) {
632  goto fail;
633  }
634 
636  px4_close(fd);
637  goto fail;
638  }
639 
640  px4_close(fd);
641  return PX4_OK;
642 
643 fail:
644 
645  if (g_dev != nullptr) {
646  delete g_dev;
647  g_dev = nullptr;
648  }
649 
650  return PX4_ERROR;
651 }
652 
653 /**
654  * Stop the driver
655  */
656 int
658 {
659  if (g_dev != nullptr) {
660  delete g_dev;
661  g_dev = nullptr;
662 
663  } else {
664  PX4_ERR("driver not running");
665  return PX4_ERROR;
666  }
667 
668  return PX4_OK;
669 }
670 
671 /**
672  * Perform some basic functional tests on the driver;
673  * make sure we can collect data from the sensor in polled
674  * and automatic modes.
675  */
676 int
678 {
679  struct distance_sensor_s report;
680  ssize_t sz;
681  int ret;
682 
683  int fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY);
684 
685  if (fd < 0) {
686  PX4_ERR("%s open failed (try 'sf1xx start' if the driver is not running)", SF1XX_DEVICE_PATH);
687  return PX4_ERROR;
688  }
689 
690  /* do a simple demand read */
691  sz = read(fd, &report, sizeof(report));
692 
693  if (sz != sizeof(report)) {
694  PX4_ERR("immediate read failed");
695  return PX4_ERROR;
696  }
697 
698  print_message(report);
699 
700  /* start the sensor polling at 2Hz */
701  if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
702  PX4_ERR("failed to set 2Hz poll rate");
703  return PX4_ERROR;
704  }
705 
706  /* read the sensor 5x and report each value */
707  for (unsigned i = 0; i < 5; i++) {
708  struct pollfd fds;
709 
710  /* wait for data to be ready */
711  fds.fd = fd;
712  fds.events = POLLIN;
713  ret = poll(&fds, 1, 2000);
714 
715  if (ret != 1) {
716  PX4_ERR("timed out waiting for sensor data");
717  return PX4_ERROR;
718  }
719 
720  /* now go get it */
721  sz = read(fd, &report, sizeof(report));
722 
723  if (sz != sizeof(report)) {
724  PX4_ERR("periodic read failed");
725  return PX4_ERROR;
726  }
727 
728  print_message(report);
729  }
730 
731  /* reset the sensor polling to default rate */
733  PX4_ERR("failed to set default poll rate");
734  return PX4_ERROR;
735  }
736 
737  px4_close(fd);
738 
739  PX4_INFO("PASS");
740  return PX4_OK;
741 }
742 
743 /**
744  * Reset the driver.
745  */
746 int
748 {
749  int fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY);
750 
751  if (fd < 0) {
752  PX4_ERR("failed");
753  return PX4_ERROR;
754  }
755 
756  if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
757  PX4_ERR("driver reset failed");
758  return PX4_ERROR;
759  }
760 
762  PX4_ERR("driver poll restart failed");
763  return PX4_ERROR;
764  }
765 
766  px4_close(fd);
767 
768  return PX4_OK;
769 }
770 
771 /**
772  * Print a little info about the driver.
773  */
774 int
776 {
777  if (g_dev == nullptr) {
778  PX4_ERR("driver not running");
779  return PX4_ERROR;
780  }
781 
782  printf("state @ %p\n", g_dev);
783  g_dev->print_info();
784 
785  return PX4_OK;
786 }
787 
788 } /* namespace */
789 
790 
791 static void
793 {
794  PRINT_MODULE_DESCRIPTION(
795  R"DESCR_STR(
796 ### Description
797 
798 I2C bus driver for Lightware SFxx series LIDAR rangefinders: SF10/a, SF10/b, SF10/c, SF11/c, SF/LW20.
799 
800 Setup/usage information: https://docs.px4.io/en/sensor/sfxx_lidar.html
801 
802 ### Examples
803 
804 Attempt to start driver on any bus (start on bus where first sensor found).
805 $ sf1xx start -a
806 Stop driver
807 $ sf1xx stop
808 )DESCR_STR");
809 
810  PRINT_MODULE_USAGE_NAME("sf1xx", "driver");
811  PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
812  PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
813  PRINT_MODULE_USAGE_PARAM_FLAG('a', "Attempt to start driver on all I2C buses", true);
814  PRINT_MODULE_USAGE_PARAM_INT('b', 1, 1, 2000, "Start driver on specific I2C bus", true);
815  PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true);
816  PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
817  PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)");
818  PRINT_MODULE_USAGE_COMMAND_DESCR("reset","Reset driver");
819  PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information");
820 
821 }
822 
823 int
824 sf1xx_main(int argc, char *argv[])
825 {
826  int ch;
827  int myoptind = 1;
828  const char *myoptarg = nullptr;
829  uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
830  bool start_all = false;
831 
832  int i2c_bus = SF1XX_BUS_DEFAULT;
833 
834  while ((ch = px4_getopt(argc, argv, "ab:R:", &myoptind, &myoptarg)) != EOF) {
835  switch (ch) {
836  case 'R':
837  rotation = (uint8_t)atoi(myoptarg);
838  break;
839 
840  case 'b':
841  i2c_bus = atoi(myoptarg);
842  break;
843 
844  case 'a':
845  start_all = true;
846  break;
847 
848  default:
849  PX4_WARN("Unknown option!");
850  goto out_error;
851  }
852  }
853 
854  if (myoptind >= argc) {
855  goto out_error;
856  }
857 
858  /*
859  * Start/load the driver.
860  */
861  if (!strcmp(argv[myoptind], "start")) {
862  if (start_all) {
863  return sf1xx::start(rotation);
864 
865  } else {
866  return sf1xx::start_bus(rotation, i2c_bus);
867  }
868  }
869 
870  /*
871  * Stop the driver
872  */
873  if (!strcmp(argv[myoptind], "stop")) {
874  return sf1xx::stop();
875  }
876 
877  /*
878  * Test the driver/device.
879  */
880  if (!strcmp(argv[myoptind], "test")) {
881  return sf1xx::test();
882  }
883 
884  /*
885  * Reset the driver.
886  */
887  if (!strcmp(argv[myoptind], "reset")) {
888  return sf1xx::reset();
889  }
890 
891  /*
892  * Print driver information.
893  */
894  if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
895  return sf1xx::info();
896  }
897 
898 out_error:
899  sf1xx_usage();
900  return PX4_ERROR;
901 }
ringbuffer::RingBuffer * _reports
Definition: sf1xx.cpp:156
#define RANGE_FINDER_BASE_DEVICE_PATH
#define SF1XX_DEVICE_PATH
Definition: sf1xx.cpp:79
measure the time elapsed performing an event
Definition: perf_counter.h:56
bool _sensor_ok
Definition: sf1xx.cpp:144
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
API for the uORB lightweight object broker.
perf_counter_t _sample_perf
Definition: sf1xx.cpp:160
virtual ~SF1XX() override
Definition: sf1xx.cpp:176
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
Definition: drv_sensor.h:136
Definition: I2C.hpp:51
float get_minimum_distance()
Definition: sf1xx.cpp:313
int collect()
Definition: sf1xx.cpp:469
int reset()
Reset the driver.
Definition: sf1xx.cpp:747
void set_maximum_distance(float max)
Definition: sf1xx.cpp:307
count the number of times an event occurs
Definition: perf_counter.h:55
High-resolution timer with callouts and timekeeping.
ssize_t read(device::file_t *filp, char *buffer, size_t buflen) override
Definition: sf1xx.cpp:386
Global flash based parameter store.
int probe() override
Definition: sf1xx.cpp:295
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
Definition: drv_sensor.h:134
int start(uint8_t rotation)
Attempt to start driver on all available I2C busses.
Definition: sf1xx.cpp:585
int _conversion_interval
Definition: sf1xx.cpp:147
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void perf_count(perf_counter_t handle)
Count a performance event.
int ioctl(device::file_t *filp, int cmd, unsigned long arg) override
Definition: sf1xx.cpp:325
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
int _class_instance
Definition: sf1xx.cpp:146
void init()
Activates/configures the hardware registers.
void set_minimum_distance(float min)
Set the min and max distance thresholds if you want the end points of the sensors range to be brought...
Definition: sf1xx.cpp:301
int init() override
Definition: sf1xx.cpp:200
#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 start_bus(uint8_t rotation, int i2c_bus)
Start the driver on a specific bus.
Definition: sf1xx.cpp:608
#define SF1XX_BUS_DEFAULT
Definition: sf1xx.cpp:77
void start()
Initialise the automatic measurement state machine and start it.
Definition: sf1xx.cpp:518
Definition: sf1xx.cpp:82
int _measure_interval
Definition: sf1xx.cpp:148
int _orb_class_instance
Definition: sf1xx.cpp:149
void perf_end(perf_counter_t handle)
End a performance event.
SF1XX * g_dev
Definition: sf1xx.cpp:566
__EXPORT int sf1xx_main(int argc, char *argv[])
Definition: sf1xx.cpp:824
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
Local functions in support of the shell command.
Definition: sf1xx.cpp:563
int info()
Print a little info about the driver.
Definition: sf1xx.cpp:775
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
static const int i2c_bus_options[]
Definition: i2c.h:46
int stop()
Stop the driver.
Definition: sf1xx.cpp:657
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
void stop()
Stop the automatic measurement state machine.
Definition: sf1xx.cpp:531
uint8_t _rotation
Definition: sf1xx.cpp:154
SF1XX(uint8_t rotation=distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus=SF1XX_BUS_DEFAULT, int address=SF1XX_BASEADDR)
Definition: sf1xx.cpp:169
void print_info()
Diagnostics - print some basic information about the driver.
Definition: sf1xx.cpp:552
int px4_open(const char *path, int flags,...)
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
perf_counter_t _comms_errors
Definition: sf1xx.cpp:161
int measure()
Definition: sf1xx.cpp:446
float get_maximum_distance()
Definition: sf1xx.cpp:319
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
static void sf1xx_usage()
Definition: sf1xx.cpp:792
#define SENSORIOCRESET
Reset the sensor to its default configuration.
Definition: drv_sensor.h:141
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
Definition: bst.cpp:62
float _min_distance
Definition: sf1xx.cpp:152
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
Definition: sf1xx.cpp:537
#define OK
Definition: uavcan_main.cpp:71
float _max_distance
Definition: sf1xx.cpp:151
#define NUM_I2C_BUS_OPTIONS
Definition: i2c.h:61
int probe_address(uint8_t address)
Test whether the device supported by the driver is present at a specific address. ...
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
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).
int test()
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: sf1xx.cpp:677
int px4_close(int fd)
#define SF1XX_BASEADDR
Definition: sf1xx.cpp:78
Performance measuring tools.
Base class for devices connected via I2C.
orb_advert_t _distance_sensor_topic
Definition: sf1xx.cpp:158