PX4 Firmware
PX4 Autopilot Software http://px4.io
lps25h.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 lps25h.cpp
36  *
37  * Driver for the LPS25H barometer connected via I2C or SPI.
38  */
39 
40 #include "lps25h.h"
41 
42 /*
43  * LPS25H internal constants and data structures.
44  */
45 
46 /* Max measurement rate is 25Hz */
47 #define LPS25H_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */
48 
49 #define ADDR_REF_P_XL 0x08
50 #define ADDR_REF_P_L 0x09
51 #define ADDR_REF_P_H 0x0A
52 #define ADDR_WHO_AM_I 0x0F
53 #define ADDR_RES_CONF 0x10
54 #define ADDR_CTRL_REG1 0x20
55 #define ADDR_CTRL_REG2 0x21
56 #define ADDR_CTRL_REG3 0x22
57 #define ADDR_CTRL_REG4 0x23
58 #define ADDR_INT_CFG 0x24
59 #define ADDR_INT_SOURCE 0x25
60 
61 #define ADDR_STATUS_REG 0x27
62 #define ADDR_P_OUT_XL 0x28
63 #define ADDR_P_OUT_L 0x29
64 #define ADDR_P_OUT_H 0x2A
65 #define ADDR_TEMP_OUT_L 0x2B
66 #define ADDR_TEMP_OUT_H 0x2C
67 
68 #define ADDR_FIFO_CTRL 0x2E
69 #define ADDR_FIFO_STATUS 0x2F
70 #define ADDR_THS_P_L 0x30
71 #define ADDR_THS_P_H 0x31
72 
73 #define ADDR_RPDS_L 0x39
74 #define ADDR_RPDS_H 0x3A
75 
76 /* Data sheet is ambigious if AVGT or AVGP is first */
77 #define RES_CONF_AVGT_8 0x00
78 #define RES_CONF_AVGT_32 0x01
79 #define RES_CONF_AVGT_128 0x02
80 #define RES_CONF_AVGT_512 0x03
81 #define RES_CONF_AVGP_8 0x00
82 #define RES_CONF_AVGP_32 0x04
83 #define RES_CONF_AVGP_128 0x08
84 #define RES_CONF_AVGP_512 0x0C
85 
86 #define CTRL_REG1_SIM (1 << 0)
87 #define CTRL_REG1_RESET_AZ (1 << 1)
88 #define CTRL_REG1_BDU (1 << 2)
89 #define CTRL_REG1_DIFF_EN (1 << 3)
90 #define CTRL_REG1_PD (1 << 7)
91 #define CTRL_REG1_ODR_SINGLE (0 << 4)
92 #define CTRL_REG1_ODR_1HZ (1 << 4)
93 #define CTRL_REG1_ODR_7HZ (2 << 4)
94 #define CTRL_REG1_ODR_12HZ5 (3 << 4)
95 #define CTRL_REG1_ODR_25HZ (4 << 4)
96 
97 #define CTRL_REG2_ONE_SHOT (1 << 0)
98 #define CTRL_REG2_AUTO_ZERO (1 << 1)
99 #define CTRL_REG2_SWRESET (1 << 2)
100 #define CTRL_REG2_FIFO_MEAN_DEC (1 << 4)
101 #define CTRL_REG2_WTM_EN (1 << 5)
102 #define CTRL_REG2_FIFO_EN (1 << 6)
103 #define CTRL_REG2_BOOT (1 << 7)
104 
105 #define CTRL_REG3_INT1_S_DATA 0x0
106 #define CTRL_REG3_INT1_S_P_HIGH 0x1
107 #define CTRL_REG3_INT1_S_P_LOW 0x2
108 #define CTRL_REG3_INT1_S_P_LIM 0x3
109 #define CTRL_REG3_PP_OD (1 << 6)
110 #define CTRL_REG3_INT_H_L (1 << 7)
111 
112 #define CTRL_REG4_P1_DRDY (1 << 0)
113 #define CTRL_REG4_P1_OVERRUN (1 << 1)
114 #define CTRL_REG4_P1_WTM (1 << 2)
115 #define CTRL_REG4_P1_EMPTY (1 << 3)
116 
117 #define INTERRUPT_CFG_PH_E (1 << 0)
118 #define INTERRUPT_CFG_PL_E (1 << 1)
119 #define INTERRUPT_CFG_LIR (1 << 2)
120 
121 #define INT_SOURCE_PH (1 << 0)
122 #define INT_SOURCE_PL (1 << 1)
123 #define INT_SOURCE_IA (1 << 2)
124 
125 #define STATUS_REG_T_DA (1 << 0)
126 #define STATUS_REG_P_DA (1 << 1)
127 #define STATUS_REG_T_OR (1 << 4)
128 #define STATUS_REG_P_OR (1 << 5)
129 
130 #define FIFO_CTRL_WTM_FMEAN_2 0x01
131 #define FIFO_CTRL_WTM_FMEAN_4 0x03
132 #define FIFO_CTRL_WTM_FMEAN_8 0x07
133 #define FIFO_CTRL_WTM_FMEAN_16 0x0F
134 #define FIFO_CTRL_WTM_FMEAN_32 0x1F
135 #define FIFO_CTRL_F_MODE_BYPASS (0x0 << 5)
136 #define FIFO_CTRL_F_MODE_FIFO (0x1 << 5)
137 #define FIFO_CTRL_F_MODE_STREAM (0x2 << 5)
138 #define FIFO_CTRL_F_MODE_SFIFO (0x3 << 5)
139 #define FIFO_CTRL_F_MODE_BSTRM (0x4 << 5)
140 #define FIFO_CTRL_F_MODE_FMEAN (0x6 << 5)
141 #define FIFO_CTRL_F_MODE_BFIFO (0x7 << 5)
142 
143 #define FIFO_STATUS_EMPTY (1 << 5)
144 #define FIFO_STATUS_FULL (1 << 6)
145 #define FIFO_STATUS_WTM (1 << 7)
146 
152 };
153 
154 class LPS25H : public cdev::CDev, public px4::ScheduledWorkItem
155 {
156 public:
157  LPS25H(device::Device *interface, const char *path);
158  virtual ~LPS25H();
159 
160  virtual int init();
161 
162  virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
163  virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
164 
165  /**
166  * Diagnostics - print some basic information about the driver.
167  */
168  void print_info();
169 
170 protected:
172 
173 private:
174  unsigned _measure_interval{0};
175 
176  ringbuffer::RingBuffer *_reports{nullptr};
177  bool _collect_phase{false};
178 
182 
185 
186  sensor_baro_s _last_report{}; /**< used for info() */
187 
188  /**
189  * Initialise the automatic measurement state machine and start it.
190  *
191  * @note This function is called at open and error time. It might make sense
192  * to make it more aggressive about resetting the bus in case of errors.
193  */
194  void start();
195 
196  /**
197  * Stop the automatic measurement state machine.
198  */
199  void stop();
200 
201  /**
202  * Reset the device
203  */
204  int reset();
205 
206  /**
207  * Perform a poll cycle; collect from the previous measurement
208  * and start a new one.
209  *
210  * This is the heart of the measurement state machine. This function
211  * alternately starts a measurement, or collects the data from the
212  * previous measurement.
213  *
214  * When the interval between measurements is greater than the minimum
215  * measurement interval, a gap is inserted between collection
216  * and measurement to provide the most recent measurement possible
217  * at the next interval.
218  */
219  void Run() override;
220 
221  /**
222  * Write a register.
223  *
224  * @param reg The register to write.
225  * @param val The value to write.
226  * @return OK on write success.
227  */
228  int write_reg(uint8_t reg, uint8_t val);
229 
230  /**
231  * Read a register.
232  *
233  * @param reg The register to read.
234  * @param val The value read.
235  * @return OK on read success.
236  */
237  int read_reg(uint8_t reg, uint8_t &val);
238 
239  /**
240  * Issue a measurement command.
241  *
242  * @return OK if the measurement command was successful.
243  */
244  int measure();
245 
246  /**
247  * Collect the result of the most recent measurement.
248  */
249  int collect();
250 
251  /* this class has pointer data members, do not allow copying it */
252  LPS25H(const LPS25H &);
253  LPS25H operator=(const LPS25H &);
254 };
255 
256 /*
257  * Driver 'main' command.
258  */
259 extern "C" __EXPORT int lps25h_main(int argc, char *argv[]);
260 
261 
262 LPS25H::LPS25H(device::Device *interface, const char *path) :
263  CDev(path),
264  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
265  _interface(interface),
266  _sample_perf(perf_alloc(PC_ELAPSED, "lps25h_read")),
267  _comms_errors(perf_alloc(PC_COUNT, "lps25h_comms_errors"))
268 {
270 }
271 
273 {
274  /* make sure we are truly inactive */
275  stop();
276 
277  if (_class_instance != -1) {
279  }
280 
281  if (_reports != nullptr) {
282  delete _reports;
283  }
284 
285  // free perf counters
288 
289  delete _interface;
290 }
291 
292 int
294 {
295  int ret;
296 
297  ret = CDev::init();
298 
299  if (ret != OK) {
300  PX4_DEBUG("CDev init failed");
301  goto out;
302  }
303 
304  /* allocate basic report buffers */
305  _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));
306 
307  if (_reports == nullptr) {
308  PX4_DEBUG("can't get memory for reports");
309  ret = -ENOMEM;
310  goto out;
311  }
312 
313  if (reset() != OK) {
314  goto out;
315  }
316 
317  /* register alternate interfaces if we have to */
319 
320  ret = OK;
321 
322 out:
323  return ret;
324 }
325 
326 ssize_t
327 LPS25H::read(struct file *filp, char *buffer, size_t buflen)
328 {
329  unsigned count = buflen / sizeof(sensor_baro_s);
330  sensor_baro_s *brp = reinterpret_cast<sensor_baro_s *>(buffer);
331  int ret = 0;
332 
333  /* buffer must be large enough */
334  if (count < 1) {
335  return -ENOSPC;
336  }
337 
338  /* if automatic measurement is enabled */
339  if (_measure_interval > 0) {
340 
341  /*
342  * While there is space in the caller's buffer, and reports, copy them.
343  * Note that we may be pre-empted by the workq thread while we are doing this;
344  * we are careful to avoid racing with them.
345  */
346  while (count--) {
347  if (_reports->get(brp)) {
348  ret += sizeof(*brp);
349  brp++;
350  }
351  }
352 
353  /* if there was no data, warn the caller */
354  return ret ? ret : -EAGAIN;
355  }
356 
357  /* manual measurement - run one conversion */
358  /* XXX really it'd be nice to lock against other readers here */
359  do {
360  _reports->flush();
361 
362  /* trigger a measurement */
363  if (OK != measure()) {
364  ret = -EIO;
365  break;
366  }
367 
368  /* wait for it to complete */
370 
371  /* run the collection phase */
372  if (OK != collect()) {
373  ret = -EIO;
374  break;
375  }
376 
377  if (_reports->get(brp)) {
378  ret = sizeof(sensor_baro_s);
379  }
380  } while (0);
381 
382  return ret;
383 }
384 
385 int
386 LPS25H::ioctl(struct file *filp, int cmd, unsigned long arg)
387 {
388  unsigned dummy = arg;
389 
390  switch (cmd) {
391  case SENSORIOCSPOLLRATE: {
392  switch (arg) {
393 
394  /* zero would be bad */
395  case 0:
396  return -EINVAL;
397 
398  /* set default polling rate */
400  /* do we need to start internal polling? */
401  bool want_start = (_measure_interval == 0);
402 
403  /* set interval for next measurement to minimum legal value */
405 
406  /* if we need to start the poll state machine, do it */
407  if (want_start) {
408  start();
409  }
410 
411  return OK;
412  }
413 
414  /* adjust to a legal polling interval in Hz */
415  default: {
416  /* do we need to start internal polling? */
417  bool want_start = (_measure_interval == 0);
418 
419  /* convert hz to tick interval via microseconds */
420  unsigned interval = (1000000 / arg);
421 
422  /* check against maximum rate */
423  if (interval < (LPS25H_CONVERSION_INTERVAL)) {
424  return -EINVAL;
425  }
426 
427  /* update interval for next measurement */
428  _measure_interval = interval;
429 
430  /* if we need to start the poll state machine, do it */
431  if (want_start) {
432  start();
433  }
434 
435  return OK;
436  }
437  }
438  }
439 
440  case SENSORIOCRESET:
441  return reset();
442 
443  case DEVIOCGDEVICEID:
444  return _interface->ioctl(cmd, dummy);
445 
446  default:
447  /* give it to the superclass */
448  return CDev::ioctl(filp, cmd, arg);
449  }
450 }
451 
452 void
454 {
455  /* reset the report ring and state machine */
456  _collect_phase = false;
457  _reports->flush();
458 
459  /* schedule a cycle to start things */
460  ScheduleNow();
461 }
462 
463 void
465 {
466  ScheduleClear();
467 }
468 
469 int
471 {
472  int ret = 0;
473 
474  // Power on
476 
477  usleep(1000);
478 
479  // Reset
481 
482  usleep(5000);
483 
484  // Power on
486 
487  usleep(1000);
488 
489  return ret;
490 }
491 
492 void
494 {
495  /* collection phase? */
496  if (_collect_phase) {
497 
498  /* perform collection */
499  if (OK != collect()) {
500  PX4_DEBUG("collection error");
501  /* restart the measurement state machine */
502  start();
503  return;
504  }
505 
506  /* next phase is measurement */
507  _collect_phase = false;
508 
509  /*
510  * Is there a collect->measure gap?
511  */
513 
514  /* schedule a fresh cycle call when we are ready to measure again */
516 
517  return;
518  }
519  }
520 
521  /* measurement phase */
522  if (OK != measure()) {
523  PX4_DEBUG("measure error");
524  }
525 
526  /* next phase is collection */
527  _collect_phase = true;
528 
529  /* schedule a fresh cycle call when the measurement is done */
530  ScheduleDelayed(LPS25H_CONVERSION_INTERVAL);
531 }
532 
533 int
535 {
536  int ret;
537 
538  /*
539  * Send the command to begin a 16-bit measurement.
540  */
542 
543  if (OK != ret) {
545  }
546 
547  return ret;
548 }
549 
550 int
552 {
553 #pragma pack(push, 1)
554  struct {
555  uint8_t status;
556  uint8_t p_xl, p_l, p_h;
557  int16_t t;
558  } report;
559 #pragma pack(pop)
560 
561  int ret;
562 
564  sensor_baro_s new_report;
565  bool sensor_is_onboard = false;
566 
567  /* this should be fairly close to the end of the measurement, so the best approximation of the time */
568  new_report.timestamp = hrt_absolute_time();
570 
571  /*
572  * @note We could read the status register 1 here, which could tell us that
573  * we were too early and that the output registers are still being
574  * written. In the common case that would just slow us down, and
575  * we're better off just never being early.
576  */
577 
578  /* get measurements from the device : MSB enables register address auto-increment */
579  ret = _interface->read(ADDR_STATUS_REG | (1 << 7), (uint8_t *)&report, sizeof(report));
580 
581  if (ret != OK) {
584  return ret;
585  }
586 
587  /* get measurements from the device */
588  new_report.temperature = 42.5f + (report.t / 480);
589 
590  /* raw pressure */
591  uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16);
592 
593  /* Pressure and MSL in mBar */
594  float p = raw / 4096.0f;
595 
596  new_report.pressure = p;
597 
598  /* get device ID */
599  new_report.device_id = _interface->get_device_id();
600 
601  if (_baro_topic != nullptr) {
602  /* publish it */
603  orb_publish(ORB_ID(sensor_baro), _baro_topic, &new_report);
604 
605  } else {
606  _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &new_report,
607  &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
608 
609  if (_baro_topic == nullptr) {
610  PX4_DEBUG("ADVERT FAIL");
611  }
612  }
613 
614  _last_report = new_report;
615 
616  /* post a report to the ring */
617  _reports->force(&new_report);
618 
619  /* notify anyone waiting for data */
620  poll_notify(POLLIN);
621 
622 
623  ret = OK;
624 
626  return ret;
627 }
628 
629 int
630 LPS25H::write_reg(uint8_t reg, uint8_t val)
631 {
632  uint8_t buf = val;
633  return _interface->write(reg, &buf, 1);
634 }
635 
636 int
637 LPS25H::read_reg(uint8_t reg, uint8_t &val)
638 {
639  uint8_t buf = val;
640  int ret = _interface->read(reg, &buf, 1);
641  val = buf;
642  return ret;
643 }
644 
645 void
647 {
650  printf("poll interval: %u \n", _measure_interval);
651  print_message(_last_report);
652 
653  _reports->print_info("report queue");
654 }
655 
656 /**
657  * Local functions in support of the shell command.
658  */
659 namespace lps25h
660 {
661 
662 /*
663  list of supported bus configurations
664  */
666  enum LPS25H_BUS busid;
667  const char *devpath;
669  uint8_t busnum;
671 } bus_options[] = {
672  { LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
673 #ifdef PX4_I2C_BUS_EXPANSION1
674  { LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext1", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
675 #endif
676 #ifdef PX4_I2C_BUS_EXPANSION2
677  { LPS25H_BUS_I2C_EXTERNAL, "/dev/lps25h_ext2", &LPS25H_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
678 #endif
679 #ifdef PX4_I2C_BUS_ONBOARD
680  { LPS25H_BUS_I2C_INTERNAL, "/dev/lps25h_int", &LPS25H_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
681 #endif
682 #ifdef PX4_SPIDEV_HMC
683  { LPS25H_BUS_SPI, "/dev/lps25h_spi", &LPS25H_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
684 #endif
685 };
686 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
687 
688 void start(enum LPS25H_BUS busid);
689 bool start_bus(struct lps25h_bus_option &bus);
690 struct lps25h_bus_option &find_bus(enum LPS25H_BUS busid);
691 void test(enum LPS25H_BUS busid);
692 void reset(enum LPS25H_BUS busid);
693 void info();
694 void usage();
695 
696 /**
697  * start driver for a specific bus option
698  */
699 bool
701 {
702  if (bus.dev != nullptr) {
703  errx(1, "bus option already started");
704  }
705 
706  device::Device *interface = bus.interface_constructor(bus.busnum);
707 
708  if (interface->init() != OK) {
709  delete interface;
710  warnx("no device on bus %u", (unsigned)bus.busid);
711  return false;
712  }
713 
714  bus.dev = new LPS25H(interface, bus.devpath);
715 
716  if (bus.dev != nullptr && OK != bus.dev->init()) {
717  delete bus.dev;
718  bus.dev = NULL;
719  return false;
720  }
721 
722  int fd = open(bus.devpath, O_RDONLY);
723 
724  /* set the poll rate to default, starts automatic data collection */
725  if (fd == -1) {
726  errx(1, "can't open baro device");
727  }
728 
730  close(fd);
731  errx(1, "failed setting default poll rate");
732  }
733 
734  close(fd);
735 
736  return true;
737 }
738 
739 
740 /**
741  * Start the driver.
742  *
743  * This function call only returns once the driver
744  * is either successfully up and running or failed to start.
745  */
746 void
747 start(enum LPS25H_BUS busid)
748 {
749  bool started = false;
750 
751  for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
752  if (busid == LPS25H_BUS_ALL && bus_options[i].dev != NULL) {
753  // this device is already started
754  continue;
755  }
756 
757  if (busid != LPS25H_BUS_ALL && bus_options[i].busid != busid) {
758  // not the one that is asked for
759  continue;
760  }
761 
762  started |= start_bus(bus_options[i]);
763  }
764 
765  if (!started) {
766  exit(1);
767  }
768 }
769 
770 /**
771  * find a bus structure for a busid
772  */
774 {
775  for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
776  if ((busid == LPS25H_BUS_ALL ||
777  busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
778  return bus_options[i];
779  }
780  }
781 
782  errx(1, "bus %u not started", (unsigned)busid);
783 }
784 
785 
786 /**
787  * Perform some basic functional tests on the driver;
788  * make sure we can collect data from the sensor in polled
789  * and automatic modes.
790  */
791 void
792 test(enum LPS25H_BUS busid)
793 {
794  struct lps25h_bus_option &bus = find_bus(busid);
795  sensor_baro_s report;
796  ssize_t sz;
797  int ret;
798 
799  int fd;
800 
801  fd = open(bus.devpath, O_RDONLY);
802 
803  if (fd < 0) {
804  err(1, "open failed (try 'lps25h start' if the driver is not running)");
805  }
806 
807  /* do a simple demand read */
808  sz = read(fd, &report, sizeof(report));
809 
810  if (sz != sizeof(report)) {
811  err(1, "immediate read failed");
812  }
813 
814  print_message(report);
815 
816  /* read the sensor 5x and report each value */
817  for (unsigned i = 0; i < 5; i++) {
818  struct pollfd fds;
819 
820  /* wait for data to be ready */
821  fds.fd = fd;
822  fds.events = POLLIN;
823  ret = poll(&fds, 1, 2000);
824 
825  if (ret != 1) {
826  errx(1, "timed out waiting for sensor data");
827  }
828 
829  /* now go get it */
830  sz = read(fd, &report, sizeof(report));
831 
832  if (sz != sizeof(report)) {
833  err(1, "periodic read failed");
834  }
835 
836  print_message(report);
837  }
838 
839  close(fd);
840  errx(0, "PASS");
841 }
842 
843 /**
844  * Reset the driver.
845  */
846 void
847 reset(enum LPS25H_BUS busid)
848 {
849  struct lps25h_bus_option &bus = find_bus(busid);
850  const char *path = bus.devpath;
851 
852  int fd = open(path, O_RDONLY);
853 
854  if (fd < 0) {
855  err(1, "failed ");
856  }
857 
858  if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
859  err(1, "driver reset failed");
860  }
861 
863  err(1, "driver poll restart failed");
864  }
865 
866  exit(0);
867 }
868 
869 /**
870  * Print a little info about the driver.
871  */
872 void
874 {
875  for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
876  struct lps25h_bus_option &bus = bus_options[i];
877 
878  if (bus.dev != nullptr) {
879  warnx("%s", bus.devpath);
880  bus.dev->print_info();
881  }
882  }
883 
884  exit(0);
885 }
886 
887 void
889 {
890  warnx("missing command: try 'start', 'info', 'test', 'reset'");
891  warnx("options:");
892  warnx(" -X (external I2C bus)");
893  warnx(" -I (internal I2C bus)");
894  warnx(" -S (external SPI bus)");
895  warnx(" -s (internal SPI bus)");
896 }
897 
898 } // namespace
899 
900 int
901 lps25h_main(int argc, char *argv[])
902 {
903  enum LPS25H_BUS busid = LPS25H_BUS_ALL;
904 
905  int myoptind = 1;
906  int ch;
907  const char *myoptarg = nullptr;
908 
909  while ((ch = px4_getopt(argc, argv, "XIS:", &myoptind, &myoptarg)) != EOF) {
910  switch (ch) {
911 #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
912 
913  case 'I':
914  busid = LPS25H_BUS_I2C_INTERNAL;
915  break;
916 #endif
917 
918  case 'X':
919  busid = LPS25H_BUS_I2C_EXTERNAL;
920  break;
921 
922  case 'S':
923  busid = LPS25H_BUS_SPI;
924  break;
925 
926  default:
927  lps25h::usage();
928  exit(0);
929  }
930  }
931 
932  if (myoptind >= argc) {
933  lps25h::usage();
934  exit(0);
935  }
936 
937  const char *verb = argv[myoptind];
938 
939  /*
940  * Start/load the driver.
941  */
942  if (!strcmp(verb, "start")) {
943  lps25h::start(busid);
944  }
945 
946  /*
947  * Test the driver/device.
948  */
949  if (!strcmp(verb, "test")) {
950  lps25h::test(busid);
951  }
952 
953  /*
954  * Reset the driver.
955  */
956  if (!strcmp(verb, "reset")) {
957  lps25h::reset(busid);
958  }
959 
960  /*
961  * Print driver information.
962  */
963  if (!strcmp(verb, "info")) {
964  lps25h::info();
965  }
966 
967  errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
968 }
#define CTRL_REG2_BOOT
Definition: lps25h.cpp:103
Shared defines for the lps25h driver.
virtual ~LPS25H()
Definition: lps25h.cpp:272
#define CTRL_REG2_SWRESET
Definition: lps25h.cpp:99
device::Device * LPS25H_I2C_interface(int bus)
Definition: lps25h_i2c.cpp:63
static struct vehicle_status_s status
Definition: Commander.cpp:138
device::Device * _interface
Definition: lps25h.cpp:171
Local functions in support of the shell command.
Definition: lps25h.cpp:659
virtual int open(file_t *filep)
Handle an open of the device.
Definition: CDev.cpp:141
#define BARO_BASE_DEVICE_PATH
Definition: drv_baro.h:50
measure the time elapsed performing an event
Definition: perf_counter.h:56
bool start_bus(struct lps25h_bus_option &bus)
start driver for a specific bus option
Definition: lps25h.cpp:700
device::Device * LPS25H_SPI_interface(int bus)
virtual int register_class_devname(const char *class_devname)
Register a class device name, automatically adding device class instance suffix if need be...
Definition: CDev.cpp:78
#define ADDR_CTRL_REG2
Definition: lps25h.cpp:55
LPS25H_BUS
Definition: lps25h.cpp:147
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
Definition: drv_sensor.h:136
const char * devpath
Definition: lps25h.cpp:667
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
#define CTRL_REG1_PD
Definition: lps25h.cpp:90
Definition: I2C.hpp:51
uint64_t error_count
Definition: sensor_baro.h:54
void stop()
Stop the automatic measurement state machine.
Definition: lps25h.cpp:464
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen)
Definition: lps25h.cpp:327
uint32_t device_id
Definition: sensor_baro.h:55
virtual int ioctl(struct file *filp, int cmd, unsigned long arg)
Definition: lps25h.cpp:386
count the number of times an event occurs
Definition: perf_counter.h:55
#define NUM_BUS_OPTIONS
Definition: lps25h.cpp:686
#define CTRL_REG2_ONE_SHOT
Definition: lps25h.cpp:97
#define LPS25H_CONVERSION_INTERVAL
Definition: lps25h.cpp:47
virtual int close(file_t *filep)
Handle a close of the device.
Definition: CDev.cpp:166
struct lps25h::lps25h_bus_option bus_options[]
void start()
Initialise the automatic measurement state machine and start it.
Definition: lps25h.cpp:453
virtual void poll_notify(pollevent_t events)
Report new poll events.
Definition: CDev.cpp:330
uint32_t get_device_id() const
Definition: Device.hpp:161
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
Definition: drv_sensor.h:134
#define DRV_BARO_DEVTYPE_LPS25H
Definition: drv_sensor.h:91
void print_info()
Diagnostics - print some basic information about the driver.
Definition: lps25h.cpp:646
float temperature
Definition: sensor_baro.h:57
int write_reg(uint8_t reg, uint8_t val)
Write a register.
Definition: lps25h.cpp:630
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
device::Device *(* LPS25H_constructor)(int)
Definition: lps25h.h:64
void perf_count(perf_counter_t handle)
Count a performance event.
float pressure
Definition: sensor_baro.h:56
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.
int read_reg(uint8_t reg, uint8_t &val)
Read a register.
Definition: lps25h.cpp:637
void init()
Activates/configures the hardware registers.
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance)
Register a class device name, automatically adding device class instance suffix if need be...
Definition: CDev.cpp:109
#define perf_alloc(a, b)
Definition: px4io.h:59
__EXPORT int lps25h_main(int argc, char *argv[])
Definition: lps25h.cpp:901
void reset(enum LPS25H_BUS busid)
Reset the driver.
Definition: lps25h.cpp:847
virtual int init()
Definition: lps25h.cpp:293
void start(enum LPS25H_BUS busid)
Start the driver.
Definition: lps25h.cpp:747
#define warnx(...)
Definition: err.h:95
ringbuffer::RingBuffer * _reports
Definition: lps25h.cpp:176
orb_advert_t _baro_topic
Definition: lps25h.cpp:179
sensor_baro_s _last_report
used for info()
Definition: lps25h.cpp:186
void perf_end(perf_counter_t handle)
End a performance event.
void test(enum LPS25H_BUS busid)
Perform some basic functional tests on the driver; make sure we can collect data from the sensor in p...
Definition: lps25h.cpp:792
int reset()
Reset the device.
Definition: lps25h.cpp:470
virtual int write(unsigned address, void *data, unsigned count)
Write directly to the device.
Definition: Device.hpp:115
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
LPS25H operator=(const LPS25H &)
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
virtual int ioctl(unsigned operation, unsigned &arg)
Perform a device-specific operation.
Definition: Device.hpp:124
int _class_instance
Definition: lps25h.cpp:181
#define err(eval,...)
Definition: err.h:83
perf_counter_t _sample_perf
Definition: lps25h.cpp:183
void set_device_type(uint8_t devtype)
Definition: Device.hpp:214
struct lps25h_bus_option & find_bus(enum LPS25H_BUS busid)
find a bus structure for a busid
Definition: lps25h.cpp:773
int collect()
Collect the result of the most recent measurement.
Definition: lps25h.cpp:551
#define ADDR_CTRL_REG1
Definition: lps25h.cpp:54
void info()
Print a little info about the driver.
Definition: lps25h.cpp:873
int _orb_class_instance
Definition: lps25h.cpp:180
#define ADDR_STATUS_REG
Definition: lps25h.cpp:61
struct @83::@85::@87 file
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define SENSORIOCRESET
Reset the sensor to its default configuration.
Definition: drv_sensor.h:141
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
Definition: lps25h.cpp:493
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
#define errx(eval,...)
Definition: err.h:89
Definition: bst.cpp:62
CDev(const char *devname)
Constructor.
Definition: CDev.cpp:50
void usage()
Prints info about the driver argument usage.
Definition: lps25h.cpp:888
#define OK
Definition: uavcan_main.cpp:71
int measure()
Issue a measurement command.
Definition: lps25h.cpp:534
LPS25H_constructor interface_constructor
Definition: lps25h.cpp:668
uint64_t timestamp
Definition: sensor_baro.h:53
LPS25H(device::Device *interface, const char *path)
Definition: lps25h.cpp:262
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
perf_counter_t _comms_errors
Definition: lps25h.cpp:184
unsigned _measure_interval
Definition: lps25h.cpp:174
void perf_begin(perf_counter_t handle)
Begin a performance event.
bool _collect_phase
Definition: lps25h.cpp:177
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
virtual int read(unsigned address, void *data, unsigned count)
Read directly from the device.
Definition: Device.hpp:103
virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
Perform a poll setup/teardown operation.
Definition: CDev.cpp:213