diff --git a/src/drivers/magnetometer/qmc5883/CMakeLists.txt b/src/drivers/magnetometer/qmc5883/CMakeLists.txt index 935c9ac0b3..fdab77bb23 100644 --- a/src/drivers/magnetometer/qmc5883/CMakeLists.txt +++ b/src/drivers/magnetometer/qmc5883/CMakeLists.txt @@ -31,15 +31,15 @@ # ############################################################################ px4_add_module( - MODULE drivers__qmc5883 + MODULE drivers__magnetometer__qmc5883 MAIN qmc5883 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS QMC5883_I2C.cpp QMC5883.cpp QMC5883.hpp qmc5883_main.cpp DEPENDS + drivers_magnetometer px4_work_queue ) diff --git a/src/drivers/magnetometer/qmc5883/QMC5883.cpp b/src/drivers/magnetometer/qmc5883/QMC5883.cpp index c427c58e9e..be072ce57e 100644 --- a/src/drivers/magnetometer/qmc5883/QMC5883.cpp +++ b/src/drivers/magnetometer/qmc5883/QMC5883.cpp @@ -35,53 +35,26 @@ QMC5883::QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus, int i2c_address) : - CDev("QMC5883", nullptr), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), + _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), _interface(interface), - _reports(nullptr), - _scale{}, - _range_scale(1.0f / 12000.0f), - _range_ga(2.0f), _collect_phase(false), - _class_instance(-1), - _orb_class_instance(-1), - _mag_topic(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": rng_err")), _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_err")), - _sensor_ok(false), - _rotation(rotation), - _range_bits(0), _conf_reg(0), _temperature_counter(0), _temperature_error_count(0) { - // set the device type from the interface - _device_id.devid_s.bus_type = _interface->get_device_bus_type(); - _device_id.devid_s.bus = _interface->get_device_bus(); - _device_id.devid_s.address = _interface->get_device_address(); - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883; + _interface->set_device_type(DRV_MAG_DEVTYPE_QMC5883); - // default scaling - _scale.x_offset = 0; - _scale.x_scale = 1.0f; - _scale.y_offset = 0; - _scale.y_scale = 1.0f; - _scale.z_offset = 0; - _scale.z_scale = 1.0f; + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_QMC5883); + _px4_mag.set_external(_interface->external()); } QMC5883::~QMC5883() { - if (_reports != nullptr) { - delete _reports; - } - - if (_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); - } - // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -89,39 +62,15 @@ QMC5883::~QMC5883() perf_free(_conf_errors); } -int -QMC5883::init() +int QMC5883::init() { - int ret = PX4_ERROR; - - ret = CDev::init(); - - if (ret != OK) { - DEVICE_DEBUG("CDev init failed"); - goto out; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s)); - - if (_reports == nullptr) { - goto out; - } - /* reset the device configuration */ reset(); - _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - - ret = OK; - /* sensor is ok */ - _sensor_ok = true; - _measure_interval = QMC5883_CONVERSION_INTERVAL; start(); -out: - return ret; + return PX4_OK; } /** @@ -129,12 +78,10 @@ out: done periodically to cope with I2C bus noise causing the configuration of the compass to change. */ -void QMC5883::check_conf(void) +void QMC5883::check_conf() { - int ret; - uint8_t conf_reg_in = 0; - ret = read_reg(QMC5883_ADDR_CONTROL_1, conf_reg_in); + int ret = read_reg(QMC5883_ADDR_CONTROL_1, conf_reg_in); if (OK != ret) { perf_count(_comms_errors); @@ -151,152 +98,16 @@ void QMC5883::check_conf(void) } } -ssize_t -QMC5883::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_mag_s); - sensor_mag_s *mag_buf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_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(mag_buf)) { - ret += sizeof(sensor_mag_s); - mag_buf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _reports->flush(); - - /* wait for it to complete */ - px4_usleep(QMC5883_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - if (_reports->get(mag_buf)) { - ret = sizeof(sensor_mag_s); - } - } while (0); - - return ret; -} - -int -QMC5883::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - unsigned dummy = arg; - - switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = QMC5883_CONVERSION_INTERVAL; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* convert hz to interval in microseconds */ - unsigned interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < QMC5883_CONVERSION_INTERVAL) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - return reset(); - - case MAGIOCSRANGE: - return OK; - - case MAGIOCSSCALE: - /* set new scale factors */ - memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale)); - return 0; - - case MAGIOCGSCALE: - /* copy out scale factors */ - memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale)); - return 0; - - case MAGIOCGEXTERNAL: - DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); - return _interface->ioctl(cmd, dummy); - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -void -QMC5883::start() +void QMC5883::start() { /* reset the report ring and state machine */ _collect_phase = false; - _reports->flush(); /* schedule a cycle to start things */ ScheduleNow(); } -int -QMC5883::reset() +int QMC5883::reset() { /* read 0x00 once */ uint8_t data_bits_in = 0; @@ -308,10 +119,8 @@ QMC5883::reset() /* set reset period to 0x01 */ write_reg(QMC5883_ADDR_SET_RESET, QMC5883_SET_DEFAULT); - //use fixed range of 2G - _range_scale = 1.0f / 12000.0f; // 12000 LSB/Gauss at +/- 2G range - _range_ga = 2.00f; - _range_bits = 0x00; + // use fixed range of 2G + _px4_mag.set_scale(1.f / 12000.f); // 12000 LSB/Gauss at +/- 2G range /* set control register */ _conf_reg = QMC5883_MODE_REG_CONTINOUS_MODE | @@ -323,8 +132,7 @@ QMC5883::reset() return OK; } -void -QMC5883::RunImpl() +void QMC5883::RunImpl() { if (_measure_interval == 0) { return; @@ -335,7 +143,7 @@ QMC5883::RunImpl() /* perform collection */ if (OK != collect()) { - DEVICE_DEBUG("collection error"); + PX4_DEBUG("collection error"); /* restart the measurement state machine */ start(); return; @@ -364,9 +172,7 @@ QMC5883::RunImpl() } } - -int -QMC5883::collect() +int QMC5883::collect() { struct { /* status register and data as read back from the device */ uint8_t x[2]; @@ -382,18 +188,12 @@ QMC5883::collect() uint8_t check_counter; perf_begin(_sample_perf); - sensor_mag_s new_report; - bool sensor_is_onboard = false; float xraw_f; float yraw_f; float zraw_f; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - new_report.timestamp = hrt_absolute_time(); - new_report.error_count = perf_event_count(_comms_errors); - new_report.scaling = _range_scale; - new_report.device_id = _device_id.devid; + _px4_mag.set_error_count(perf_event_count(_comms_errors)); /* * @note We could read the status register here, which could tell us that @@ -403,11 +203,12 @@ QMC5883::collect() */ /* get measurements from the device */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); ret = _interface->read(QMC5883_ADDR_DATA_OUT_X_LSB, (uint8_t *)&qmc_report, sizeof(qmc_report)); if (ret != OK) { perf_count(_comms_errors); - DEVICE_DEBUG("data/status read error"); + PX4_DEBUG("data/status read error"); goto out; } @@ -428,8 +229,6 @@ QMC5883::collect() } /* get temperature measurements from the device */ - new_report.temperature = 0; - if (_temperature_counter++ == 100) { uint8_t raw_temperature[2]; @@ -441,33 +240,19 @@ QMC5883::collect() if (ret == OK) { int16_t temp16 = (((int16_t)raw_temperature[1]) << 8) + raw_temperature[0]; - new_report.temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f; + float temperature = QMC5883_TEMP_OFFSET + temp16 * 1.0f / 100.0f; + _px4_mag.set_temperature(temperature); } - - } else { - new_report.temperature = _last_report.temperature; } - /* * RAW outputs * * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ - new_report.x_raw = -report.y; - new_report.y_raw = report.x; - /* z remains z */ - new_report.z_raw = report.z; - /* scale values for output */ - - // XXX revisit for SPI part, might require a bus type IOCTL - unsigned dummy; - sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); - new_report.is_external = !sensor_is_onboard; - - if (sensor_is_onboard) { + if (!_px4_mag.external()) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; @@ -482,38 +267,7 @@ QMC5883::collect() yraw_f = report.x; zraw_f = report.z; - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; - - if (!(_pub_blocked)) { - - if (_mag_topic != nullptr) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); - - } else { - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, - &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); - - if (_mag_topic == nullptr) { - DEVICE_DEBUG("ADVERT FAIL"); - } - } - } - - _last_report = new_report; - - /* post a report to the ring */ - _reports->force(&new_report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); + _px4_mag.update(timestamp_sample, xraw_f, yraw_f, zraw_f); /* periodically check the configuration @@ -524,7 +278,6 @@ QMC5883::collect() */ check_counter = perf_event_count(_sample_perf) % 256; - if (check_counter == 0) { check_conf(); } @@ -536,15 +289,13 @@ out: return ret; } -int -QMC5883::write_reg(uint8_t reg, uint8_t val) +int QMC5883::write_reg(uint8_t reg, uint8_t val) { uint8_t buf = val; return _interface->write(reg, &buf, 1); } -int -QMC5883::read_reg(uint8_t reg, uint8_t &val) +int QMC5883::read_reg(uint8_t reg, uint8_t &val) { uint8_t buf = val; int ret = _interface->read(reg, &buf, 1); @@ -552,13 +303,11 @@ QMC5883::read_reg(uint8_t reg, uint8_t &val) return ret; } -void -QMC5883::print_status() +void QMC5883::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); printf("interval: %u us\n", _measure_interval); - print_message(_last_report); - _reports->print_info("report queue"); + _px4_mag.print_status(); } diff --git a/src/drivers/magnetometer/qmc5883/QMC5883.hpp b/src/drivers/magnetometer/qmc5883/QMC5883.hpp index e91fa103cf..44ed767cac 100644 --- a/src/drivers/magnetometer/qmc5883/QMC5883.hpp +++ b/src/drivers/magnetometer/qmc5883/QMC5883.hpp @@ -42,35 +42,13 @@ #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 "qmc5883.h" @@ -124,7 +102,7 @@ #define QMC5883_TEMP_OFFSET 50 /* deg celsius */ -class QMC5883 : public device::CDev, public I2CSPIDriver +class QMC5883 : public I2CSPIDriver { public: QMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus, int i2c_address); @@ -136,42 +114,23 @@ public: void RunImpl(); - int init() override; - - ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen) override; - int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; + int init(); protected: void print_status() override; private: - - Device *_interface; + PX4Magnetometer _px4_mag; + device::Device *_interface; unsigned _measure_interval{0}; - ringbuffer::RingBuffer *_reports; - struct mag_calibration_s _scale; - float _range_scale; - float _range_ga; bool _collect_phase; - int _class_instance; - int _orb_class_instance; - - orb_advert_t _mag_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _range_errors; perf_counter_t _conf_errors; - /* status reporting */ - bool _sensor_ok; /**< sensor was found and reports ok */ - - enum Rotation _rotation; - - sensor_mag_s _last_report {}; /**< used for info() */ - - uint8_t _range_bits; uint8_t _conf_reg; uint8_t _temperature_counter; uint8_t _temperature_error_count; @@ -196,7 +155,7 @@ private: * cope with communication errors causing the configuration to * change */ - void check_conf(void); + void check_conf(); /** * Write a register. diff --git a/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp b/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp index 03cba7db45..fb980e212f 100644 --- a/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp +++ b/src/drivers/magnetometer/qmc5883/QMC5883_I2C.cpp @@ -55,11 +55,8 @@ public: virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); - virtual int ioctl(unsigned operation, unsigned &arg); - protected: virtual int probe(); - }; device::Device * @@ -74,28 +71,7 @@ QMC5883_I2C::QMC5883_I2C(int bus, int bus_frequency, int i2c_address) : _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_QMC5883; } -int -QMC5883_I2C::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - - case MAGIOCGEXTERNAL: - return external(); - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: - ret = -EINVAL; - } - - return ret; -} - -int -QMC5883_I2C::probe() +int QMC5883_I2C::probe() { uint8_t data[2] = {0, 0}; @@ -140,8 +116,7 @@ QMC5883_I2C::probe() return -EIO; } -int -QMC5883_I2C::write(unsigned address, void *data, unsigned count) +int QMC5883_I2C::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; @@ -155,8 +130,7 @@ QMC5883_I2C::write(unsigned address, void *data, unsigned count) return transfer(&buf[0], count + 1, nullptr, 0); } -int -QMC5883_I2C::read(unsigned address, void *data, unsigned count) +int QMC5883_I2C::read(unsigned address, void *data, unsigned count) { uint8_t cmd = address; return transfer(&cmd, 1, (uint8_t *)data, count);