47 ScheduledWorkItem(MODULE_NAME,
px4::device_bus_to_wq(interface->get_device_id())),
48 _interface(interface),
51 _device_type(device_type),
52 _collect_phase(false),
58 _orb_class_instance(-1),
92 bool autodetect =
false;
97 PX4_DEBUG(
"CDev init failed");
105 PX4_DEBUG(
"can't get memory for reports");
195 warnx(
"failed to create sensor_baro publication");
233 return ret ? ret : -EAGAIN;
311 unsigned interval = (1000000 / arg);
344 return CDev::ioctl(filp, cmd, arg);
394 ScheduleDelayed(2800);
503 int32_t T2 =
POW2(dT) >> 31;
506 int64_t OFF2 = 5 * f >> 1;
507 int64_t SENS2 = 5 * f >> 2;
513 SENS2 += 11 * f2 >> 1;
532 int32_t T2 =
POW2(dT) >> 31;
535 int64_t OFF2 = 61 * f >> 4;
536 int64_t SENS2 = 2 *
f;
553 int32_t
P = (((raw *
_SENS) >> 21) -
_OFF) >> 15;
590 _reports->print_info(
"report queue");
618 crc_read = n_prom[7];
621 n_prom[7] = (0xFF00 & (n_prom[7]));
623 for (cnt = 0; cnt < 16; cnt++) {
626 n_rem ^= (uint8_t)((n_prom[cnt >> 1]) & 0x00FF);
629 n_rem ^= (uint8_t)(n_prom[cnt >> 1] >> 8);
632 for (n_bit = 8; n_bit > 0; n_bit--) {
633 if (n_rem & 0x8000) {
634 n_rem = (n_rem << 1) ^ 0x3000;
637 n_rem = (n_rem << 1);
643 n_rem = (0x000F & (n_rem >> 12));
644 n_prom[7] = crc_read;
647 return (0x000F & crc_read) == (n_rem ^ 0x00);
#define ADDR_CMD_CONVERT_D1
#define MS5611_CONVERSION_INTERVAL
uint16_t c1_pressure_sens
bool crc4(uint16_t *n_prom)
MS5611 crc4 cribbed from the datasheet.
perf_counter_t _measure_perf
#define BARO_BASE_DEVICE_PATH
measure the time elapsed performing an event
virtual ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen)
Perform a read from the device.
MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path, enum MS56XX_DEVICE_TYPES device_type)
perf_counter_t _sample_perf
virtual int register_class_devname(const char *class_devname)
Register a class device name, automatically adding device class instance suffix if need be...
perf_counter_t _comms_errors
#define DRV_BARO_DEVTYPE_MS5611
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
count the number of times an event occurs
#define MS5611_MEASUREMENT_RATIO
void start()
Initialize 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. ...
uint16_t c4_temp_coeff_pres_offset
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Shared defines for the ms5611 driver.
void perf_count(perf_counter_t handle)
Count a performance event.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
device::Device * _interface
uint16_t c5_reference_temp
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...
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
void perf_end(perf_counter_t handle)
End a performance event.
virtual int collect()
Collect the result of the most recent measurement.
uint16_t c2_pressure_offset
#define INCREMENT(_x, _lim)
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
uint16_t c6_temp_coeff_temp
virtual int ioctl(unsigned operation, unsigned &arg)
Perform a device-specific operation.
void set_device_type(uint8_t devtype)
void stop()
Stop the automatic measurement state machine.
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
virtual bool external() const
#define ADDR_CMD_CONVERT_D2
const char * get_devname() const
Get the device name.
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
Local functions in support of the shell command.
#define SENSORIOCRESET
Reset the sensor to its default configuration.
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
virtual int measure()
Issue a measurement command for the current state.
Fundamental base class for all physical drivers (I2C, SPI).
uint16_t c3_temp_coeff_pres_sens
ringbuffer::RingBuffer * _reports
virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
Perform an ioctl operation on the device.
enum MS56XX_DEVICE_TYPES _device_type
void print_info()
Diagnostics - print some basic information about the driver.
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
unsigned _measure_interval
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define DRV_BARO_DEVTYPE_MS5607
__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.