From cf12969a2979851260f8c01c7b439b779aaf01f5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 30 Mar 2020 15:15:34 -0400 Subject: [PATCH] ist8310: move to PX4Magnetometer and cleanup --- .../magnetometer/ist8310/CMakeLists.txt | 3 +- src/drivers/magnetometer/ist8310/ist8310.cpp | 406 ++---------------- 2 files changed, 41 insertions(+), 368 deletions(-) diff --git a/src/drivers/magnetometer/ist8310/CMakeLists.txt b/src/drivers/magnetometer/ist8310/CMakeLists.txt index a2bfd23057..67f24cffca 100644 --- a/src/drivers/magnetometer/ist8310/CMakeLists.txt +++ b/src/drivers/magnetometer/ist8310/CMakeLists.txt @@ -33,10 +33,9 @@ px4_add_module( MODULE drivers__ist8310 MAIN ist8310 - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS ist8310.cpp DEPENDS + drivers_magnetometer px4_work_queue ) diff --git a/src/drivers/magnetometer/ist8310/ist8310.cpp b/src/drivers/magnetometer/ist8310/ist8310.cpp index 574aefe103..1161895f68 100644 --- a/src/drivers/magnetometer/ist8310/ist8310.cpp +++ b/src/drivers/magnetometer/ist8310/ist8310.cpp @@ -46,40 +46,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 -#include - -#include -#include - - /* * IST8310 internal constants and data structures. */ @@ -188,9 +161,6 @@ public: virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - /** * Initialise the automatic measurement state machine and start it. * @@ -219,35 +189,22 @@ public: */ void RunImpl(); -protected: +private: int probe() override; void print_status() override; -private: + PX4Magnetometer _px4_mag; unsigned _measure_interval{IST8310_CONVERSION_INTERVAL}; - ringbuffer::RingBuffer *_reports{nullptr}; - - struct mag_calibration_s _scale {}; - float _range_scale{0.003f}; /* default range scale from counts to gauss */ - bool _collect_phase{false}; - int _class_instance{-1}; - int _orb_class_instance{-1}; - - orb_advert_t _mag_topic{nullptr}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _range_errors; perf_counter_t _conf_errors; - enum Rotation _rotation; - - sensor_mag_s _last_report{}; /**< used for info() */ - uint8_t _ctl3_reg{0}; uint8_t _ctl4_reg{0}; @@ -258,7 +215,7 @@ private: * cope with communication errors causing the configuration to * change */ - void check_conf(void); + void check_conf(); /** * Write a register. @@ -309,61 +266,28 @@ private: * Collect the result of the most recent measurement. */ int collect(); - - /** - * Convert a big-endian signed 16-bit value to a float. - * - * @param in A signed 16-bit big-endian value. - * @return The floating-point representation of the value. - */ - float meas_to_float(uint8_t in[2]); - - /** - * Place the device in self test mode - * - * @return 0 if mode is set, 1 else - */ - int set_selftest(unsigned enable); - - /* this class has pointer data members, do not allow copying it */ - IST8310(const IST8310 &); - IST8310 operator=(const IST8310 &); }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ist8310_main(int argc, char *argv[]); - - IST8310::IST8310(I2CSPIBusOption bus_option, int bus_number, int address, enum Rotation rotation, int bus_frequency) : I2C("IST8310", nullptr, bus_number, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus_number, address), - _sample_perf(perf_alloc(PC_ELAPSED, "ist8310_read")), - _comms_errors(perf_alloc(PC_COUNT, "ist8310_com_err")), - _range_errors(perf_alloc(PC_COUNT, "ist8310_rng_err")), - _conf_errors(perf_alloc(PC_COUNT, "ist8310_conf_err")), - _rotation(rotation) + _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _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")) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_IST8310; + 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(external()); + + // default range scale from counts to gauss + _px4_mag.set_scale(0.003f); } IST8310::~IST8310() { - 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); @@ -371,37 +295,24 @@ IST8310::~IST8310() perf_free(_conf_errors); } -int -IST8310::init() +int IST8310::init() { - int ret = PX4_ERROR; - - ret = I2C::init(); + int ret = I2C::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) { + DEVICE_DEBUG("I2C init failed"); goto out; } /* reset the device configuration */ reset(); - _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - ret = OK; out: return ret; } -int -IST8310::write(unsigned address, void *data, unsigned count) +int IST8310::write(unsigned address, void *data, unsigned count) { uint8_t buf[32]; @@ -415,20 +326,18 @@ IST8310::write(unsigned address, void *data, unsigned count) return transfer(&buf[0], count + 1, nullptr, 0); } -int -IST8310::read(unsigned address, void *data, unsigned count) +int IST8310::read(unsigned address, void *data, unsigned count) { uint8_t cmd = address; return transfer(&cmd, 1, (uint8_t *)data, count); } - /** check that the configuration register has the right value. This is done periodically to cope with I2C bus noise causing the configuration of the compass to change. */ -void IST8310::check_conf(void) +void IST8310::check_conf() { int ret; @@ -466,155 +375,16 @@ void IST8310::check_conf(void) } } -ssize_t -IST8310::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(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - usleep(IST8310_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 -IST8310::ioctl(struct file *filp, int cmd, unsigned long 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 = IST8310_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 tick interval via microseconds */ - unsigned interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < IST8310_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 MAGIOCEXSTRAP: - return set_selftest(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 MAGIOCGEXTERNAL: - return external(); - - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); - } -} - -void -IST8310::start() +void IST8310::start() { /* reset the report ring and state machine */ _collect_phase = false; - _reports->flush(); /* schedule a cycle to start things */ ScheduleNow(); } -int -IST8310::reset() +int IST8310::reset() { /* software reset */ write_reg(ADDR_CTRL2, CTRL2_SRST); @@ -630,8 +400,7 @@ IST8310::reset() return OK; } -int -IST8310::probe() +int IST8310::probe() { uint8_t data[1] = {0}; @@ -652,8 +421,7 @@ IST8310::probe() return OK; } -void -IST8310::RunImpl() +void IST8310::RunImpl() { /* collection phase? */ if (_collect_phase) { @@ -693,8 +461,7 @@ IST8310::RunImpl() ScheduleDelayed(IST8310_CONVERSION_INTERVAL); } -int -IST8310::measure() +int IST8310::measure() { /* * Send the command to begin a measurement. @@ -708,37 +475,28 @@ IST8310::measure() return ret; } -int -IST8310::collect() +int IST8310::collect() { -#pragma pack(push, 1) struct { /* status register and data as read back from the device */ uint8_t x[2]; uint8_t y[2]; uint8_t z[2]; - } report_buffer; -#pragma pack(pop) + } report_buffer{}; + struct { int16_t x, y, z; - } report; + } report{}; int ret; uint8_t check_counter; perf_begin(_sample_perf); - sensor_mag_s new_report; - const bool sensor_is_external = external(); 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.is_external = sensor_is_external; - 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 @@ -748,6 +506,7 @@ IST8310::collect() */ /* get measurements from the device */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); ret = read(ADDR_DATA_OUT_X_LSB, (uint8_t *)&report_buffer, sizeof(report_buffer)); if (ret != OK) { @@ -761,7 +520,6 @@ IST8310::collect() report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0]; report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0]; - /* * Check if value makes sense according to the FSR and Resolution of * this sensor, discarding outliers @@ -774,50 +532,17 @@ IST8310::collect() goto out; } - /* temperature measurement is not available on IST8310 */ - new_report.temperature = 0; - /* * raw outputs * * Sensor doesn't follow right hand rule, swap x and y to make it obey * it. */ - new_report.x_raw = report.y; - new_report.y_raw = report.x; - new_report.z_raw = report.z; - - /* scale values for output */ xraw_f = report.y; 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; - new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; - 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_external ? ORB_PRIO_MAX : ORB_PRIO_HIGH); - - if (_mag_topic == nullptr) { - DEVICE_DEBUG("ADVERT FAIL"); - } - } - } - - _last_report = new_report; - - /* post a report to the ring */ - _reports->force(&new_report); + _px4_mag.update(timestamp_sample, xraw_f, yraw_f, zraw_f); /* periodically check the range register and configuration @@ -839,46 +564,13 @@ out: return ret; } -int -IST8310::set_selftest(unsigned enable) -{ - int ret; - uint8_t str; - /* arm the excitement strap */ - ret = read_reg(ADDR_STR, str); - - if (OK != ret) { - perf_count(_comms_errors); - } - - str &= ~STR_SELF_TEST_ON; // reset previous test - - if (enable > 0) { - str |= STR_SELF_TEST_ON; - - } - - ret = write_reg(ADDR_STR, str); - - if (OK != ret) { - perf_count(_comms_errors); - } - - uint8_t str_reg_ret = 0; - read_reg(ADDR_STR, str_reg_ret); - - return !(str == str_reg_ret); -} - -int -IST8310::write_reg(uint8_t reg, uint8_t val) +int IST8310::write_reg(uint8_t reg, uint8_t val) { uint8_t buf = val; return write(reg, &buf, 1); } -int -IST8310::read_reg(uint8_t reg, uint8_t &val) +int IST8310::read_reg(uint8_t reg, uint8_t &val) { uint8_t buf = 0; int ret = read(reg, &buf, 1); @@ -886,29 +578,13 @@ IST8310::read_reg(uint8_t reg, uint8_t &val) return ret; } -float -IST8310::meas_to_float(uint8_t in[2]) -{ - union { - uint8_t b[2]; - int16_t w; - } u; - - u.b[0] = in[1]; - u.b[1] = in[0]; - - return (float) u.w; -} - -void -IST8310::print_status() +void IST8310::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); printf("poll interval: %u interval\n", _measure_interval); - print_message(_last_report); - _reports->print_info("report queue"); + _px4_mag.print_status(); } I2CSPIDriverBase * @@ -933,8 +609,7 @@ IST8310::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iter return interface; } -void -IST8310::print_usage() +void IST8310::print_usage() { PRINT_MODULE_USAGE_NAME("ist8310", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); @@ -945,8 +620,7 @@ IST8310::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -ist8310_main(int argc, char *argv[]) +extern "C" __EXPORT int ist8310_main(int argc, char *argv[]) { int ch; using ThisDriver = IST8310;