From 5f6f1c554b911e756519c56c068efe4e3d78feea Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 30 Mar 2020 15:15:51 -0400 Subject: [PATCH] lis3mdl: move to PX4Magnetometer and cleanup --- .../magnetometer/lis3mdl/CMakeLists.txt | 8 +- src/drivers/magnetometer/lis3mdl/lis3mdl.cpp | 598 ++---------------- src/drivers/magnetometer/lis3mdl/lis3mdl.h | 77 +-- .../magnetometer/lis3mdl/lis3mdl_i2c.cpp | 26 +- .../magnetometer/lis3mdl/lis3mdl_spi.cpp | 37 +- 5 files changed, 58 insertions(+), 688 deletions(-) diff --git a/src/drivers/magnetometer/lis3mdl/CMakeLists.txt b/src/drivers/magnetometer/lis3mdl/CMakeLists.txt index 15a4900bdd..41809d2277 100644 --- a/src/drivers/magnetometer/lis3mdl/CMakeLists.txt +++ b/src/drivers/magnetometer/lis3mdl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Copyright (c) 2016-2020 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,15 +31,15 @@ # ############################################################################ px4_add_module( - MODULE drivers__lis3mdl + MODULE drivers__magnetometer__lis3mdl MAIN lis3mdl COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS lis3mdl_i2c.cpp lis3mdl_spi.cpp lis3mdl_main.cpp lis3mdl.cpp DEPENDS - conversion + drivers_magnetometer + px4_work_queue ) diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp index 2ec1595cd8..0e4d7306c5 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp @@ -43,29 +43,20 @@ #include "lis3mdl.h" LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : - CDev("LIS3MDL", 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, "lis3mdl_comms_errors")), - _conf_errors(perf_alloc(PC_COUNT, "lis3mdl_conf_errors")), - _range_errors(perf_alloc(PC_COUNT, "lis3mdl_range_errors")), - _sample_perf(perf_alloc(PC_ELAPSED, "lis3mdl_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(CONTINUOUS), - _rotation(rotation), _measure_interval(0), - _class_instance(-1), - _orb_class_instance(-1), _range_ga(4.0f), - _range_scale(0), // default range scale from counts to gauss */ _check_state_cnt(0), _cntl_reg1( - CNTL_REG1_DEFAULT), // 1 11 111 0 0 | temp-en, ultra high performance (XY), fast_odr disabled, self test disabled + CNTL_REG1_DEFAULT), // 1 11 111 0 0 | temp-en, ultra high performance (XY), fast_odr disabled, self test disabled _cntl_reg2(CNTL_REG2_DEFAULT), // 4 gauss FS range, reboot settings default _cntl_reg3(CNTL_REG3_DEFAULT), // operating mode CONTINUOUS! _cntl_reg4(CNTL_REG4_DEFAULT), // Z-axis ultra high performance mode @@ -74,35 +65,14 @@ LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOpt _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_LIS3MDL; + _interface->set_device_type(DRV_MAG_DEVTYPE_IST8310); - // 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_IST8310); + _px4_mag.set_external(_interface->external()); } LIS3MDL::~LIS3MDL() { - if (_mag_topic != nullptr) { - orb_unadvertise(_mag_topic); - } - - 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); @@ -110,332 +80,61 @@ LIS3MDL::~LIS3MDL() perf_free(_conf_errors); } -int -LIS3MDL::calibrate(struct file *file_pointer, unsigned enable) +int LIS3MDL::collect() { - sensor_mag_s report; - ssize_t sz; - int ret = 1; - uint8_t num_samples = 10; - - // XXX do something smarter here - int fd = (int)enable; - - float sum_excited[3] = {0.0f, 0.0f, 0.0f}; - float sum_non_excited[3] = {0.0f, 0.0f, 0.0f}; - - /* start the sensor polling at 50 Hz */ - if (ioctl(file_pointer, SENSORIOCSPOLLRATE, 50) != OK) { - warn("FAILED: SENSORIOCSPOLLRATE 50Hz"); - ret = 1; - goto out; - } - - /* Set to 12 Gauss */ - if (ioctl(file_pointer, MAGIOCSRANGE, 12) != OK) { - PX4_WARN("FAILED: MAGIOCSRANGE 12 Ga"); - ret = 1; - goto out; - } - - px4_usleep(20000); - - /* discard 10 samples to let the sensor settle */ - for (uint8_t i = 0; i < num_samples; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = ::poll(&fds, 1, 2000); - - if (ret != 1) { - warn("ERROR: TIMEOUT 1"); - goto out; - } - - /* now go get it */ - sz = ::read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("ERROR: READ 1"); - ret = -EIO; - goto out; - } - } - - /* read the sensor up to 10x */ - for (uint8_t i = 0; i < num_samples; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = ::poll(&fds, 1, 2000); - - if (ret != 1) { - warn("ERROR: TIMEOUT 2"); - goto out; - } - - /* now go get it */ - sz = ::read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("ERROR: READ 2"); - ret = -EIO; - goto out; - } - - sum_non_excited[0] += report.x; - sum_non_excited[1] += report.y; - sum_non_excited[2] += report.z; - } - - sum_non_excited[0] /= num_samples; - sum_non_excited[1] /= num_samples; - sum_non_excited[2] /= num_samples; - - /* excite strap and take measurements */ - if (ioctl(file_pointer, MAGIOCEXSTRAP, 1) != OK) { - PX4_WARN("FAILED: MAGIOCEXSTRAP 1"); - ret = 1; - goto out; - } - - px4_usleep(60000); - - /* discard 10 samples to let the sensor settle */ - for (uint8_t i = 0; i < num_samples; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = ::poll(&fds, 1, 2000); - - if (ret != 1) { - warn("ERROR: TIMEOUT 1"); - goto out; - } - - /* now go get it */ - sz = ::read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("ERROR: READ 1"); - ret = -EIO; - goto out; - } - } - - /* read the sensor up to 10x */ - for (uint8_t i = 0; i < 10; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = ::poll(&fds, 1, 2000); - - if (ret != 1) { - warn("ERROR: TIMEOUT 2"); - goto out; - } - - /* now go get it */ - sz = ::read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("ERROR: READ 2"); - ret = -EIO; - goto out; - } - - sum_excited[0] += report.x; - sum_excited[1] += report.y; - sum_excited[2] += report.z; - } - - sum_excited[0] /= num_samples; - sum_excited[1] /= num_samples; - sum_excited[2] /= num_samples; - - if (1.0f < fabsf(sum_excited[0] - sum_non_excited[0]) && fabsf(sum_excited[0] - sum_non_excited[0]) < 3.0f && - 1.0f < fabsf(sum_excited[1] - sum_non_excited[1]) && fabsf(sum_excited[1] - sum_non_excited[1]) < 3.0f && - 0.1f < fabsf(sum_excited[2] - sum_non_excited[2]) && fabsf(sum_excited[2] - sum_non_excited[2]) < 1.0f) { - ret = OK; - - } else { - ret = -EIO; - goto out; - } - -out: - - /* set back to normal mode */ - set_range(4); - set_default_register_values(); - - px4_usleep(20000); - - return ret; -} - -int -LIS3MDL::check_offset() -{ - bool offset_valid; - - if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { - /* offset is zero */ - offset_valid = false; - - } else { - offset_valid = true; - } - - /* return 0 if calibrated, 1 else */ - return !offset_valid; -} - -int -LIS3MDL::check_scale() -{ - bool scale_valid; - - if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { - /* scale is one */ - scale_valid = false; - - } else { - scale_valid = true; - } - - /* return 0 if calibrated, 1 else */ - return !scale_valid; -} - -int -LIS3MDL::collect() -{ -#pragma pack(push, 1) struct { uint8_t x[2]; uint8_t y[2]; uint8_t z[2]; - } lis_report; + } lis_report{}; struct { int16_t x; int16_t y; int16_t z; int16_t t; - } report; -#pragma pack(pop) + } report{}; - int ret = 0; - uint8_t buf_rx[2] = {0}; + uint8_t buf_rx[2] {}; - 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_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report)); /** * Silicon Bug: the X axis will be read instead of the temperature registers if you do a sequential read through XYZ. * The temperature registers must be addressed directly. */ - ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx)); + + int ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx)); if (ret != OK) { + perf_end(_sample_perf); perf_count(_comms_errors); PX4_WARN("Register read error."); return ret; } + perf_end(_sample_perf); + report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]); report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]); report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]); report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]); - float temperature = report.t; - new_mag_report.temperature = 25.0f + (temperature / 8.0f); + float temperature = 25.0f + (report.t / 8.0f); + _px4_mag.set_temperature(temperature); - // XXX revisit for SPI part, might require a bus type IOCTL + _px4_mag.update(timestamp_sample, report.x, report.y, report.z); - unsigned dummy = 0; - sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); - new_mag_report.is_external = !sensor_is_onboard; - - /** - * RAW outputs - */ - new_mag_report.x_raw = report.x; - new_mag_report.y_raw = report.y; - new_mag_report.z_raw = report.z; - - xraw_f = report.x; - yraw_f = report.y; - zraw_f = report.z; - - // 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; - /* flip axes and negate value for y */ - new_mag_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - 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); - - ret = OK; - - perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -LIS3MDL::RunImpl() +void LIS3MDL::RunImpl() { /* _measure_interval == 0 is used as _task_should_exit */ if (_measure_interval == 0) { @@ -444,15 +143,14 @@ LIS3MDL::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) { @@ -461,122 +159,18 @@ LIS3MDL::RunImpl() } } -int -LIS3MDL::init() +int LIS3MDL::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); - _measure_interval = LIS3MDL_CONVERSION_INTERVAL; start(); return PX4_OK; } -int -LIS3MDL::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 = (LIS3MDL_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); - - /* 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: - return set_range(arg); - - 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: - return calibrate(file_pointer, arg); - - case MAGIOCEXSTRAP: - return set_excitement(arg); - - 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 -LIS3MDL::measure() +int LIS3MDL::measure() { int ret = 0; @@ -590,7 +184,6 @@ LIS3MDL::measure() _continuous_mode_set = false; } - if (ret != OK) { perf_count(_comms_errors); } @@ -598,23 +191,18 @@ LIS3MDL::measure() return ret; } -void -LIS3MDL::print_status() +void LIS3MDL::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 -LIS3MDL::reset() +int LIS3MDL::reset() { - int ret = 0; - - ret = set_default_register_values(); + int ret = set_default_register_values(); if (ret != OK) { return PX4_ERROR; @@ -629,64 +217,6 @@ LIS3MDL::reset() return PX4_OK; } -int -LIS3MDL::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 */ - px4_usleep(LIS3MDL_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 LIS3MDL::set_default_register_values() { @@ -699,69 +229,33 @@ LIS3MDL::set_default_register_values() return PX4_OK; } -int -LIS3MDL::set_excitement(unsigned enable) -{ - int ret; - /* arm the excitement strap */ - ret = read_reg(ADDR_CTRL_REG1, _cntl_reg1); - - if (ret != OK) { - perf_count(_comms_errors); - } - - _cntl_reg1 &= ~0x01; // reset previous excitement mode - - if (enable > 0) { - _cntl_reg1 |= 0x01; - } - - ::printf("set_excitement enable=%d cntl1=0x%x\n", (int)enable, (unsigned)_cntl_reg1); - - ret = write_reg(ADDR_CTRL_REG1, _cntl_reg1); - - if (ret != OK) { - perf_count(_comms_errors); - } - - uint8_t conf_reg_ret = 0; - read_reg(ADDR_CTRL_REG1, conf_reg_ret); - - //print_info(); - - return !(_cntl_reg1 == conf_reg_ret); -} - -int -LIS3MDL::set_range(unsigned range) +int LIS3MDL::set_range(unsigned range) { if (range <= 4) { _range_bits = 0x00; - _range_scale = 1.0f / 6842.0f; + _px4_mag.set_scale(1.0f / 6842.0f); _range_ga = 4.0f; } else if (range <= 8) { _range_bits = 0x01; - _range_scale = 1.0f / 3421.0f; + _px4_mag.set_scale(1.0f / 3421.0f); _range_ga = 8.0f; } else if (range <= 12) { _range_bits = 0x02; - _range_scale = 1.0f / 2281.0f; + _px4_mag.set_scale(1.0f / 2281.0f); _range_ga = 12.0f; } else { _range_bits = 0x03; - _range_scale = 1.0f / 1711.0f; + _px4_mag.set_scale(1.0f / 1711.0f); _range_ga = 16.0f; } - int ret = 0; - /* * Send the command to set the range */ - ret = write_reg(ADDR_CTRL_REG2, (_range_bits << 5)); + int ret = write_reg(ADDR_CTRL_REG2, (_range_bits << 5)); if (ret != OK) { perf_count(_comms_errors); @@ -782,20 +276,15 @@ LIS3MDL::set_range(unsigned range) } } -void -LIS3MDL::start() +void LIS3MDL::start() { - /* reset the report ring and state machine */ - _reports->flush(); - set_default_register_values(); /* schedule a cycle to start things */ ScheduleNow(); } -int -LIS3MDL::read_reg(uint8_t reg, uint8_t &val) +int LIS3MDL::read_reg(uint8_t reg, uint8_t &val) { uint8_t buf = val; int ret = _interface->read(reg, &buf, 1); @@ -803,8 +292,7 @@ LIS3MDL::read_reg(uint8_t reg, uint8_t &val) return ret; } -int -LIS3MDL::write_reg(uint8_t reg, uint8_t val) +int LIS3MDL::write_reg(uint8_t reg, uint8_t val) { uint8_t buf = val; return _interface->write(reg, &buf, 1); diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.h b/src/drivers/magnetometer/lis3mdl/lis3mdl.h index e23f671240..8725761da8 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.h +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.h @@ -39,19 +39,13 @@ #pragma once -#include - #include -#include #include #include - -#include -#include #include - -#include +#include #include +#include /** * LIS3MDL internal constants and data structures. @@ -99,8 +93,7 @@ enum OPERATING_MODE { SINGLE }; - -class LIS3MDL : public device::CDev, public I2CSPIDriver +class LIS3MDL : public I2CSPIDriver { public: LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); @@ -114,10 +107,6 @@ public: virtual int init(); - virtual int ioctl(struct file *file_pointer, int cmd, unsigned long arg); - - virtual int read(struct file *file_pointer, char *buffer, size_t buffer_len); - void print_status() override; /** @@ -127,18 +116,10 @@ public: 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; @@ -146,19 +127,13 @@ 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_ga; - float _range_scale; uint8_t _check_state_cnt; uint8_t _cntl_reg1; @@ -170,37 +145,11 @@ private: uint8_t _temperature_counter; uint8_t _temperature_error_count; - - /** - * @brief Performs the on-sensor scale calibration routine. - * - * @note The sensor will continue to provide measurements, these - * will however reflect the uncalibrated sensor state until - * the calibration routine has been completed. - * - * @param enable set to 1 to enable self-test strap, 0 to disable - */ - int calibrate(struct file *file_pointer, unsigned enable); - /** * Collect the result of the most recent measurement. */ int collect(); - /** - * Check the current scale calibration - * - * @return 0 if scale calibration is ok, 1 else - */ - int check_scale(); - - /** - * Check the current offset calibration - * - * @return 0 if offset calibration is ok, 1 else - */ - int check_offset(); - /** * Issue a measurement command. * @@ -221,18 +170,6 @@ private: */ void start(); - /** - * @brief Performs the on-sensor scale calibration routine. - * - * @note The sensor will continue to provide measurements, these - * will however reflect the uncalibrated sensor state until - * the calibration routine has been completed. - * - * @param enable set to 1 to enable self-test positive strap, -1 to enable - * negative strap, 0 to set to normal mode - */ - int set_excitement(unsigned enable); - /** * @brief Sets the sensor internal range to handle at least the argument in Gauss. * @@ -258,8 +195,4 @@ private: */ int write_reg(uint8_t reg, uint8_t val); - /* this class has pointer data members, do not allow copying it */ - LIS3MDL(const LIS3MDL &); - - LIS3MDL operator=(const LIS3MDL &); }; // class LIS3MDL diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp index bfbd63fba8..e0a72d2bc4 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp @@ -63,7 +63,6 @@ public: LIS3MDL_I2C(int bus, int bus_frequency); virtual ~LIS3MDL_I2C() = default; - 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); @@ -87,24 +86,7 @@ LIS3MDL_I2C::LIS3MDL_I2C(int bus, int bus_frequency) : _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL; } -int -LIS3MDL_I2C::ioctl(unsigned operation, unsigned &arg) -{ - switch (operation) { - - case MAGIOCGEXTERNAL: - return external(); - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: - return -EINVAL; - } -} - -int -LIS3MDL_I2C::probe() +int LIS3MDL_I2C::probe() { uint8_t data = 0; @@ -125,15 +107,13 @@ LIS3MDL_I2C::probe() return OK; } -int -LIS3MDL_I2C::read(unsigned address, void *data, unsigned count) +int LIS3MDL_I2C::read(unsigned address, void *data, unsigned count) { uint8_t cmd = address; return transfer(&cmd, 1, (uint8_t *)data, count); } -int -LIS3MDL_I2C::write(unsigned address, void *data, unsigned count) +int LIS3MDL_I2C::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp index 5fbff8467f..a1c5f6a513 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp @@ -67,7 +67,6 @@ public: virtual ~LIS3MDL_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); }; @@ -90,9 +89,7 @@ LIS3MDL_SPI::LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e int LIS3MDL_SPI::init() { - int ret; - - ret = SPI::init(); + int ret = SPI::init(); if (ret != OK) { DEVICE_DEBUG("SPI init failed"); @@ -114,34 +111,7 @@ LIS3MDL_SPI::init() return OK; } -int -LIS3MDL_SPI::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - switch (operation) { - - case MAGIOCGEXTERNAL: - /* - * Even if this sensor is on the external SPI - * bus it is still internal to the autopilot - * assembly, so always return 0 for internal. - */ - return 0; - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: { - ret = -EINVAL; - } - } - - return ret; -} - -int -LIS3MDL_SPI::read(unsigned address, void *data, unsigned count) +int LIS3MDL_SPI::read(unsigned address, void *data, unsigned count) { uint8_t buf[32]; @@ -156,8 +126,7 @@ LIS3MDL_SPI::read(unsigned address, void *data, unsigned count) return ret; } -int -LIS3MDL_SPI::write(unsigned address, void *data, unsigned count) +int LIS3MDL_SPI::write(unsigned address, void *data, unsigned count) { uint8_t buf[32];