diff --git a/src/drivers/barometer/bmp388/CMakeLists.txt b/src/drivers/barometer/bmp388/CMakeLists.txt index cf22eef6f4b0..f4e09193d41c 100644 --- a/src/drivers/barometer/bmp388/CMakeLists.txt +++ b/src/drivers/barometer/bmp388/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_module( bmp388_spi.cpp bmp388_i2c.cpp bmp388.cpp + bmp388_main.cpp DEPENDS drivers_barometer px4_work_queue diff --git a/src/drivers/barometer/bmp388/bmp388.cpp b/src/drivers/barometer/bmp388/bmp388.cpp index ad00a9e6e621..aa67fb76d12f 100644 --- a/src/drivers/barometer/bmp388/bmp388.cpp +++ b/src/drivers/barometer/bmp388/bmp388.cpp @@ -33,116 +33,177 @@ /** * @file bmp388.cpp + * * Driver for the BMP388 barometric pressure sensor connected via I2C * (SPI still TODO/test). + * + * Refer to: https://github.com/BoschSensortec/BMP3-Sensor-API */ #include "bmp388.h" -#include - -enum BMP388_BUS { - BMP388_BUS_ALL = 0, - BMP388_BUS_I2C_INTERNAL, - BMP388_BUS_I2C_INTERNAL1, - BMP388_BUS_I2C_EXTERNAL, - BMP388_BUS_SPI_INTERNAL, - BMP388_BUS_SPI_EXTERNAL -}; - -/* - * BMP388 internal constants and data structures. - */ +BMP388::BMP388(IBMP388 *interface, const char *path) : + CDev(path), + ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), + _px4_baro(interface->get_device_id()), + _interface(interface), + _osr_t(BMP3_NO_OVERSAMPLING), + _osr_p(BMP3_NO_OVERSAMPLING), + _sample_perf(perf_alloc(PC_ELAPSED, "bmp388: read")), + _measure_perf(perf_alloc(PC_ELAPSED, "bmp388: measure")), + _comms_errors(perf_alloc(PC_COUNT, "bmp388: comms errors")) +{ + _px4_baro.set_device_type(DRV_DEVTYPE_BMP388); +} -class BMP388 : public cdev::CDev, public px4::ScheduledWorkItem +BMP388::~BMP388() { -public: - BMP388(bmp388::IBMP388 *interface, const char *path); - virtual ~BMP388(); + /* make sure we are truly inactive */ + stop(); - virtual int init(); + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + delete _interface; +} -private: - PX4Barometer _px4_baro; +int +BMP388::init() +{ + int ret = CDev::init(); - bmp388::IBMP388 *_interface; + if (ret != OK) { + PX4_ERR("CDev init failed"); + return ret; + } - bool _running{false}; + if (!soft_reset()) { + PX4_WARN("failed to reset baro during init"); + return -EIO; + } - uint8_t _curr_ctrl{0}; + if (_interface->get_reg(BMP3_CHIP_ID_ADDR) != BMP3_CHIP_ID) { + PX4_WARN("id of your baro is not: 0x%02x", BMP3_CHIP_ID); + return -EIO; + } - unsigned _report_interval{0}; // 0 - no cycling, otherwise period of sending a report - unsigned _max_measure_interval{0}; // interval in microseconds needed to measure + _cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR); + if (!_cal) { + PX4_WARN("failed to get baro cal init"); + return -EIO; + } - bool _collect_phase{false}; + if (!validate_trimming_param()) { + PX4_WARN("failed to validate trim param"); + return -EIO; + } - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; + if (!set_sensor_settings()) { + PX4_WARN("failed to set sensor settings"); + return -EIO; + } - struct bmp388::calibration_s *_cal; // stored calibration constants - struct bmp388::fcalibration_s _fcal; // pre processed calibration constants + /* do a first measurement cycle to populate reports with valid data */ + if (measure()) { + return -EIO; + } - float _P; /* in Pa */ - float _T; /* in K */ + start(); + return OK; +} - /* periodic execution helpers */ - void start_cycle(); - void stop_cycle(); +void +BMP388::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + printf("measurement interval: %u us \n", _measure_interval); - void Run() override; + _px4_baro.print_status(); +} - int measure(); //start measure - int collect(); //get results and publish +void +BMP388::start() +{ + /* make sure we are stopped first */ + uint32_t last_call_interval = _call_interval; + stop(); + _call_interval = last_call_interval; - // from BMP3 library... - bool soft_reset(); - bool get_calib_data(); - bool validate_trimming_param(); - bool set_sensor_settings(); - bool set_op_mode(uint8_t op_mode); + ScheduleOnInterval(_call_interval, 1000); +} - bool get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data); - bool compensate_data(uint8_t sensor_comp, const struct bmp3_uncomp_data *uncomp_data, struct bmp3_data *comp_data); -}; +void +BMP388::stop() +{ + ScheduleClear(); +} /* - * Driver 'main' command. + * ScheduledWorkItem override */ -extern "C" __EXPORT int bmp388_main(int argc, char *argv[]); - -BMP388::BMP388(bmp388::IBMP388 *interface, const char *path) : - CDev(path), - ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), - _px4_baro(interface->get_device_id()), - _interface(interface), - _sample_perf(perf_alloc(PC_ELAPSED, "bmp388: read")), - _measure_perf(perf_alloc(PC_ELAPSED, "bmp388: measure")), - _comms_errors(perf_alloc(PC_COUNT, "bmp388: comms errors")) +void +BMP388::Run() { - _px4_baro.set_device_type(DRV_DEVTYPE_BMP388); + measure(); } -BMP388::~BMP388() +int +BMP388::measure() { - // make sure we are truly inactive - stop_cycle(); + /* enable pressure and temperature */ + uint8_t sensor_comp = BMP3_PRESS | BMP3_TEMP; + bmp3_data data{}; - // free perf counters - perf_free(_sample_perf); - perf_free(_measure_perf); - perf_free(_comms_errors); + perf_begin(_measure_perf); - delete _interface; + /* start measurement */ + if (!set_op_mode(BMP3_FORCED_MODE)) { + PX4_WARN("failed to set operating mode"); + perf_count(_comms_errors); + perf_cancel(_measure_perf); + return -EIO; + } + + /* wait based on sampling config */ + px4_usleep(_measure_interval); + + perf_end(_measure_perf); + + perf_begin(_sample_perf); + + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (!get_sensor_data(sensor_comp, &data)) { + perf_count(_comms_errors); + perf_cancel(_sample_perf); + return -EIO; + } + + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + + float temperature = (float)(data.temperature / 100.0f); + float pressure = (float)(data.pressure / 100.0f); // to hecto Pascal + pressure = pressure / 100.0f; // to mbar + + _px4_baro.set_temperature(temperature); + _px4_baro.update(timestamp_sample, pressure); // to mbar + + perf_end(_sample_perf); + + return OK; } +/*! + * @brief This API performs the soft reset of the sensor. + * + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c + */ bool BMP388::soft_reset() { @@ -171,7 +232,7 @@ BMP388::soft_reset() /* * @brief function to calculate CRC for the trimming parameters * - * See https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/self-test/bmp3_selftest.c + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/self-test/bmp3_selftest.c * */ static int8_t cal_crc(uint8_t seed, uint8_t data) { @@ -198,7 +259,7 @@ static int8_t cal_crc(uint8_t seed, uint8_t data) /* * @brief Function to verify the trimming parameters * - * See https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/self-test/bmp3_selftest.c + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/self-test/bmp3_selftest.c * */ bool BMP388::validate_trimming_param() @@ -206,12 +267,12 @@ BMP388::validate_trimming_param() bool result = false; uint8_t crc = 0xFF; uint8_t stored_crc; - uint8_t trim_param[21]; + uint8_t trim_param[BMP3_CALIB_DATA_LEN]; uint8_t i; - memcpy(&trim_param, _cal, 21); + memcpy(&trim_param, _cal, BMP3_CALIB_DATA_LEN); - for (i = 0; i < 21; i++) { + for (i = 0; i < BMP3_CALIB_DATA_LEN; i++) { crc = (uint8_t)cal_crc(crc, trim_param[i]); } @@ -226,31 +287,81 @@ BMP388::validate_trimming_param() return result; } +uint32_t +BMP388::get_measurement_time(uint8_t osr_t, uint8_t osr_p) +{ + /* + From BST-BMP388-DS001.pdf + + Pressure Temperature Measurement + Oversample Oversample Time (Forced) + x1 1x 4.9 ms + x2 1x 6.9 ms + x4 1x 10.9 ms + x8 1x 18.9 ms + x16 2x 36.9 ms + x32 2x 68.9 m + */ + + uint32_t meas_time_us = 0; // unsupported value by default + + if (osr_t == BMP3_NO_OVERSAMPLING) { + switch (osr_p) { + case BMP3_NO_OVERSAMPLING: + meas_time_us = 4900; + break; + + case BMP3_OVERSAMPLING_2X: + meas_time_us = 6900; + break; + + case BMP3_OVERSAMPLING_4X: + meas_time_us = 10900; + break; + + case BMP3_OVERSAMPLING_8X: + meas_time_us = 18900; + break; + } + + } else if (osr_t == BMP3_OVERSAMPLING_2X) { + switch (osr_p) { + case BMP3_OVERSAMPLING_16X: + meas_time_us = 36900; + break; + + case BMP3_OVERSAMPLING_32X: + meas_time_us = 68900; + break; + } + } + + return meas_time_us; +} + +/*! + * @brief This API sets the power control(pressure enable and + * temperature enable), over sampling, odr and filter + * settings in the sensor. + * + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c + */ bool BMP388::set_sensor_settings() { - int ret; + _measure_interval = get_measurement_time(_osr_t, _osr_p); - uint8_t press_os = BMP3_NO_OVERSAMPLING; - uint8_t temp_os = BMP3_NO_OVERSAMPLING; - - /* - Pressure Temperature Measurement - Oversample Oversample Time Max - x1 1x 6.4 ms - x2 1x 8.7 ms - x4 1x 13.3 ms - x8 1x 22.5 ms - x16 2x 43.2 ms - */ - _max_measure_interval = 6400; + if (_measure_interval == 0) { + PX4_WARN("unsupported oversampling selected"); + return false; + } /* Select the pressure and temperature sensor to be enabled */ uint8_t pwc_ctl_reg = 0; pwc_ctl_reg = BMP3_SET_BITS_POS_0(pwc_ctl_reg, BMP3_PRESS_EN, BMP3_ENABLE); pwc_ctl_reg = BMP3_SET_BITS(pwc_ctl_reg, BMP3_TEMP_EN, BMP3_ENABLE); - ret = _interface->set_reg(pwc_ctl_reg, BMP3_PWR_CTRL_ADDR); + int ret = _interface->set_reg(pwc_ctl_reg, BMP3_PWR_CTRL_ADDR); if (ret != OK) { PX4_WARN("failed to set settings BMP3_PWR_CTRL_ADDR"); @@ -259,10 +370,9 @@ BMP388::set_sensor_settings() /* Select the output data rate and over sampling settings for pressure and temperature */ uint8_t osr_ctl_reg = 0; - osr_ctl_reg = BMP3_SET_BITS_POS_0(osr_ctl_reg, BMP3_PRESS_OS, press_os); - osr_ctl_reg = BMP3_SET_BITS(osr_ctl_reg, BMP3_TEMP_OS, temp_os); + osr_ctl_reg = BMP3_SET_BITS_POS_0(osr_ctl_reg, BMP3_PRESS_OS, _osr_p); + osr_ctl_reg = BMP3_SET_BITS(osr_ctl_reg, BMP3_TEMP_OS, _osr_t); - /* 0x1C is the register address of over sampling register */ ret = _interface->set_reg(osr_ctl_reg, BMP3_OSR_ADDR); if (ret != OK) { @@ -273,8 +383,7 @@ BMP388::set_sensor_settings() uint8_t odr_ctl_reg = 0; odr_ctl_reg = BMP3_SET_BITS_POS_0(odr_ctl_reg, BMP3_ODR, BMP3_ODR_25_HZ); - /* 0x1D is the register address of output data rate register */ - ret = _interface->set_reg(odr_ctl_reg, 0x1D); + ret = _interface->set_reg(odr_ctl_reg, BMP3_ODR_ADDR); if (ret != OK) { PX4_WARN("failed to set settingsoutput data rate register"); @@ -287,6 +396,8 @@ BMP388::set_sensor_settings() /*! * @brief This API sets the power mode of the sensor. + * + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c */ bool BMP388::set_op_mode(uint8_t op_mode) @@ -299,11 +410,8 @@ BMP388::set_op_mode(uint8_t op_mode) op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR); last_set_mode = BMP3_GET_BITS(op_mode_reg_val, BMP3_OP_MODE); - /* If the sensor is not in sleep mode put the device to sleep - * mode */ + /* Device needs to be put in sleep mode to transition */ if (last_set_mode != BMP3_SLEEP_MODE) { - /* Device should be put to sleep before transiting to - * forced mode or normal mode */ op_mode_reg_val = op_mode_reg_val & (~(BMP3_OP_MODE_MSK)); ret = _interface->set_reg(op_mode_reg_val, BMP3_PWR_CTRL_ADDR); @@ -332,21 +440,20 @@ BMP388::set_op_mode(uint8_t op_mode) /*! * @brief This internal API is used to parse the pressure or temperature or * both the data and store it in the bmp3_uncomp_data structure instance. + * + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c */ static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data *uncomp_data) { - /* Temporary variables to store the sensor data */ uint32_t data_xlsb; uint32_t data_lsb; uint32_t data_msb; - /* Store the parsed register values for pressure data */ data_xlsb = (uint32_t)reg_data[0]; data_lsb = (uint32_t)reg_data[1] << 8; data_msb = (uint32_t)reg_data[2] << 16; uncomp_data->pressure = data_msb | data_lsb | data_xlsb; - /* Store the parsed register values for temperature data */ data_xlsb = (uint32_t)reg_data[3]; data_lsb = (uint32_t)reg_data[4] << 8; data_msb = (uint32_t)reg_data[5] << 16; @@ -358,6 +465,8 @@ static void parse_sensor_data(const uint8_t *reg_data, struct bmp3_uncomp_data * * @brief This internal API is used to compensate the raw temperature data and * return the compensated temperature data in integer data type. * For eg if returned temperature is 2426 then it is 2426/100 = 24.26 deg Celsius + * + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c */ static int64_t compensate_temperature(const struct bmp3_uncomp_data *uncomp_data, struct bmp3_calib_data *calib_data) { @@ -387,6 +496,8 @@ static int64_t compensate_temperature(const struct bmp3_uncomp_data *uncomp_data * @brief This internal API is used to compensate the raw pressure data and * return the compensated pressure data in integer data type. * for eg return if pressure is 9528709 which is 9528709/100 = 95287.09 Pascal or 952.8709 hecto Pascal + * + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c */ static uint64_t compensate_pressure(const struct bmp3_uncomp_data *uncomp_data, const struct bmp3_calib_data *calib_data) @@ -432,6 +543,8 @@ static uint64_t compensate_pressure(const struct bmp3_uncomp_data *uncomp_data, /*! * @brief This internal API is used to compensate the pressure or temperature * or both the data according to the component selected by the user. + * + * Refer: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c */ bool BMP388::compensate_data(uint8_t sensor_comp, @@ -449,7 +562,6 @@ BMP388::compensate_data(uint8_t sensor_comp, } if (sensor_comp & BMP3_PRESS) { - /* Compensate the pressure data */ comp_data->pressure = compensate_pressure(uncomp_data, &calib_data); } @@ -471,429 +583,15 @@ BMP388::get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data) bool result = false; int8_t rslt; - /* Array to store the pressure and temperature data read from - * the sensor */ uint8_t reg_data[BMP3_P_T_DATA_LEN] = { 0 }; struct bmp3_uncomp_data uncomp_data = { 0 }; - /* Read the pressure and temperature data from the sensor */ rslt = _interface->get_reg_buf(BMP3_DATA_ADDR, reg_data, BMP3_P_T_DATA_LEN); if (rslt == OK) { - /* Parse the read data from the sensor */ parse_sensor_data(reg_data, &uncomp_data); - - /* Compensate the pressure/temperature/both data read - * from the sensor */ result = compensate_data(sensor_comp, &uncomp_data, comp_data); } return result; } - -int -BMP388::init() -{ - int ret = CDev::init(); - - if (ret != OK) { - PX4_ERR("CDev init failed"); - return ret; - } - - if (!soft_reset()) { - PX4_WARN("failed to reset baro during init"); - return -EIO; - } - - if (_interface->get_reg(BMP3_CHIP_ID_ADDR) != BMP3_CHIP_ID) { - PX4_WARN("id of your baro is not: 0x%02x", BMP3_CHIP_ID); - return -EIO; - } - - _cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR); - - if (!_cal) { - PX4_WARN("failed to get baro cal init"); - return -EIO; - } - - if (!validate_trimming_param()) { - PX4_WARN("failed to validate trim param"); - return -EIO; - } - - if (!set_sensor_settings()) { - PX4_WARN("failed to set sensor settings"); - return -EIO; - } - - /* do a first measurement cycle to populate reports with valid data */ - if (measure()) { - return -EIO; - } - - usleep(_max_measure_interval); - - if (collect()) { - return -EIO; - } - - return OK; -} - -void -BMP388::start_cycle() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _running = true; - - /* schedule a cycle to start things */ - ScheduleNow(); -} - -void -BMP388::stop_cycle() -{ - _running = false; - - ScheduleClear(); -} - -void -BMP388::Run() -{ - if (_collect_phase) { - collect(); - unsigned wait_gap = _report_interval - _max_measure_interval; - - if ((wait_gap != 0) && (_running)) { - //need to wait some time before new measurement - ScheduleDelayed(wait_gap); - - return; - } - - } - - measure(); - - if (_running) { - ScheduleDelayed(_max_measure_interval); - } -} - -int -BMP388::measure() -{ - _collect_phase = true; - - perf_begin(_measure_perf); - - /* start measure */ - if (!set_op_mode(BMP3_FORCED_MODE)) { - PX4_WARN("failed to set operating mode"); - perf_count(_comms_errors); - perf_cancel(_measure_perf); - return -EIO; - } - - perf_end(_measure_perf); - - return OK; -} - -int -BMP388::collect() -{ - uint8_t sensor_comp = BMP3_PRESS | BMP3_TEMP; - - _collect_phase = false; - - perf_begin(_sample_perf); - - /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - /* Variable used to store the compensated data */ - bmp3_data data{}; - - if (!get_sensor_data(sensor_comp, &data)) { - perf_count(_comms_errors); - perf_cancel(_sample_perf); - return -EIO; - } - - _px4_baro.set_error_count(perf_event_count(_comms_errors)); - - // Temperature - _T = (float)(data.temperature / 100.0f); - - // Pressure - _P = (float)(data.pressure / 100.0f); // to hecto Pascal - - float temperature = _T; - float pressure = _P / 100.0f; // to mbar - - _px4_baro.set_temperature(temperature); - _px4_baro.update(timestamp_sample, pressure); // to mbar - - perf_end(_sample_perf); - - return OK; -} - -void -BMP388::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u us \n", _report_interval); - - _px4_baro.print_status(); -} - -/** - * Local functions in support of the shell command. - */ -namespace bmp388 -{ - -/* - list of supported bus configurations - */ -struct bmp388_bus_option { - enum BMP388_BUS busid; - const char *devpath; - BMP388_constructor interface_constructor; - uint8_t busnum; - uint32_t device; - bool external; - BMP388 *dev; -} bus_options[] = { -#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) - { BMP388_BUS_SPI_EXTERNAL, "/dev/bmp388_spi_ext", &bmp388_spi_interface, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BARO, true, NULL }, -#endif -#if defined(PX4_SPIDEV_BARO) -# if defined(PX4_SPIDEV_BARO_BUS) - { BMP388_BUS_SPI_INTERNAL, "/dev/bmp388_spi_int", &bmp388_spi_interface, PX4_SPIDEV_BARO_BUS, PX4_SPIDEV_BARO, false, NULL }, -# else - { BMP388_BUS_SPI_INTERNAL, "/dev/bmp388_spi_int", &bmp388_spi_interface, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BARO, false, NULL }, -# endif -#endif -#if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_BMP388) - { BMP388_BUS_I2C_INTERNAL, "/dev/bmp388_i2c_int", &bmp388_i2c_interface, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP388, false, NULL }, -#endif -#if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV1_BMP388) - { BMP388_BUS_I2C_INTERNAL1, "/dev/bmp388_i2c_int1", &bmp388_i2c_interface, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV1_BMP388, false, NULL }, -#endif -#if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_OBDEV_BMP388) - { BMP388_BUS_I2C_EXTERNAL, "/dev/bmp388_i2c_ext", &bmp388_i2c_interface, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_BMP388, true, NULL }, -#endif -}; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) - -bool start_bus(struct bmp388_bus_option &bus); -struct bmp388_bus_option &find_bus(enum BMP388_BUS busid); -void start(enum BMP388_BUS busid); -void test(enum BMP388_BUS busid); -void reset(enum BMP388_BUS busid); -void info(); -void usage(); - - -/** - * Start the driver. - */ -bool -start_bus(struct bmp388_bus_option &bus) -{ - if (bus.dev != nullptr) { - PX4_ERR("bus option already started"); - exit(1); - } - - bmp388::IBMP388 *interface = bus.interface_constructor(bus.busnum, bus.device, bus.external); - - if (interface->init() != OK) { - delete interface; - PX4_WARN("no device on bus %u", (unsigned)bus.busid); - return false; - } - - bus.dev = new BMP388(interface, bus.devpath); - - if (bus.dev == nullptr) { - return false; - } - - if (OK != bus.dev->init()) { - delete bus.dev; - bus.dev = nullptr; - return false; - } - - int fd = open(bus.devpath, O_RDONLY); - - /* set the poll rate to default, starts automatic data collection */ - if (fd == -1) { - PX4_ERR("can't open baro device"); - exit(1); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - close(fd); - PX4_ERR("failed setting default poll rate"); - exit(1); - } - - close(fd); - return true; -} - - -/** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. - */ -void -start(enum BMP388_BUS busid) -{ - uint8_t i; - bool started = false; - - for (i = 0; i < NUM_BUS_OPTIONS; i++) { - if (busid == BMP388_BUS_ALL && bus_options[i].dev != NULL) { - // this device is already started - continue; - } - - if (busid != BMP388_BUS_ALL && bus_options[i].busid != busid) { - // not the one that is asked for - continue; - } - - started |= start_bus(bus_options[i]); - } - - if (!started) { - PX4_WARN("bus option number is %d", i); - PX4_ERR("driver start failed"); - exit(1); - } - - // one or more drivers started OK - exit(0); -} - - -/** - * find a bus structure for a busid - */ -struct bmp388_bus_option &find_bus(enum BMP388_BUS busid) -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - if ((busid == BMP388_BUS_ALL || - busid == bus_options[i].busid) && bus_options[i].dev != NULL) { - return bus_options[i]; - } - } - - PX4_ERR("bus %u not started", (unsigned)busid); - exit(1); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct bmp388_bus_option &bus = bus_options[i]; - - if (bus.dev != nullptr) { - PX4_WARN("%s", bus.devpath); - bus.dev->print_info(); - } - } - - exit(0); -} - -void -usage() -{ - PX4_WARN("missing command: try 'start', 'info'"); - PX4_WARN("options:"); - PX4_WARN(" -X (external I2C bus TODO)"); - PX4_WARN(" -I (internal I2C bus TODO)"); - PX4_WARN(" -S (external SPI bus)"); - PX4_WARN(" -s (internal SPI bus)"); -} - -} // namespace - -int -bmp388_main(int argc, char *argv[]) -{ - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - enum BMP388_BUS busid = BMP388_BUS_ALL; - - while ((ch = px4_getopt(argc, argv, "XIJSs", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'X': - busid = BMP388_BUS_I2C_EXTERNAL; - break; - - case 'I': - busid = BMP388_BUS_I2C_INTERNAL; - break; - - case 'J': - busid = BMP388_BUS_I2C_INTERNAL1; - break; - - case 'S': - busid = BMP388_BUS_SPI_EXTERNAL; - break; - - case 's': - busid = BMP388_BUS_SPI_INTERNAL; - break; - - default: - bmp388::usage(); - return 0; - } - } - - if (myoptind >= argc) { - bmp388::usage(); - return -1; - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - bmp388::start(busid); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - bmp388::info(); - } - - PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'"); - return -1; -} diff --git a/src/drivers/barometer/bmp388/bmp388.h b/src/drivers/barometer/bmp388/bmp388.h index a2a7a71735c5..3afd2915660b 100644 --- a/src/drivers/barometer/bmp388/bmp388.h +++ b/src/drivers/barometer/bmp388/bmp388.h @@ -39,18 +39,12 @@ #pragma once #include -#include - #include #include -#include -#include -#include #include #include -#include -#include #include +#include #include "board_config.h" #include "bmp3_defs.h" @@ -61,6 +55,7 @@ #define BMP3_POST_INIT_WAIT_TIME 40000 #define BMP3_TRIM_CRC_DATA_ADDR 0x30 #define BPM3_CMD_SOFT_RESET 0xB6 +#define BMP3_ODR_ADDR 0x1D // https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c /*! Power control settings */ @@ -72,9 +67,6 @@ /*! Advance settings */ #define ADV_SETT (0x1800) -namespace bmp388 -{ - #pragma pack(push,1) struct calibration_s { uint16_t par_t1; @@ -122,6 +114,11 @@ struct fcalibration_s { float p9; }; +/* + * BMP388 internal constants and data structures. + */ + + class IBMP388 { public: @@ -140,19 +137,63 @@ class IBMP388 virtual int set_reg(uint8_t value, uint8_t addr) = 0; // bulk read of data into buffer, return same pointer - virtual bmp388::data_s *get_data(uint8_t addr) = 0; + virtual data_s *get_data(uint8_t addr) = 0; // bulk read of calibration data into buffer, return same pointer - virtual bmp388::calibration_s *get_calibration(uint8_t addr) = 0; + virtual calibration_s *get_calibration(uint8_t addr) = 0; virtual uint32_t get_device_id() const = 0; }; -} /* namespace */ +class BMP388 : public cdev::CDev, public px4::ScheduledWorkItem +{ +public: + BMP388(IBMP388 *interface, const char *path); + virtual ~BMP388(); + + virtual int init(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + unsigned _call_interval{1000}; + + PX4Barometer _px4_baro; + IBMP388 *_interface; + + unsigned _measure_interval{0}; // interval in microseconds needed to measure + uint8_t _osr_t; // oversampling rate, temperature + uint8_t _osr_p; // oversampling rate, pressure + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + + struct calibration_s *_cal; // stored calibration constants + + void Run() override; + void start(); + void stop(); + int measure(); + int collect(); //get results and publish + uint32_t get_measurement_time(uint8_t osr_t, uint8_t osr_p); + + bool soft_reset(); + bool get_calib_data(); + bool validate_trimming_param(); + bool set_sensor_settings(); + bool set_op_mode(uint8_t op_mode); + + bool get_sensor_data(uint8_t sensor_comp, struct bmp3_data *comp_data); + bool compensate_data(uint8_t sensor_comp, const struct bmp3_uncomp_data *uncomp_data, struct bmp3_data *comp_data); +}; /* interface factories */ -extern bmp388::IBMP388 *bmp388_spi_interface(uint8_t busnum, uint32_t device, bool external); -extern bmp388::IBMP388 *bmp388_i2c_interface(uint8_t busnum, uint32_t device, bool external); -typedef bmp388::IBMP388 *(*BMP388_constructor)(uint8_t, uint32_t, bool); +extern IBMP388 *bmp388_spi_interface(uint8_t busnum, uint32_t device, bool external); +extern IBMP388 *bmp388_i2c_interface(uint8_t busnum, uint32_t device, bool external); +typedef IBMP388 *(*BMP388_constructor)(uint8_t, uint32_t, bool); diff --git a/src/drivers/barometer/bmp388/bmp388_i2c.cpp b/src/drivers/barometer/bmp388/bmp388_i2c.cpp index 48c1cb7170c3..354289ed3c38 100644 --- a/src/drivers/barometer/bmp388/bmp388_i2c.cpp +++ b/src/drivers/barometer/bmp388/bmp388_i2c.cpp @@ -32,16 +32,18 @@ ****************************************************************************/ /** - * @file bmp388_spi.cpp + * @file bmp388_i2c.cpp * - * SPI interface for BMP388 + * I2C interface for BMP388 */ +#include + #include "bmp388.h" #if defined(PX4_I2C_OBDEV_BMP388) || defined(PX4_I2C_EXT_OBDEV_BMP388) -class BMP388_I2C: public device::I2C, public bmp388::IBMP388 +class BMP388_I2C: public device::I2C, public IBMP388 { public: BMP388_I2C(uint8_t bus, uint32_t device, bool external); @@ -53,18 +55,18 @@ class BMP388_I2C: public device::I2C, public bmp388::IBMP388 uint8_t get_reg(uint8_t addr); int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); int set_reg(uint8_t value, uint8_t addr); - bmp388::data_s *get_data(uint8_t addr); - bmp388::calibration_s *get_calibration(uint8_t addr); + data_s *get_data(uint8_t addr); + calibration_s *get_calibration(uint8_t addr); uint32_t get_device_id() const override { return device::I2C::get_device_id(); } private: - struct bmp388::calibration_s _cal; - struct bmp388::data_s _data; + struct calibration_s _cal; + struct data_s _data; bool _external; }; -bmp388::IBMP388 *bmp388_i2c_interface(uint8_t busnum, uint32_t device, bool external) +IBMP388 *bmp388_i2c_interface(uint8_t busnum, uint32_t device, bool external) { return new BMP388_I2C(busnum, device, external); } @@ -105,11 +107,11 @@ int BMP388_I2C::set_reg(uint8_t value, uint8_t addr) return transfer(cmd, sizeof(cmd), nullptr, 0); } -bmp388::data_s *BMP388_I2C::get_data(uint8_t addr) +data_s *BMP388_I2C::get_data(uint8_t addr) { const uint8_t cmd = (uint8_t)(addr); - if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_data, sizeof(struct bmp388::data_s)) == OK) { + if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_data, sizeof(struct data_s)) == OK) { return (&_data); } else { @@ -117,11 +119,11 @@ bmp388::data_s *BMP388_I2C::get_data(uint8_t addr) } } -bmp388::calibration_s *BMP388_I2C::get_calibration(uint8_t addr) +calibration_s *BMP388_I2C::get_calibration(uint8_t addr) { const uint8_t cmd = (uint8_t)(addr); - if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(struct bmp388::calibration_s)) == OK) { + if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(struct calibration_s)) == OK) { return &(_cal); } else { diff --git a/src/drivers/barometer/bmp388/bmp388_main.cpp b/src/drivers/barometer/bmp388/bmp388_main.cpp new file mode 100644 index 000000000000..5b3ccad30fe6 --- /dev/null +++ b/src/drivers/barometer/bmp388/bmp388_main.cpp @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "bmp388.h" + +enum BMP388_BUS { + BMP388_BUS_ALL = 0, + BMP388_BUS_I2C_INTERNAL, + BMP388_BUS_I2C_INTERNAL1, + BMP388_BUS_I2C_EXTERNAL, + BMP388_BUS_SPI_INTERNAL, + BMP388_BUS_SPI_EXTERNAL +}; + +/** + * Local functions in support of the shell command. + */ +namespace bmp388 +{ + +/* + * list of supported bus configurations + */ +struct bmp388_bus_option { + enum BMP388_BUS busid; + const char *devpath; + BMP388_constructor interface_constructor; + uint8_t busnum; + uint32_t device; + bool external; + BMP388 *dev; +} bus_options[] = { +#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) + { BMP388_BUS_SPI_EXTERNAL, "/dev/bmp388_spi_ext", &bmp388_spi_interface, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BARO, true, NULL }, +#endif +#if defined(PX4_SPIDEV_BARO) +# if defined(PX4_SPIDEV_BARO_BUS) + { BMP388_BUS_SPI_INTERNAL, "/dev/bmp388_spi_int", &bmp388_spi_interface, PX4_SPIDEV_BARO_BUS, PX4_SPIDEV_BARO, false, NULL }, +# else + { BMP388_BUS_SPI_INTERNAL, "/dev/bmp388_spi_int", &bmp388_spi_interface, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BARO, false, NULL }, +# endif +#endif +#if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_BMP388) + { BMP388_BUS_I2C_INTERNAL, "/dev/bmp388_i2c_int", &bmp388_i2c_interface, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP388, false, NULL }, +#endif +#if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV1_BMP388) + { BMP388_BUS_I2C_INTERNAL1, "/dev/bmp388_i2c_int1", &bmp388_i2c_interface, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV1_BMP388, false, NULL }, +#endif +#if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_OBDEV_BMP388) + { BMP388_BUS_I2C_EXTERNAL, "/dev/bmp388_i2c_ext", &bmp388_i2c_interface, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_BMP388, true, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +/** + * Start the driver. + */ +bool +start_bus(struct bmp388_bus_option &bus) +{ + if (bus.dev != nullptr) { + PX4_ERR("bus option already started"); + exit(1); + } + + IBMP388 *interface = bus.interface_constructor(bus.busnum, bus.device, bus.external); + + if (interface->init() != OK) { + delete interface; + PX4_WARN("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new BMP388(interface, bus.devpath); + + if (bus.dev == nullptr) { + return false; + } + + if (OK != bus.dev->init()) { + delete bus.dev; + bus.dev = nullptr; + return false; + } + + return true; +} + +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum BMP388_BUS busid) +{ + uint8_t i; + bool started = false; + + for (i = 0; i < NUM_BUS_OPTIONS; i++) { + if (busid == BMP388_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (busid != BMP388_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i]); + } + + if (!started) { + PX4_WARN("bus option number is %d", i); + PX4_ERR("driver start failed"); + exit(1); + } + + // one or more drivers started OK + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { + struct bmp388_bus_option &bus = bus_options[i]; + + if (bus.dev != nullptr) { + PX4_WARN("%s", bus.devpath); + bus.dev->print_info(); + } + } + + exit(0); +} + +void +usage() +{ + PX4_WARN("missing command: try 'start', 'info'"); + PX4_WARN("options:"); + PX4_WARN(" -X (external I2C bus TODO)"); + PX4_WARN(" -I (internal I2C bus TODO)"); + PX4_WARN(" -S (external SPI bus)"); + PX4_WARN(" -s (internal SPI bus)"); +} + +} // namespace + +extern "C" __EXPORT int bmp388_main(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + enum BMP388_BUS busid = BMP388_BUS_ALL; + + while ((ch = px4_getopt(argc, argv, "XIJSs", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + busid = BMP388_BUS_I2C_EXTERNAL; + break; + + case 'I': + busid = BMP388_BUS_I2C_INTERNAL; + break; + + case 'J': + busid = BMP388_BUS_I2C_INTERNAL1; + break; + + case 'S': + busid = BMP388_BUS_SPI_EXTERNAL; + break; + + case 's': + busid = BMP388_BUS_SPI_INTERNAL; + break; + + default: + bmp388::usage(); + return 0; + } + } + + if (myoptind >= argc) { + bmp388::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + bmp388::start(busid); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + bmp388::info(); + } + + PX4_ERR("unrecognized command, try 'start' or 'info'"); + return -1; +} diff --git a/src/drivers/barometer/bmp388/bmp388_spi.cpp b/src/drivers/barometer/bmp388/bmp388_spi.cpp index dd6e4848f423..818c11fb0c64 100644 --- a/src/drivers/barometer/bmp388/bmp388_spi.cpp +++ b/src/drivers/barometer/bmp388/bmp388_spi.cpp @@ -34,9 +34,11 @@ /** * @file bmp388_spi.cpp * - * SPI interface for BMP388 + * SPI interface for BMP388 (NOTE: untested!) */ +#include + #include "bmp388.h" @@ -49,16 +51,16 @@ #pragma pack(push,1) struct spi_data_s { uint8_t addr; - struct bmp388::data_s data; + struct data_s data; }; struct spi_calibration_s { uint8_t addr; - struct bmp388::calibration_s cal; + struct calibration_s cal; }; #pragma pack(pop) -class BMP388_SPI: public device::SPI, public bmp388::IBMP388 +class BMP388_SPI: public device::SPI, public IBMP388 { public: BMP388_SPI(uint8_t bus, uint32_t device, bool is_external_device); @@ -68,9 +70,10 @@ class BMP388_SPI: public device::SPI, public bmp388::IBMP388 int init(); uint8_t get_reg(uint8_t addr); + int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); int set_reg(uint8_t value, uint8_t addr); - bmp388::data_s *get_data(uint8_t addr); - bmp388::calibration_s *get_calibration(uint8_t addr); + data_s *get_data(uint8_t addr); + calibration_s *get_calibration(uint8_t addr); uint32_t get_device_id() const override { return device::SPI::get_device_id(); } @@ -80,7 +83,7 @@ class BMP388_SPI: public device::SPI, public bmp388::IBMP388 bool _external; }; -bmp388::IBMP388 *bmp388_spi_interface(uint8_t busnum, uint32_t device, bool external) +IBMP388 *bmp388_spi_interface(uint8_t busnum, uint32_t device, bool external) { return new BMP388_SPI(busnum, device, external); } @@ -109,13 +112,19 @@ uint8_t BMP388_SPI::get_reg(uint8_t addr) return cmd[1]; } +int BMP388_SPI::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) +{ + uint8_t cmd[1] = {(uint8_t)(addr | DIR_READ)}; + return transfer(&cmd[0], buf, len); +} + int BMP388_SPI::set_reg(uint8_t value, uint8_t addr) { uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit return transfer(&cmd[0], nullptr, 2); } -bmp388::data_s *BMP388_SPI::get_data(uint8_t addr) +data_s *BMP388_SPI::get_data(uint8_t addr) { _data.addr = (uint8_t)(addr | DIR_READ); //set MSB bit @@ -127,7 +136,7 @@ bmp388::data_s *BMP388_SPI::get_data(uint8_t addr) } } -bmp388::calibration_s *BMP388_SPI::get_calibration(uint8_t addr) +calibration_s *BMP388_SPI::get_calibration(uint8_t addr) { _cal.addr = addr | DIR_READ;