diff --git a/boards/holybro/kakutef7/init/rc.board_sensors b/boards/holybro/kakutef7/init/rc.board_sensors index e5ff547d2b..c294458e08 100644 --- a/boards/holybro/kakutef7/init/rc.board_sensors +++ b/boards/holybro/kakutef7/init/rc.board_sensors @@ -12,11 +12,10 @@ then fi # Internal Baro -bmp280 start +bmp280 -I start # Possible external compasses ist8310 -C -b 1 start ist8310 -C -b 2 start hmc5883 -C -T -X start qmc5883 -X start - diff --git a/boards/nxp/fmuk66-v3/init/rc.board_sensors b/boards/nxp/fmuk66-v3/init/rc.board_sensors index 4e2c2540ba..1e3913ccae 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_sensors +++ b/boards/nxp/fmuk66-v3/init/rc.board_sensors @@ -19,7 +19,7 @@ bmp280 -I start mpl3115a2 -I start # Internal SPI (accel + mag) -fxos8701cq start -R 0 +fxos8701cq start # Internal SPI (gyro) fxas21002c start diff --git a/src/drivers/barometer/bmp280/BMP280.cpp b/src/drivers/barometer/bmp280/BMP280.cpp new file mode 100644 index 0000000000..c38f5728db --- /dev/null +++ b/src/drivers/barometer/bmp280/BMP280.cpp @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * Copyright (c) 2016-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 "BMP280.hpp" + +BMP280::BMP280(bmp280::IBMP280 *interface) : + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), + _px4_baro(interface->get_device_id()), + _interface(interface), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) +{ +} + +BMP280::~BMP280() +{ + // make sure we are truly inactive + Stop(); + + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + + delete _interface; +} + +int +BMP280::init() +{ + // reset sensor + _interface->set_reg(BMP280_VALUE_RESET, BMP280_ADDR_RESET); + usleep(10000); + + // check id + if (_interface->get_reg(BMP280_ADDR_ID) != BMP280_VALUE_ID) { + PX4_WARN("id of your baro is not: 0x%02x", BMP280_VALUE_ID); + return -EIO; + } + + // set config, recommended settings + _interface->set_reg(_curr_ctrl, BMP280_ADDR_CTRL); + _interface->set_reg(BMP280_CONFIG_F16, BMP280_ADDR_CONFIG); + + // get calibration and pre process them + _cal = _interface->get_calibration(BMP280_ADDR_CAL); + + _fcal.t1 = _cal->t1 * powf(2, 4); + _fcal.t2 = _cal->t2 * powf(2, -14); + _fcal.t3 = _cal->t3 * powf(2, -34); + + _fcal.p1 = _cal->p1 * (powf(2, 4) / -100000.0f); + _fcal.p2 = _cal->p1 * _cal->p2 * (powf(2, -31) / -100000.0f); + _fcal.p3 = _cal->p1 * _cal->p3 * (powf(2, -51) / -100000.0f); + + _fcal.p4 = _cal->p4 * powf(2, 4) - powf(2, 20); + _fcal.p5 = _cal->p5 * powf(2, -14); + _fcal.p6 = _cal->p6 * powf(2, -31); + + _fcal.p7 = _cal->p7 * powf(2, -4); + _fcal.p8 = _cal->p8 * powf(2, -19) + 1.0f; + _fcal.p9 = _cal->p9 * powf(2, -35); + + Start(); + + return OK; +} + +void +BMP280::Start() +{ + // reset the report ring and state machine + _collect_phase = false; + + // schedule a cycle to start things + ScheduleNow(); +} + +void +BMP280::Stop() +{ + ScheduleClear(); +} + +void +BMP280::Run() +{ + if (_collect_phase) { + collect(); + + } else { + measure(); + } + + ScheduleDelayed(_measure_interval); +} + +int +BMP280::measure() +{ + perf_begin(_measure_perf); + + _collect_phase = true; + + // start measure + int ret = _interface->set_reg(_curr_ctrl | BMP280_CTRL_MODE_FORCE, BMP280_ADDR_CTRL); + + if (ret != OK) { + perf_count(_comms_errors); + perf_cancel(_measure_perf); + return -EIO; + } + + perf_end(_measure_perf); + + return OK; +} + +int +BMP280::collect() +{ + perf_begin(_sample_perf); + + _collect_phase = false; + + // 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(); + bmp280::data_s *data = _interface->get_data(BMP280_ADDR_DATA); + + if (data == nullptr) { + perf_count(_comms_errors); + perf_cancel(_sample_perf); + return -EIO; + } + + // convert data to number 20 bit + uint32_t p_raw = data->p_msb << 12 | data->p_lsb << 4 | data->p_xlsb >> 4; + uint32_t t_raw = data->t_msb << 12 | data->t_lsb << 4 | data->t_xlsb >> 4; + + // Temperature + float ofs = (float) t_raw - _fcal.t1; + float t_fine = (ofs * _fcal.t3 + _fcal.t2) * ofs; + const float T = t_fine * (1.0f / 5120.0f); + + // Pressure + float tf = t_fine - 128000.0f; + float x1 = (tf * _fcal.p6 + _fcal.p5) * tf + _fcal.p4; + float x2 = (tf * _fcal.p3 + _fcal.p2) * tf + _fcal.p1; + + float pf = ((float) p_raw + x1) / x2; + const float P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7; + + _px4_baro.set_error_count(perf_event_count(_comms_errors)); + _px4_baro.set_temperature(T); + + float pressure = P / 100.0f; // to mbar + _px4_baro.update(timestamp_sample, pressure); + + perf_end(_sample_perf); + + return OK; +} + +void +BMP280::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_measure_perf); + perf_print_counter(_comms_errors); + + _px4_baro.print_status(); +} diff --git a/src/drivers/barometer/bmp280/BMP280.hpp b/src/drivers/barometer/bmp280/BMP280.hpp index d7a68c4f35..1461e981cc 100644 --- a/src/drivers/barometer/bmp280/BMP280.hpp +++ b/src/drivers/barometer/bmp280/BMP280.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -35,61 +35,43 @@ #include "bmp280.h" -/* - * BMP280 internal constants and data structures. - */ +#include +#include +#include +#include +#include -class BMP280 : public cdev::CDev, public px4::ScheduledWorkItem +class BMP280 : public px4::ScheduledWorkItem { public: - BMP280(bmp280::IBMP280 *interface, const char *path); + BMP280(bmp280::IBMP280 *interface); virtual ~BMP280(); - virtual int init(); - - virtual ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ + int init(); void print_info(); private: - bmp280::IBMP280 *_interface; + void Run() override; + void Start(); + void Stop(); - bool _running; + int measure(); //start measure + int collect(); //get results and publish - uint8_t _curr_ctrl; + PX4Barometer _px4_baro; - unsigned _report_interval; // 0 - no cycling, otherwise period of sending a report - unsigned _max_measure_interval; // interval in microseconds needed to measure + bmp280::IBMP280 *_interface; - ringbuffer::RingBuffer *_reports; + // set config, recommended settings + static constexpr uint8_t _curr_ctrl{BMP280_CTRL_P16 | BMP280_CTRL_T2}; + static constexpr uint32_t _measure_interval{BMP280_MT_INIT + BMP280_MT *(16 - 1 + 2 - 1)}; - bool _collect_phase; - - orb_advert_t _baro_topic; - int _orb_class_instance; - int _class_instance; + bool _collect_phase{false}; perf_counter_t _sample_perf; perf_counter_t _measure_perf; perf_counter_t _comms_errors; - struct bmp280::calibration_s *_cal; //stored calibration constants - struct bmp280::fcalibration_s _fcal; //pre processed calibration constants - - float _P; /* in Pa */ - float _T; /* in K */ - - - /* periodic execution helpers */ - void start_cycle(); - void stop_cycle(); - - void Run() override; - - int measure(); //start measure - int collect(); //get results and publish + bmp280::calibration_s *_cal{nullptr}; //stored calibration constants + bmp280::fcalibration_s _fcal{}; //pre processed calibration constants }; diff --git a/src/drivers/barometer/bmp280/bmp280_i2c.cpp b/src/drivers/barometer/bmp280/BMP280_I2C.cpp similarity index 67% rename from src/drivers/barometer/bmp280/bmp280_i2c.cpp rename to src/drivers/barometer/bmp280/BMP280_I2C.cpp index 99553a3e8a..0e2a8a6315 100644 --- a/src/drivers/barometer/bmp280/bmp280_i2c.cpp +++ b/src/drivers/barometer/bmp280/BMP280_I2C.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -37,55 +37,46 @@ * SPI interface for BMP280 */ - #include "bmp280.h" +#include +#include + #if defined(PX4_I2C_OBDEV_BMP280) || defined(PX4_I2C_EXT_OBDEV_BMP280) class BMP280_I2C: public device::I2C, public bmp280::IBMP280 { public: - BMP280_I2C(uint8_t bus, uint32_t device, bool external); - virtual ~BMP280_I2C() = default; + BMP280_I2C(uint8_t bus, uint32_t device); + virtual ~BMP280_I2C() override = default; - bool is_external(); - int init(); + int init() override { return I2C::init(); } - uint8_t get_reg(uint8_t addr); - int set_reg(uint8_t value, uint8_t addr); - bmp280::data_s *get_data(uint8_t addr); - bmp280::calibration_s *get_calibration(uint8_t addr); + uint8_t get_reg(uint8_t addr) override; + int set_reg(uint8_t value, uint8_t addr) override; + + bmp280::data_s *get_data(uint8_t addr) override; + bmp280::calibration_s *get_calibration(uint8_t addr) override; uint32_t get_device_id() const override { return device::I2C::get_device_id(); } private: - struct bmp280::calibration_s _cal; - struct bmp280::data_s _data; - bool _external; + bmp280::calibration_s _cal{}; + bmp280::data_s _data{}; }; -bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint32_t device, bool external) +bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint32_t device) { - return new BMP280_I2C(busnum, device, external); + return new BMP280_I2C(busnum, device); } -BMP280_I2C::BMP280_I2C(uint8_t bus, uint32_t device, bool external) : +BMP280_I2C::BMP280_I2C(uint8_t bus, uint32_t device) : I2C("BMP280_I2C", nullptr, bus, device, 100 * 1000) { - _external = external; } -bool BMP280_I2C::is_external() -{ - return _external; -} - -int BMP280_I2C::init() -{ - return I2C::init(); -} - -uint8_t BMP280_I2C::get_reg(uint8_t addr) +uint8_t +BMP280_I2C::get_reg(uint8_t addr) { uint8_t cmd[2] = { (uint8_t)(addr), 0}; transfer(&cmd[0], 1, &cmd[1], 1); @@ -93,17 +84,19 @@ uint8_t BMP280_I2C::get_reg(uint8_t addr) return cmd[1]; } -int BMP280_I2C::set_reg(uint8_t value, uint8_t addr) +int +BMP280_I2C::set_reg(uint8_t value, uint8_t addr) { uint8_t cmd[2] = { (uint8_t)(addr), value}; return transfer(cmd, sizeof(cmd), nullptr, 0); } -bmp280::data_s *BMP280_I2C::get_data(uint8_t addr) +bmp280::data_s * +BMP280_I2C::get_data(uint8_t addr) { - const uint8_t cmd = (uint8_t)(addr); + const uint8_t cmd = addr; - if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_data, sizeof(struct bmp280::data_s)) == OK) { + if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_data, sizeof(bmp280::data_s)) == OK) { return (&_data); } else { @@ -111,11 +104,12 @@ bmp280::data_s *BMP280_I2C::get_data(uint8_t addr) } } -bmp280::calibration_s *BMP280_I2C::get_calibration(uint8_t addr) +bmp280::calibration_s * +BMP280_I2C::get_calibration(uint8_t addr) { - const uint8_t cmd = (uint8_t)(addr) ; + const uint8_t cmd = addr; - if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(struct bmp280::calibration_s)) == OK) { + if (transfer(&cmd, sizeof(cmd), (uint8_t *)&_cal, sizeof(bmp280::calibration_s)) == OK) { return &(_cal); } else { diff --git a/src/drivers/barometer/bmp280/bmp280_spi.cpp b/src/drivers/barometer/bmp280/BMP280_SPI.cpp similarity index 65% rename from src/drivers/barometer/bmp280/bmp280_spi.cpp rename to src/drivers/barometer/bmp280/BMP280_SPI.cpp index 3d9ff8b7f6..fcd6484147 100644 --- a/src/drivers/barometer/bmp280/bmp280_spi.cpp +++ b/src/drivers/barometer/bmp280/BMP280_SPI.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -39,13 +39,15 @@ #include "bmp280.h" +#include +#include + +#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO) /* SPI protocol address bits */ #define DIR_READ (1<<7) //for set #define DIR_WRITE ~(1<<7) //for clear -#if defined(PX4_SPIDEV_BARO) || defined(PX4_SPIDEV_EXT_BARO) - #pragma pack(push,1) struct spi_data_s { uint8_t addr; @@ -61,65 +63,57 @@ struct spi_calibration_s { class BMP280_SPI: public device::SPI, public bmp280::IBMP280 { public: - BMP280_SPI(uint8_t bus, uint32_t device, bool is_external_device); - virtual ~BMP280_SPI() = default; + BMP280_SPI(uint8_t bus, uint32_t device); + virtual ~BMP280_SPI() override = default; - bool is_external(); - int init(); + int init() override { return SPI::init(); } - uint8_t get_reg(uint8_t addr); - int set_reg(uint8_t value, uint8_t addr); - bmp280::data_s *get_data(uint8_t addr); - bmp280::calibration_s *get_calibration(uint8_t addr); + uint8_t get_reg(uint8_t addr) override; + int set_reg(uint8_t value, uint8_t addr) override; + + bmp280::data_s *get_data(uint8_t addr) override; + bmp280::calibration_s *get_calibration(uint8_t addr) override; uint32_t get_device_id() const override { return device::SPI::get_device_id(); } private: - spi_calibration_s _cal; - spi_data_s _data; - bool _external; + spi_calibration_s _cal{}; + spi_data_s _data{}; }; -bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint32_t device, bool external) +bmp280::IBMP280 * +bmp280_spi_interface(uint8_t busnum, uint32_t device) { - return new BMP280_SPI(busnum, device, external); + return new BMP280_SPI(busnum, device); } -BMP280_SPI::BMP280_SPI(uint8_t bus, uint32_t device, bool is_external_device) : +BMP280_SPI::BMP280_SPI(uint8_t bus, uint32_t device) : SPI("BMP280_SPI", nullptr, bus, device, SPIDEV_MODE3, 10 * 1000 * 1000) { - _external = is_external_device; } -bool BMP280_SPI::is_external() +uint8_t +BMP280_SPI::get_reg(uint8_t addr) { - return _external; -}; - -int BMP280_SPI::init() -{ - return SPI::init(); -}; - -uint8_t BMP280_SPI::get_reg(uint8_t addr) -{ - uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit + uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; // set MSB bit transfer(&cmd[0], &cmd[0], 2); return cmd[1]; } -int BMP280_SPI::set_reg(uint8_t value, uint8_t addr) +int +BMP280_SPI::set_reg(uint8_t value, uint8_t addr) { - uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit + uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; // clear MSB bit return transfer(&cmd[0], nullptr, 2); } -bmp280::data_s *BMP280_SPI::get_data(uint8_t addr) +bmp280::data_s * +BMP280_SPI::get_data(uint8_t addr) { - _data.addr = (uint8_t)(addr | DIR_READ); //set MSB bit + _data.addr = (uint8_t)(addr | DIR_READ); // set MSB bit - if (transfer((uint8_t *)&_data, (uint8_t *)&_data, sizeof(struct spi_data_s)) == OK) { + if (transfer((uint8_t *)&_data, (uint8_t *)&_data, sizeof(spi_data_s)) == OK) { return &(_data.data); } else { @@ -127,11 +121,12 @@ bmp280::data_s *BMP280_SPI::get_data(uint8_t addr) } } -bmp280::calibration_s *BMP280_SPI::get_calibration(uint8_t addr) +bmp280::calibration_s * +BMP280_SPI::get_calibration(uint8_t addr) { _cal.addr = addr | DIR_READ; - if (transfer((uint8_t *)&_cal, (uint8_t *)&_cal, sizeof(struct spi_calibration_s)) == OK) { + if (transfer((uint8_t *)&_cal, (uint8_t *)&_cal, sizeof(spi_calibration_s)) == OK) { return &(_cal.cal); } else { diff --git a/src/drivers/barometer/bmp280/CMakeLists.txt b/src/drivers/barometer/bmp280/CMakeLists.txt index 72770b31ef..7bdb1ea18a 100644 --- a/src/drivers/barometer/bmp280/CMakeLists.txt +++ b/src/drivers/barometer/bmp280/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Copyright (c) 2016-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 @@ -32,16 +32,16 @@ ############################################################################ px4_add_module( - MODULE drivers__bmp280 + MODULE drivers__barometer__bmp280 MAIN bmp280 - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS + BMP280.cpp BMP280.hpp - bmp280_spi.cpp - bmp280_i2c.cpp - bmp280.cpp + BMP280_I2C.cpp + BMP280_SPI.cpp + bmp280_main.cpp DEPENDS + drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/bmp280/bmp280.cpp b/src/drivers/barometer/bmp280/bmp280.cpp deleted file mode 100644 index b6279bfbd2..0000000000 --- a/src/drivers/barometer/bmp280/bmp280.cpp +++ /dev/null @@ -1,403 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 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 "BMP280.hpp" - -BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) : - CDev(path), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), - _interface(interface), - _running(false), - _report_interval(0), - _reports(nullptr), - _collect_phase(false), - _baro_topic(nullptr), - _orb_class_instance(-1), - _class_instance(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "bmp280_read")), - _measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")), - _comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors")) -{ -} - -BMP280::~BMP280() -{ - /* make sure we are truly inactive */ - stop_cycle(); - - if (_class_instance != -1) { - unregister_class_devname(BARO_BASE_DEVICE_PATH, _class_instance); - } - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - if (_baro_topic != nullptr) { - orb_unadvertise(_baro_topic); - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_measure_perf); - perf_free(_comms_errors); - - delete _interface; -} - -int -BMP280::init() -{ - int ret = CDev::init(); - - if (ret != OK) { - PX4_ERR("CDev init failed"); - return ret; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s)); - - if (_reports == nullptr) { - PX4_ERR("can't get memory for reports"); - ret = -ENOMEM; - return ret; - } - - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(BARO_BASE_DEVICE_PATH); - - /* reset sensor */ - _interface->set_reg(BMP280_VALUE_RESET, BMP280_ADDR_RESET); - usleep(10000); - - /* check id*/ - if (_interface->get_reg(BMP280_ADDR_ID) != BMP280_VALUE_ID) { - PX4_WARN("id of your baro is not: 0x%02x", BMP280_VALUE_ID); - return -EIO; - } - - /* set config, recommended settings */ - _curr_ctrl = BMP280_CTRL_P16 | BMP280_CTRL_T2; - _interface->set_reg(_curr_ctrl, BMP280_ADDR_CTRL); - _max_measure_interval = (BMP280_MT_INIT + BMP280_MT * (16 - 1 + 2 - 1)); - _interface->set_reg(BMP280_CONFIG_F16, BMP280_ADDR_CONFIG); - - /* get calibration and pre process them*/ - _cal = _interface->get_calibration(BMP280_ADDR_CAL); - - _fcal.t1 = _cal->t1 * powf(2, 4); - _fcal.t2 = _cal->t2 * powf(2, -14); - _fcal.t3 = _cal->t3 * powf(2, -34); - - _fcal.p1 = _cal->p1 * (powf(2, 4) / -100000.0f); - _fcal.p2 = _cal->p1 * _cal->p2 * (powf(2, -31) / -100000.0f); - _fcal.p3 = _cal->p1 * _cal->p3 * (powf(2, -51) / -100000.0f); - - _fcal.p4 = _cal->p4 * powf(2, 4) - powf(2, 20); - _fcal.p5 = _cal->p5 * powf(2, -14); - _fcal.p6 = _cal->p6 * powf(2, -31); - - _fcal.p7 = _cal->p7 * powf(2, -4); - _fcal.p8 = _cal->p8 * powf(2, -19) + 1.0f; - _fcal.p9 = _cal->p9 * powf(2, -35); - - /* do a first measurement cycle to populate reports with valid data */ - sensor_baro_s brp; - _reports->flush(); - - if (measure()) { - return -EIO; - } - - usleep(_max_measure_interval); - - if (collect()) { - return -EIO; - } - - _reports->get(&brp); - - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, _interface->is_external() ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic == nullptr) { - PX4_WARN("failed to create sensor_baro publication"); - return -ENOMEM; - } - - return OK; - -} - -ssize_t -BMP280::read(cdev::file_t *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_baro_s); - sensor_baro_s *brp = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_report_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(brp)) { - ret += sizeof(*brp); - brp++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - - _reports->flush(); - - if (measure()) { - return -EIO; - } - - usleep(_max_measure_interval); - - if (collect()) { - return -EIO; - } - - if (_reports->get(brp)) { //get new generated report - ret = sizeof(*brp); - } - - return ret; -} - -int -BMP280::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - - unsigned interval = 0; - - switch (arg) { - - case 0: - return -EINVAL; - - case SENSOR_POLLRATE_DEFAULT: - interval = _max_measure_interval; - - /* FALLTHROUGH */ - default: { - if (interval == 0) { - interval = (1000000 / arg); - } - - /* do we need to start internal polling? */ - bool want_start = (_report_interval == 0); - - /* check against maximum rate */ - if (interval < _max_measure_interval) { - return -EINVAL; - } - - _report_interval = interval; - - if (want_start) { - start_cycle(); - } - - return OK; - } - } - - break; - } - - case SENSORIOCRESET: - /* - * Since we are initialized, we do not need to do anything, since the - * PROM is correctly read and the part does not need to be configured. - */ - return OK; - - default: - break; - } - - return CDev::ioctl(filp, cmd, arg); -} - -void -BMP280::start_cycle() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _running = true; - _reports->flush(); - - /* schedule a cycle to start things */ - ScheduleNow(); -} - -void -BMP280::stop_cycle() -{ - _running = false; - ScheduleClear(); -} - -void -BMP280::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 -BMP280::measure() -{ - _collect_phase = true; - - perf_begin(_measure_perf); - - /* start measure */ - int ret = _interface->set_reg(_curr_ctrl | BMP280_CTRL_MODE_FORCE, BMP280_ADDR_CTRL); - - if (ret != OK) { - perf_count(_comms_errors); - perf_cancel(_measure_perf); - return -EIO; - } - - perf_end(_measure_perf); - - return OK; -} - -int -BMP280::collect() -{ - _collect_phase = false; - - perf_begin(_sample_perf); - - sensor_baro_s report; - /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.device_id = _interface->get_device_id(); - - bmp280::data_s *data = _interface->get_data(BMP280_ADDR_DATA); - - if (data == nullptr) { - perf_count(_comms_errors); - perf_cancel(_sample_perf); - return -EIO; - } - - //convert data to number 20 bit - uint32_t p_raw = data->p_msb << 12 | data->p_lsb << 4 | data->p_xlsb >> 4; - uint32_t t_raw = data->t_msb << 12 | data->t_lsb << 4 | data->t_xlsb >> 4; - - // Temperature - float ofs = (float) t_raw - _fcal.t1; - float t_fine = (ofs * _fcal.t3 + _fcal.t2) * ofs; - _T = t_fine * (1.0f / 5120.0f); - - // Pressure - float tf = t_fine - 128000.0f; - float x1 = (tf * _fcal.p6 + _fcal.p5) * tf + _fcal.p4; - float x2 = (tf * _fcal.p3 + _fcal.p2) * tf + _fcal.p1; - - float pf = ((float) p_raw + x1) / x2; - _P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7; - - - report.temperature = _T; - report.pressure = _P / 100.0f; // to mbar - - /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - perf_end(_sample_perf); - - return OK; -} - -void -BMP280::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u us \n", _report_interval); - _reports->print_info("report queue"); - - sensor_baro_s brp = {}; - _reports->get(&brp); - print_message(brp); -} diff --git a/src/drivers/barometer/bmp280/bmp280.h b/src/drivers/barometer/bmp280/bmp280.h index 7afae768b2..55bdcfe721 100644 --- a/src/drivers/barometer/bmp280/bmp280.h +++ b/src/drivers/barometer/bmp280/bmp280.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016-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 @@ -38,23 +38,6 @@ */ #pragma once -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "board_config.h" - - #define BMP280_ADDR_CAL 0x88 /* address of 12x 2 bytes calibration data */ #define BMP280_ADDR_DATA 0xF7 /* address of 2x 3 bytes p-t data */ @@ -150,7 +133,6 @@ class IBMP280 public: virtual ~IBMP280() = default; - virtual bool is_external() = 0; virtual int init() = 0; // read reg value @@ -173,6 +155,6 @@ public: /* interface factories */ -extern bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint32_t device, bool external); -extern bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint32_t device, bool external); -typedef bmp280::IBMP280 *(*BMP280_constructor)(uint8_t, uint32_t, bool); +extern bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint32_t device); +extern bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint32_t device); +typedef bmp280::IBMP280 *(*BMP280_constructor)(uint8_t, uint32_t); diff --git a/src/drivers/barometer/bmp280/bmp280_main.cpp b/src/drivers/barometer/bmp280/bmp280_main.cpp index ac6395d0dc..071633c11a 100644 --- a/src/drivers/barometer/bmp280/bmp280_main.cpp +++ b/src/drivers/barometer/bmp280/bmp280_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -31,204 +31,178 @@ * ****************************************************************************/ -/** - * @file bmp280.cpp - * Driver for the BMP280 barometric pressure sensor connected via I2C TODO or SPI. - */ +#include +#include #include "BMP280.hpp" -enum BMP280_BUS { - BMP280_BUS_ALL = 0, - BMP280_BUS_I2C_INTERNAL, - BMP280_BUS_I2C_EXTERNAL, - BMP280_BUS_SPI_INTERNAL, - BMP280_BUS_SPI_EXTERNAL +enum class BMP280_BUS { + ALL = 0, + I2C_INTERNAL, + I2C_EXTERNAL, + SPI_INTERNAL, + SPI_EXTERNAL }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int bmp280_main(int argc, char *argv[]); - -/** - * Local functions in support of the shell command. - */ namespace bmp280 { -/* - list of supported bus configurations - */ +// list of supported bus configurations struct bmp280_bus_option { - enum BMP280_BUS busid; - const char *devpath; + BMP280_BUS busid; BMP280_constructor interface_constructor; uint8_t busnum; - uint32_t device; - bool external; - BMP280 *dev; + uint32_t address; + BMP280 *dev; } bus_options[] = { -#if defined(PX4_SPIDEV_EXT_BARO) && defined(PX4_SPI_BUS_EXT) - { BMP280_BUS_SPI_EXTERNAL, "/dev/bmp280_spi_ext", &bmp280_spi_interface, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BARO, true, NULL }, +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BARO) + { BMP280_BUS::SPI_EXTERNAL, &bmp280_spi_interface, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BARO, nullptr }, #endif -#if defined(PX4_SPIDEV_BARO) -# if defined(PX4_SPIDEV_BARO_BUS) - { BMP280_BUS_SPI_INTERNAL, "/dev/bmp280_spi_int", &bmp280_spi_interface, PX4_SPIDEV_BARO_BUS, PX4_SPIDEV_BARO, false, NULL }, -# else - { BMP280_BUS_SPI_INTERNAL, "/dev/bmp280_spi_int", &bmp280_spi_interface, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BARO, false, NULL }, -# endif +#if defined(PX4_SPIDEV_BARO_BUS) && defined(PX4_SPIDEV_BARO) + { BMP280_BUS::SPI_INTERNAL, &bmp280_spi_interface, PX4_SPIDEV_BARO_BUS, PX4_SPIDEV_BARO, nullptr }, +#elif defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_BARO) + { BMP280_BUS::SPI_INTERNAL, &bmp280_spi_interface, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BARO, nullptr }, #endif #if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_BMP280) - { BMP280_BUS_I2C_INTERNAL, "/dev/bmp280_i2c_int", &bmp280_i2c_interface, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP280, false, NULL }, + { BMP280_BUS::I2C_INTERNAL, &bmp280_i2c_interface, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_BMP280, nullptr }, #endif #if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_OBDEV_BMP280) - { BMP280_BUS_I2C_EXTERNAL, "/dev/bmp280_i2c_ext", &bmp280_i2c_interface, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_BMP280, true, NULL }, + { BMP280_BUS::I2C_EXTERNAL, &bmp280_i2c_interface, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_BMP280, nullptr }, #endif }; -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -/** - * Start the driver. - */ -static bool -start_bus(struct bmp280_bus_option &bus) +// find a bus structure for a busid +static struct bmp280_bus_option *find_bus(BMP280_BUS busid) { - if (bus.dev != nullptr) { - PX4_ERR("bus option already started"); - return false; + for (bmp280_bus_option &bus_option : bus_options) { + if ((busid == BMP280_BUS::ALL || + busid == bus_option.busid) && bus_option.dev != nullptr) { + + return &bus_option; + } } - bmp280::IBMP280 *interface = bus.interface_constructor(bus.busnum, bus.device, bus.external); + return nullptr; +} - if (interface->init() != OK) { - delete interface; +static bool start_bus(bmp280_bus_option &bus) +{ + bmp280::IBMP280 *interface = bus.interface_constructor(bus.busnum, bus.address); + + if ((interface == nullptr) || (interface->init() != PX4_OK)) { PX4_WARN("no device on bus %u", (unsigned)bus.busid); + delete interface; return false; } - bus.dev = new BMP280(interface, bus.devpath); + BMP280 *dev = new BMP280(interface); - if (bus.dev == nullptr) { + if (dev == nullptr) { + PX4_ERR("driver allocate failed"); + delete interface; return false; } - if (OK != bus.dev->init()) { - delete bus.dev; - bus.dev = nullptr; + if (dev->init() != PX4_OK) { + PX4_ERR("driver start failed"); + delete dev; // BMP280 deletes the interface return false; } - int fd = px4_open(bus.devpath, O_RDONLY); + bus.dev = dev; - /* set the poll rate to default, starts automatic data collection */ - if (fd == -1) { - PX4_ERR("can't open baro device"); - return false; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); - PX4_ERR("failed setting default poll rate"); - return false; - } - - px4_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. - */ -static int -start(enum BMP280_BUS busid) +static int start(BMP280_BUS busid) { - uint8_t i; - bool started = false; - - for (i = 0; i < NUM_BUS_OPTIONS; i++) { - if (busid == BMP280_BUS_ALL && bus_options[i].dev != NULL) { + for (bmp280_bus_option &bus_option : bus_options) { + if (bus_option.dev != nullptr) { // this device is already started + PX4_WARN("already started"); continue; } - if (busid != BMP280_BUS_ALL && bus_options[i].busid != busid) { + if (busid != BMP280_BUS::ALL && bus_option.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"); - return PX4_ERROR; - } - - // one or more drivers started OK - return PX4_OK; -} - -/** - * Print a little info about the driver. - */ -static int -info() -{ - for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { - struct bmp280_bus_option *bus = &bus_options[i]; - - if (bus != nullptr && bus->dev != nullptr) { - PX4_WARN("%s", bus->devpath); - bus->dev->print_info(); + if (start_bus(bus_option)) { + return PX4_OK; } } - return 0; + return PX4_ERROR; } -static int -usage() +static int stop(BMP280_BUS busid) { - 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)"); + bmp280_bus_option *bus = find_bus(busid); + + if (bus != nullptr && bus->dev != nullptr) { + delete bus->dev; + bus->dev = nullptr; + + } else { + PX4_WARN("driver not running"); + return PX4_ERROR; + } + + return PX4_OK; +} + +static int status(BMP280_BUS busid) +{ + bmp280_bus_option *bus = find_bus(busid); + + if (bus != nullptr && bus->dev != nullptr) { + bus->dev->print_info(); + return PX4_OK; + } + + PX4_WARN("driver not running"); + return PX4_ERROR; +} + +static int usage() +{ + PX4_INFO("missing command: try 'start', 'stop', 'status'"); + PX4_INFO("options:"); + PX4_INFO(" -X (i2c external bus)"); + PX4_INFO(" -I (i2c internal bus)"); + PX4_INFO(" -s (spi internal bus)"); + PX4_INFO(" -S (spi external bus)"); + return 0; } } // namespace -int -bmp280_main(int argc, char *argv[]) +extern "C" int bmp280_main(int argc, char *argv[]) { int myoptind = 1; int ch; const char *myoptarg = nullptr; - enum BMP280_BUS busid = BMP280_BUS_ALL; - while ((ch = px4_getopt(argc, argv, "XISs", &myoptind, &myoptarg)) != EOF) { + BMP280_BUS busid = BMP280_BUS::ALL; + + while ((ch = px4_getopt(argc, argv, "XISs:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'X': - busid = BMP280_BUS_I2C_EXTERNAL; + busid = BMP280_BUS::I2C_EXTERNAL; break; case 'I': - busid = BMP280_BUS_I2C_INTERNAL; + busid = BMP280_BUS::I2C_INTERNAL; break; case 'S': - busid = BMP280_BUS_SPI_EXTERNAL; + busid = BMP280_BUS::SPI_EXTERNAL; break; case 's': - busid = BMP280_BUS_SPI_INTERNAL; + busid = BMP280_BUS::SPI_INTERNAL; break; default: @@ -242,18 +216,14 @@ bmp280_main(int argc, char *argv[]) const char *verb = argv[myoptind]; - /* - * Start/load the driver. - */ if (!strcmp(verb, "start")) { return bmp280::start(busid); - } - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - return bmp280::info(); + } else if (!strcmp(verb, "stop")) { + return bmp280::stop(busid); + + } else if (!strcmp(verb, "status")) { + return bmp280::status(busid); } return bmp280::usage(); diff --git a/src/drivers/magnetometer/bmm150/bmm150.cpp b/src/drivers/magnetometer/bmm150/bmm150.cpp index 7bca3a8f97..83a89f001d 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.cpp +++ b/src/drivers/magnetometer/bmm150/bmm150.cpp @@ -275,12 +275,12 @@ BMM150::BMM150(int bus, const char *path, enum Rotation rotation) : dig_xy1(0), dig_xy2(0), dig_xyz1(0), - _sample_perf(perf_alloc(PC_ELAPSED, "bmm150_read")), - _bad_transfers(perf_alloc(PC_COUNT, "bmm150_bad_transfers")), - _good_transfers(perf_alloc(PC_COUNT, "bmm150_good_transfers")), - _measure_perf(perf_alloc(PC_ELAPSED, "bmp280_measure")), - _comms_errors(perf_alloc(PC_COUNT, "bmp280_comms_errors")), - _duplicates(perf_alloc(PC_COUNT, "bmm150_duplicates")), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), + _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good transfers")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")), + _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates")), _rotation(rotation), _got_duplicate(false) { @@ -295,7 +295,7 @@ BMM150::BMM150(int bus, const char *path, enum Rotation rotation) : _scale.z_scale = 1.0f; } -BMM150 :: ~BMM150() +BMM150::~BMM150() { /* make sure we are truly inactive */ stop(); @@ -322,7 +322,6 @@ BMM150 :: ~BMM150() perf_free(_measure_perf); perf_free(_comms_errors); perf_free(_duplicates); - } int BMM150::init()