47 #define LPS25H_CONVERSION_INTERVAL (1000000 / 25) 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 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 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 73 #define ADDR_RPDS_L 0x39 74 #define ADDR_RPDS_H 0x3A 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 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) 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) 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) 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) 117 #define INTERRUPT_CFG_PH_E (1 << 0) 118 #define INTERRUPT_CFG_PL_E (1 << 1) 119 #define INTERRUPT_CFG_LIR (1 << 2) 121 #define INT_SOURCE_PH (1 << 0) 122 #define INT_SOURCE_PL (1 << 1) 123 #define INT_SOURCE_IA (1 << 2) 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) 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) 143 #define FIFO_STATUS_EMPTY (1 << 5) 144 #define FIFO_STATUS_FULL (1 << 6) 145 #define FIFO_STATUS_WTM (1 << 7) 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);
237 int read_reg(uint8_t reg, uint8_t &val);
264 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(interface->get_device_id())),
300 PX4_DEBUG(
"CDev init failed");
308 PX4_DEBUG(
"can't get memory for reports");
354 return ret ? ret : -EAGAIN;
388 unsigned dummy = arg;
420 unsigned interval = (1000000 / arg);
443 case DEVIOCGDEVICEID:
448 return CDev::ioctl(filp, cmd, arg);
500 PX4_DEBUG(
"collection error");
523 PX4_DEBUG(
"measure error");
553 #pragma pack(push, 1) 556 uint8_t p_xl, p_l, p_h;
565 bool sensor_is_onboard =
false;
591 uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16);
594 float p = raw / 4096.0f;
610 PX4_DEBUG(
"ADVERT FAIL");
653 _reports->print_info(
"report queue");
673 #ifdef PX4_I2C_BUS_EXPANSION1 676 #ifdef PX4_I2C_BUS_EXPANSION2 679 #ifdef PX4_I2C_BUS_ONBOARD 682 #ifdef PX4_SPIDEV_HMC 686 #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) 689 bool start_bus(
struct lps25h_bus_option &bus);
702 if (bus.
dev !=
nullptr) {
703 errx(1,
"bus option already started");
706 device::Device *
interface = bus.interface_constructor(bus.busnum);
708 if (interface->init() !=
OK) {
710 warnx(
"no device on bus %u", (
unsigned)bus.busid);
714 bus.dev =
new LPS25H(interface, bus.devpath);
716 if (bus.dev !=
nullptr &&
OK != bus.dev->init()) {
722 int fd =
open(bus.devpath, O_RDONLY);
726 errx(1,
"can't open baro device");
731 errx(1,
"failed setting default poll rate");
749 bool started =
false;
782 errx(1,
"bus %u not started", (
unsigned)busid);
804 err(1,
"open failed (try 'lps25h start' if the driver is not running)");
808 sz =
read(fd, &report,
sizeof(report));
810 if (sz !=
sizeof(report)) {
811 err(1,
"immediate read failed");
814 print_message(report);
817 for (
unsigned i = 0; i < 5; i++) {
823 ret =
poll(&fds, 1, 2000);
826 errx(1,
"timed out waiting for sensor data");
830 sz =
read(fd, &report,
sizeof(report));
832 if (sz !=
sizeof(report)) {
833 err(1,
"periodic read failed");
836 print_message(report);
850 const char *path = bus.
devpath;
852 int fd =
open(path, O_RDONLY);
859 err(1,
"driver reset failed");
863 err(1,
"driver poll restart failed");
878 if (bus.
dev !=
nullptr) {
890 warnx(
"missing command: try 'start', 'info', 'test', 'reset'");
892 warnx(
" -X (external I2C bus)");
893 warnx(
" -I (internal I2C bus)");
894 warnx(
" -S (external SPI bus)");
895 warnx(
" -s (internal SPI bus)");
907 const char *myoptarg =
nullptr;
909 while ((ch = px4_getopt(argc, argv,
"XIS:", &myoptind, &myoptarg)) != EOF) {
911 #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) 932 if (myoptind >= argc) {
937 const char *verb = argv[myoptind];
942 if (!strcmp(verb,
"start")) {
949 if (!strcmp(verb,
"test")) {
956 if (!strcmp(verb,
"reset")) {
963 if (!strcmp(verb,
"info")) {
967 errx(1,
"unrecognised command, try 'start', 'test', 'reset' or 'info'");
Shared defines for the lps25h driver.
#define CTRL_REG2_SWRESET
device::Device * LPS25H_I2C_interface(int bus)
static struct vehicle_status_s status
device::Device * _interface
Local functions in support of the shell command.
virtual int open(file_t *filep)
Handle an open of the device.
#define BARO_BASE_DEVICE_PATH
measure the time elapsed performing an event
bool start_bus(struct lps25h_bus_option &bus)
start driver for a specific bus option
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...
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
void usage(const char *reason)
Print the correct usage.
void stop()
Stop the automatic measurement state machine.
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen)
virtual int ioctl(struct file *filp, int cmd, unsigned long arg)
count the number of times an event occurs
#define CTRL_REG2_ONE_SHOT
#define LPS25H_CONVERSION_INTERVAL
virtual int close(file_t *filep)
Handle a close of the device.
struct lps25h::lps25h_bus_option bus_options[]
void start()
Initialise the automatic measurement state machine and start it.
virtual void poll_notify(pollevent_t events)
Report new poll events.
uint32_t get_device_id() const
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
#define DRV_BARO_DEVTYPE_LPS25H
void print_info()
Diagnostics - print some basic information about the driver.
int write_reg(uint8_t reg, uint8_t val)
Write a register.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
device::Device *(* LPS25H_constructor)(int)
void perf_count(perf_counter_t handle)
Count a performance event.
Abstract class for any character device.
void perf_free(perf_counter_t handle)
Free a counter.
int read_reg(uint8_t reg, uint8_t &val)
Read a register.
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...
__EXPORT int lps25h_main(int argc, char *argv[])
void reset(enum LPS25H_BUS busid)
Reset the driver.
void start(enum LPS25H_BUS busid)
Start the driver.
ringbuffer::RingBuffer * _reports
sensor_baro_s _last_report
used for info()
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...
int reset()
Reset the device.
virtual int write(unsigned address, void *data, unsigned count)
Write directly to the device.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
LPS25H operator=(const LPS25H &)
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
virtual int ioctl(unsigned operation, unsigned &arg)
Perform a device-specific operation.
perf_counter_t _sample_perf
void set_device_type(uint8_t devtype)
struct lps25h_bus_option & find_bus(enum LPS25H_BUS busid)
find a bus structure for a busid
int collect()
Collect the result of the most recent measurement.
void info()
Print a little info about the driver.
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.
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.
Fundamental base class for all physical drivers (I2C, SPI).
CDev(const char *devname)
Constructor.
void usage()
Prints info about the driver argument usage.
int measure()
Issue a measurement command.
LPS25H_constructor interface_constructor
LPS25H(device::Device *interface, const char *path)
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
perf_counter_t _comms_errors
unsigned _measure_interval
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).
virtual int read(unsigned address, void *data, unsigned count)
Read directly from the device.
virtual int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
Perform a poll setup/teardown operation.