diff --git a/src/drivers/magnetometer/rm3100/CMakeLists.txt b/src/drivers/magnetometer/rm3100/CMakeLists.txt index f464153c49..7f3f41d191 100644 --- a/src/drivers/magnetometer/rm3100/CMakeLists.txt +++ b/src/drivers/magnetometer/rm3100/CMakeLists.txt @@ -34,10 +34,12 @@ px4_add_module( MODULE drivers__rm3100 MAIN rm3100 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS rm3100_i2c.cpp rm3100_spi.cpp rm3100_main.cpp rm3100.cpp + DEPENDS + drivers_magnetometer + px4_work_queue ) diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index 47caae5b03..6f7d3b9fe4 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -42,52 +42,28 @@ #include "rm3100.h" RM3100::RM3100(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : - CDev("RM3100", nullptr), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), + _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), _interface(interface), - _reports(nullptr), - _scale{}, - _last_report{}, - _mag_topic(nullptr), - _comms_errors(perf_alloc(PC_COUNT, "rm3100_comms_errors")), - _conf_errors(perf_alloc(PC_COUNT, "rm3100_conf_errors")), - _range_errors(perf_alloc(PC_COUNT, "rm3100_range_errors")), - _sample_perf(perf_alloc(PC_ELAPSED, "rm3100_read")), - _calibrated(false), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), + _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), + _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _continuous_mode_set(false), _mode(SINGLE), - _rotation(rotation), _measure_interval(0), - _class_instance(-1), - _orb_class_instance(-1), - _range_scale(1.0f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS)), _check_state_cnt(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_RM3100; + _interface->set_device_type(DRV_MAG_DEVTYPE_RM3100); - // 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_RM3100); + _px4_mag.set_external(_interface->external()); + + _px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS)); } RM3100::~RM3100() { - 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); @@ -95,8 +71,7 @@ RM3100::~RM3100() perf_free(_conf_errors); } -int -RM3100::self_test() +int RM3100::self_test() { /* Chances are that a poll event was triggered, so wait for conversion and read registers in order to clear DRDY bit */ usleep(RM3100_CONVERSION_INTERVAL); @@ -141,13 +116,10 @@ RM3100::self_test() return ret; } -int -RM3100::check_measurement() +int RM3100::check_measurement() { uint8_t status = 0; - int ret = -1; - - ret = _interface->read(ADDR_STATUS, &status, 1); + int ret = _interface->read(ADDR_STATUS, &status, 1); if (ret != 0) { return ret; @@ -156,120 +128,55 @@ RM3100::check_measurement() return !((status & STATUS_DRDY) == STATUS_DRDY) ; } -int -RM3100::collect() +int RM3100::collect() { /* Check whether a measurement is available or not, otherwise return immediately */ if (check_measurement() != 0) { - DEVICE_DEBUG("No measurement available"); + PX4_DEBUG("No measurement available"); return 0; } -#pragma pack(push, 1) struct { uint8_t x[3]; uint8_t y[3]; uint8_t z[3]; - } rm_report; -#pragma pack(pop) + } rm_report{}; - int ret = 0; - - int32_t xraw; - int32_t yraw; - int32_t zraw; - - float xraw_f; - float yraw_f; - float zraw_f; - - sensor_mag_s new_mag_report; - bool sensor_is_onboard = false; + _px4_mag.set_error_count(perf_event_count(_comms_errors)); perf_begin(_sample_perf); - new_mag_report.timestamp = hrt_absolute_time(); - new_mag_report.error_count = perf_event_count(_comms_errors); - new_mag_report.scaling = _range_scale; - new_mag_report.device_id = _device_id.devid; - - ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report)); if (ret != OK) { + perf_end(_sample_perf); perf_count(_comms_errors); PX4_WARN("Register read error."); return ret; } + perf_end(_sample_perf); + /* Rearrange mag data */ - xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]); - yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]); - zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]); + int32_t xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]); + int32_t yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]); + int32_t zraw = ((rm_report.z[0] << 16) | (rm_report.z[1] << 8) | rm_report.z[2]); /* Convert 24 bit signed values to 32 bit signed values */ convert_signed(&xraw); convert_signed(&yraw); convert_signed(&zraw); - /* There is no temperature sensor */ - new_mag_report.temperature = 0.0f; - - // XXX revisit for SPI part, might require a bus type IOCTL - unsigned dummy = 0; - sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); - new_mag_report.is_external = !sensor_is_onboard; - - /** - * RAW outputs - * As we only have 16 bits to store raw data, the following values are not correct - */ - new_mag_report.x_raw = (int16_t)(xraw >> 8); - new_mag_report.y_raw = (int16_t)(yraw >> 8); - new_mag_report.z_raw = (int16_t)(zraw >> 8); - - xraw_f = xraw; - yraw_f = yraw; - zraw_f = zraw; - - /* apply user specified rotation */ - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - new_mag_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; - new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; - new_mag_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_mag_report); - - } else { - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_mag_report, - &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); - - if (_mag_topic == nullptr) { - DEVICE_DEBUG("ADVERT FAIL"); - } - } - } - - _last_report = new_mag_report; - - /* post a report to the ring */ - _reports->force(&new_mag_report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); + _px4_mag.update(timestamp_sample, xraw, yraw, zraw); ret = OK; - perf_end(_sample_perf); + return ret; } -void -RM3100::convert_signed(int32_t *n) +void RM3100::convert_signed(int32_t *n) { /* Sensor returns values as 24 bit signed values, so we need to manually convert to 32 bit signed values */ if ((*n & (1 << 23)) == (1 << 23)) { @@ -277,8 +184,7 @@ RM3100::convert_signed(int32_t *n) } } -void -RM3100::RunImpl() +void RM3100::RunImpl() { /* _measure_interval == 0 is used as _task_should_exit */ if (_measure_interval == 0) { @@ -287,15 +193,14 @@ RM3100::RunImpl() /* Collect last measurement at the start of every cycle */ if (collect() != OK) { - DEVICE_DEBUG("collection error"); + PX4_DEBUG("collection error"); /* restart the measurement state machine */ start(); return; } - if (measure() != OK) { - DEVICE_DEBUG("measure error"); + PX4_DEBUG("measure error"); } if (_measure_interval > 0) { @@ -304,31 +209,12 @@ RM3100::RunImpl() } } -int -RM3100::init() +int RM3100::init() { - int ret = PX4_ERROR; - - ret = CDev::init(); - - if (ret != OK) { - DEVICE_DEBUG("CDev init failed"); - return ret; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s)); - - if (_reports == nullptr) { - return PX4_ERROR; - } - /* reset the device configuration */ reset(); - _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - - ret = self_test(); + int ret = self_test(); if (ret != PX4_OK) { PX4_ERR("self test failed"); @@ -340,97 +226,7 @@ RM3100::init() return ret; } -int -RM3100::ioctl(struct file *file_pointer, int cmd, unsigned long arg) -{ - unsigned dummy = 0; - - switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool not_started = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = (RM3100_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (not_started) { - start(); - } - - return PX4_OK; - } - - /* Uses arg (hz) for a custom poll rate */ - default: { - /* do we need to start internal polling? */ - bool not_started = (_measure_interval == 0); - - /* convert hz to tick interval via microseconds */ - unsigned interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < RM3100_CONVERSION_INTERVAL) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (not_started) { - start(); - } - - return PX4_OK; - } - } - } - - case SENSORIOCRESET: - return reset(); - - case MAGIOCSRANGE: - /* field measurement range cannot be configured for this sensor (8 Gauss) */ - 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 MAGIOCCALIBRATE: - /* This is left for compatibility with the IOCTL call in mag calibration */ - return OK; - - case MAGIOCGEXTERNAL: - DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); - return _interface->ioctl(cmd, dummy); - - case DEVIOCGDEVICEID: - return _interface->ioctl(cmd, dummy); - - default: - /* give it to the superclass */ - return CDev::ioctl(file_pointer, cmd, arg); - } -} - -int -RM3100::measure() +int RM3100::measure() { int ret = 0; uint8_t cmd = 0; @@ -467,23 +263,18 @@ RM3100::measure() return ret; } -void -RM3100::print_status() +void RM3100::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); PX4_INFO("poll interval: %u", _measure_interval); - print_message(_last_report); - _reports->print_info("report queue"); + _px4_mag.print_status(); } -int -RM3100::reset() +int RM3100::reset() { - int ret = 0; - - ret = set_default_register_values(); + int ret = set_default_register_values(); if (ret != OK) { return PX4_ERROR; @@ -492,66 +283,7 @@ RM3100::reset() return PX4_OK; } -int -RM3100::read(struct file *file_pointer, char *buffer, size_t buffer_len) -{ - unsigned count = buffer_len / 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(); - - /* trigger a measurement */ - if (measure() != OK) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(RM3100_CONVERSION_INTERVAL); - - /* run the collection phase */ - if (collect() != OK) { - ret = -EIO; - break; - } - - if (_reports->get(mag_buf)) { - ret = sizeof(sensor_mag_s); - } - } while (0); - - return ret; -} - -int -RM3100::set_default_register_values() +int RM3100::set_default_register_values() { uint8_t cmd[2] = {0, 0}; @@ -579,15 +311,10 @@ RM3100::set_default_register_values() return PX4_OK; } -void -RM3100::start() +void RM3100::start() { - /* reset the report ring and state machine */ - _reports->flush(); - set_default_register_values(); /* schedule a cycle to start things */ ScheduleNow(); } - diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h index bf64a0455e..4caae1e0f3 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.h +++ b/src/drivers/magnetometer/rm3100/rm3100.h @@ -39,20 +39,13 @@ #pragma once -#include - #include -#include #include #include - -#include - -#include +#include #include -#include - #include +#include /** * RM3100 internal constants and data structures. @@ -105,8 +98,7 @@ enum OPERATING_MODE { SINGLE }; - -class RM3100 : public device::CDev, public I2CSPIDriver +class RM3100 : public I2CSPIDriver { public: RM3100(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); @@ -118,11 +110,7 @@ public: void custom_method(const BusCLIArguments &cli) override; - int init() override; - - int ioctl(struct file *file_pointer, int cmd, unsigned long arg) override; - - int read(struct file *file_pointer, char *buffer, size_t buffer_len) override; + int init(); void print_status() override; @@ -132,18 +120,11 @@ public: int set_default_register_values(); void RunImpl(); -protected: - Device *_interface; private: + PX4Magnetometer _px4_mag; - ringbuffer::RingBuffer *_reports; - - struct mag_calibration_s _scale; - - sensor_mag_s _last_report {}; /**< used for info() */ - - orb_advert_t _mag_topic; + device::Device *_interface; perf_counter_t _comms_errors; perf_counter_t _conf_errors; @@ -151,19 +132,12 @@ private: perf_counter_t _sample_perf; /* status reporting */ - bool _calibrated; /**< the calibration is valid */ bool _continuous_mode_set; enum OPERATING_MODE _mode; - enum Rotation _rotation; unsigned int _measure_interval; - int _class_instance; - int _orb_class_instance; - - float _range_scale; - uint8_t _check_state_cnt; /** @@ -210,8 +184,4 @@ private: */ void start(); - /* this class has pointer data members, do not allow copying it */ - RM3100(const RM3100 &); - - RM3100 operator=(const RM3100 &); }; // class RM3100 diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp index ab4ce9f939..9a16d6fcff 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp @@ -48,8 +48,6 @@ #include #include - - #include #include #include @@ -63,16 +61,13 @@ class RM3100_I2C : public device::I2C { public: RM3100_I2C(int bus, int bus_frequency); - virtual ~RM3100_I2C() = default; + ~RM3100_I2C() override = default; - virtual int init(); - virtual int ioctl(unsigned operation, unsigned &arg); - virtual int read(unsigned address, void *data, unsigned count); - virtual int write(unsigned address, void *data, unsigned count); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; protected: - virtual int probe(); - + int probe() override; }; device::Device * @@ -90,31 +85,7 @@ RM3100_I2C::RM3100_I2C(int bus, int bus_frequency) : _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100; } -int -RM3100_I2C::init() -{ - /* this will call probe() */ - return I2C::init(); -} - -int -RM3100_I2C::ioctl(unsigned operation, unsigned &arg) -{ - switch (operation) { - - case MAGIOCGEXTERNAL: - return external(); - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: - return -EINVAL; - } -} - -int -RM3100_I2C::probe() +int RM3100_I2C::probe() { uint8_t data = 0; @@ -135,8 +106,7 @@ RM3100_I2C::probe() return OK; } -int -RM3100_I2C::read(unsigned address, void *data, unsigned count) +int RM3100_I2C::read(unsigned address, void *data, unsigned count) { uint8_t cmd = address; int ret; @@ -154,8 +124,7 @@ RM3100_I2C::read(unsigned address, void *data, unsigned count) return ret; } -int -RM3100_I2C::write(unsigned address, void *data, unsigned count) +int RM3100_I2C::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; diff --git a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp index a6b503e114..8858c4fad1 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp @@ -66,7 +66,6 @@ public: virtual ~RM3100_SPI() = default; virtual int init(); - virtual int ioctl(unsigned operation, unsigned &arg); virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); }; @@ -86,8 +85,7 @@ RM3100_SPI::RM3100_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e sp _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_RM3100; } -int -RM3100_SPI::init() +int RM3100_SPI::init() { int ret; @@ -113,29 +111,7 @@ RM3100_SPI::init() return OK; } -int -RM3100_SPI::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 -RM3100_SPI::read(unsigned address, void *data, unsigned count) +int RM3100_SPI::read(unsigned address, void *data, unsigned count) { uint8_t buf[32]; @@ -150,8 +126,7 @@ RM3100_SPI::read(unsigned address, void *data, unsigned count) return ret; } -int -RM3100_SPI::write(unsigned address, void *data, unsigned count) +int RM3100_SPI::write(unsigned address, void *data, unsigned count) { uint8_t buf[32];