diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index 2c1d8183ff..f829c07a65 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/src/drivers/imu/bma180/bma180.cpp @@ -40,40 +40,14 @@ #include #include #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - +#include +#include #include -#include - #include -#include -#include #include +#include -#define ACCEL_DEVICE_PATH "/dev/bma180" - - -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) +static constexpr uint8_t DIR_READ{1 << 7}; #define ADDR_CHIP_ID 0x00 #define CHIP_ID 0x03 @@ -121,8 +95,6 @@ #define ADDR_OFFSET_T 0x37 #define OFFSET_T_READOUT_12BIT (1<<0) -extern "C" __EXPORT int bma180_main(int argc, char *argv[]); - class BMA180 : public device::SPI, public I2CSPIDriver { public: @@ -132,30 +104,15 @@ public: static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); - static void print_usage(); - + static void print_usage(); int init() override; - - ssize_t read(struct file *filp, char *buffer, size_t buflen) override; - int ioctl(struct file *filp, int cmd, unsigned long arg) override; - - void print_status() override; - + void print_status() override; void RunImpl(); -protected: - int probe() override; private: + int probe() override; - unsigned _call_interval; - - ringbuffer::RingBuffer *_reports; - - struct accel_calibration_s _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - int _class_instance; + PX4Accelerometer _px4_accel; unsigned _current_lowpass; unsigned _current_range; @@ -178,7 +135,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(uint8_t reg); /** * Write a register in the BMA180 @@ -186,7 +143,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(uint8_t reg, uint8_t value); /** * Modify a register in the BMA180 @@ -197,7 +154,7 @@ private: * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + void modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits); /** * Set the BMA180 measurement range. @@ -222,38 +179,18 @@ BMA180::BMA180(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio spi_mode_e spi_mode) : SPI(DRV_ACC_DEVTYPE_BMA180, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _call_interval(0), - _reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(nullptr), - _class_instance(-1), - _current_lowpass(0), - _current_range(0), - _sample_perf(perf_alloc(PC_ELAPSED, "bma180_read")) + _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")) { - // default scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; } BMA180::~BMA180() { - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - /* delete the perf counter */ perf_free(_sample_perf); } -int -BMA180::init() +int BMA180::init() { int ret = PX4_ERROR; @@ -262,13 +199,6 @@ BMA180::init() goto out; } - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - - if (_reports == nullptr) { - goto out; - } - /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -293,9 +223,13 @@ BMA180::init() /* disable writing to chip config */ modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); - if (set_range(4)) { warnx("Failed setting range"); } + if (set_range(4)) { + PX4_ERR("Failed setting range"); + } - if (set_lowpass(75)) { warnx("Failed setting lowpass"); } + if (set_lowpass(75)) { + PX4_ERR("Failed setting lowpass"); + } if (read_reg(ADDR_CHIP_ID) == CHIP_ID) { ret = OK; @@ -304,28 +238,16 @@ BMA180::init() ret = PX4_ERROR; } - _class_instance = register_class_devname(ACCEL_DEVICE_PATH); - /* advertise sensor topic, measure manually to initialize valid report */ measure(); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - sensor_accel_s arp; - _reports->get(&arp); - - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp); - } - - _call_interval = 1000000 / 250; start(); out: return ret; } -int -BMA180::probe() +int BMA180::probe() { /* dummy read to ensure SPI state machine is sane */ read_reg(ADDR_CHIP_ID); @@ -337,100 +259,29 @@ BMA180::probe() return -EIO; } -ssize_t -BMA180::read(struct file *filp, char *buffer, size_t buflen) +uint8_t BMA180::read_reg(uint8_t reg) { - unsigned count = buflen / sizeof(sensor_accel_s); - sensor_accel_s *arp = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_call_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. - */ - while (count--) { - if (_reports->get(arp)) { - ret += sizeof(*arp); - arp++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _reports->flush(); - measure(); - - /* measurement will have generated a report, copy it out */ - if (_reports->get(arp)) { - ret = sizeof(*arp); - } - - return ret; -} - -int -BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case ACCELIOCSSCALE: - /* copy scale in */ - memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale)); - return OK; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -uint8_t -BMA180::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - + uint8_t cmd[2] {}; cmd[0] = reg | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); - return cmd[1]; } -void -BMA180::write_reg(unsigned reg, uint8_t value) +void BMA180::write_reg(uint8_t reg, uint8_t value) { - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - + uint8_t cmd[2] {reg, value}; transfer(cmd, nullptr, sizeof(cmd)); } -void -BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +void BMA180::modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits) { - uint8_t val; - - val = read_reg(reg); + uint8_t val = read_reg(reg); val &= ~clearbits; val |= setbits; write_reg(reg, val); } -int -BMA180::set_range(unsigned max_g) +int BMA180::set_range(unsigned max_g) { uint8_t rangebits; @@ -467,8 +318,8 @@ BMA180::set_range(unsigned max_g) } /* set new range scaling factor */ - _accel_range_m_s2 = _current_range * CONSTANTS_ONE_G; - _accel_range_scale = _accel_range_m_s2 / 8192.0f; + float accel_range_m_s2 = _current_range * CONSTANTS_ONE_G; + _px4_accel.set_scale(accel_range_m_s2 / 8192.f); /* enable writing to chip config */ modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE); @@ -484,8 +335,7 @@ BMA180::set_range(unsigned max_g) (OFFSET_LSB1_RANGE_MASK & rangebits)); } -int -BMA180::set_lowpass(unsigned frequency) +int BMA180::set_lowpass(unsigned frequency) { uint8_t bwbits; @@ -527,112 +377,64 @@ BMA180::set_lowpass(unsigned frequency) modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0); /* check if wanted value is now in register */ - return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == - (BW_TCS_BW_MASK & bwbits)); + return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == (BW_TCS_BW_MASK & bwbits)); } -void -BMA180::start() +void BMA180::start() { - /* reset the report ring */ - _reports->flush(); - /* start polling at the specified rate */ - ScheduleOnInterval(_call_interval, 10000); + ScheduleOnInterval(1000, 10000); } -void -BMA180::RunImpl() +void BMA180::RunImpl() { /* make another measurement */ measure(); } -void -BMA180::measure() +void BMA180::measure() { - /* BMA180 measurement registers */ -// #pragma pack(push, 1) -// struct { -// uint8_t cmd; -// int16_t x; -// int16_t y; -// int16_t z; -// } raw_report; -// #pragma pack(pop) - - sensor_accel_s report; - /* start the performance counter */ perf_begin(_sample_perf); - /* - * Fetch the full set of measurements from the BMA180 in one pass; - * starting from the X LSB. - */ - //raw_report.cmd = ADDR_ACC_X_LSB; - // XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); - /* - * Adjust and scale results to SI units. - * - * Note that we ignore the "new data" bits. At any time we read, each - * of the axis measurements are the "most recent", even if we've seen - * them before. There is no good way to synchronise with the internal - * measurement flow without using the external interrupt. - */ - report.timestamp = hrt_absolute_time(); - report.error_count = 0; /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report.y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report.x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report.z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + int16_t y_raw = read_reg(ADDR_ACC_X_LSB + 0); + y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + + int16_t x_raw = read_reg(ADDR_ACC_X_LSB + 2); + x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + + int16_t z_raw = read_reg(ADDR_ACC_X_LSB + 4); + z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report.x_raw = (report.x_raw / 4); - report.y_raw = (report.y_raw / 4); - report.z_raw = (report.z_raw / 4); + x_raw = (x_raw / 4); + y_raw = (y_raw / 4); + z_raw = (z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report.y_raw = -report.y_raw; + y_raw = -y_raw; - report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report.scaling = _accel_range_scale; - - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - /* publish for subscribers */ - if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); - } + _px4_accel.update(timestamp_sample, x_raw, y_raw, z_raw); /* stop the perf counter */ perf_end(_sample_perf); } -void -BMA180::print_status() +void BMA180::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); - _reports->print_info("report queue"); + _px4_accel.print_status(); } - -void -BMA180::print_usage() +void BMA180::print_usage() { PRINT_MODULE_USAGE_NAME("bma180", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); @@ -661,7 +463,7 @@ I2CSPIDriverBase *BMA180::instantiate(const BusCLIArguments &cli, const BusInsta return instance; } -extern "C" int bma180_main(int argc, char *argv[]) +extern "C" __EXPORT int bma180_main(int argc, char *argv[]) { int ch; using ThisDriver = BMA180;