From 87200d79548063ebf4b2637b8f3816812367722d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 27 Jul 2019 13:17:18 -0400 Subject: [PATCH] lsm303d driver move to PX4Accelerometer/PX4Magnetometer and cleanup --- src/drivers/imu/lsm303d/CMakeLists.txt | 3 + src/drivers/imu/lsm303d/lsm303d.cpp | 1227 ++++-------------------- 2 files changed, 168 insertions(+), 1062 deletions(-) diff --git a/src/drivers/imu/lsm303d/CMakeLists.txt b/src/drivers/imu/lsm303d/CMakeLists.txt index 355183916a..e76ce8894a 100644 --- a/src/drivers/imu/lsm303d/CMakeLists.txt +++ b/src/drivers/imu/lsm303d/CMakeLists.txt @@ -38,4 +38,7 @@ px4_add_module( -Wno-cast-align # TODO: fix and enable SRCS lsm303d.cpp + DEPENDS + drivers_accelerometer + drivers_magnetometer ) diff --git a/src/drivers/imu/lsm303d/lsm303d.cpp b/src/drivers/imu/lsm303d/lsm303d.cpp index 35dc130436..fb1f3c0261 100644 --- a/src/drivers/imu/lsm303d/lsm303d.cpp +++ b/src/drivers/imu/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 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 @@ -36,29 +36,19 @@ * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. */ -#include -#include #include -#include -#include #include -#include -#include #include #include #include -#include - +#include +#include /* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" -#define LSM303D_DEVICE_PATH_ACCEL_EXT "/dev/lsm303d_accel_ext" -#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" - /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM 0x49 @@ -179,7 +169,6 @@ #define LSM303D_ACCEL_DEFAULT_RATE 800 #define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 #define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define LSM303D_ACCEL_MAX_OUTPUT_RATE 280 #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 @@ -194,121 +183,28 @@ extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } - -class LSM303D_mag; - class LSM303D : public device::SPI, public px4::ScheduledWorkItem { public: - LSM303D(int bus, const char *path, uint32_t device, enum Rotation rotation); + LSM303D(int bus, uint32_t device, enum Rotation rotation); virtual ~LSM303D(); 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); - /** * Diagnostics - print some basic information about the driver. */ void print_info(); - /** - * dump register values - */ - void print_registers(); - - /** - * deliberately trigger an error - */ - void test_error(); - protected: virtual int probe(); - friend class LSM303D_mag; - - virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); - private: - void Run() override; + void Run() override; - LSM303D_mag *_mag; - - unsigned _call_accel_interval; - unsigned _call_mag_interval; - - ringbuffer::RingBuffer *_accel_reports; - ringbuffer::RingBuffer *_mag_reports; - - struct accel_calibration_s _accel_scale; - unsigned _accel_range_m_s2; - float _accel_range_scale; - unsigned _accel_samplerate; - unsigned _accel_onchip_filter_bandwith; - - struct mag_calibration_s _mag_scale; - unsigned _mag_range_ga; - float _mag_range_scale; - unsigned _mag_samplerate; - - orb_advert_t _accel_topic; - int _accel_orb_class_instance; - int _accel_class_instance; - - unsigned _accel_read; - unsigned _mag_read; - - perf_counter_t _accel_sample_perf; - perf_counter_t _mag_sample_perf; - perf_counter_t _bad_registers; - perf_counter_t _bad_values; - perf_counter_t _accel_duplicates; - - uint8_t _register_wait; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - - Integrator _accel_int; - - enum Rotation _rotation; - - // values used to - float _last_accel[3]; - uint8_t _constant_accel_count; - - // last temperature value - float _last_temperature; - - hrt_abstime _mag_last_measure{0}; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset -#define LSM303D_NUM_CHECKED_REGISTERS 8 - static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; - - /** - * Start automatic measurement. - */ void start(); - - /** - * Stop automatic measurement. - */ void stop(); - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ void reset(); /** @@ -324,12 +220,12 @@ private: /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + void measureAccelerometer(); /** * Fetch mag measurements from the sensor and update the report ring. */ - void mag_measure(); + void measureMagnetometer(); /** * Read a register from the LSM303D @@ -393,15 +289,6 @@ private: */ int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); - /** - * Set the driver lowpass filter bandwidth. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * Zero selects the highest bandwidth - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); - /** * Set the LSM303D internal accel sampling frequency. * @@ -422,141 +309,81 @@ private: */ int mag_set_samplerate(unsigned frequency); - /* this class cannot be copied */ - LSM303D(const LSM303D &); - LSM303D operator=(const LSM303D &); + + PX4Accelerometer _px4_accel; + PX4Magnetometer _px4_mag; + + unsigned _call_accel_interval{1000000 / LSM303D_ACCEL_DEFAULT_RATE}; + unsigned _call_mag_interval{1000000 / LSM303D_MAG_DEFAULT_RATE}; + + perf_counter_t _accel_sample_perf; + perf_counter_t _accel_sample_interval_perf; + perf_counter_t _mag_sample_perf; + perf_counter_t _mag_sample_interval_perf; + perf_counter_t _bad_registers; + perf_counter_t _bad_values; + perf_counter_t _accel_duplicates; + + uint8_t _register_wait{0}; + + int16_t _last_accel[3] {}; + uint8_t _constant_accel_count{0}; + + hrt_abstime _mag_last_measure{0}; + + float _last_temperature{0.0f}; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset + static constexpr int LSM303D_NUM_CHECKED_REGISTERS{8}; + uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS] {}; + uint8_t _checked_next{0}; + }; /* list of registers that will be checked in check_registers(). Note that ADDR_WHO_AM_I must be first in the list. */ -const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, - ADDR_CTRL_REG1, - ADDR_CTRL_REG2, - ADDR_CTRL_REG3, - ADDR_CTRL_REG4, - ADDR_CTRL_REG5, - ADDR_CTRL_REG6, - ADDR_CTRL_REG7 - }; - -/** - * Helper class implementing the mag driver node. - */ -class LSM303D_mag : public device::CDev -{ -public: - LSM303D_mag(LSM303D *parent); - ~LSM303D_mag(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - virtual int init(); - -protected: - friend class LSM303D; - - void parent_poll_notify(); -private: - LSM303D *_parent; - - orb_advert_t _mag_topic; - int _mag_orb_class_instance; - int _mag_class_instance; - - void measure(); - - /* this class does not allow copying due to ptr data members */ - LSM303D_mag(const LSM303D_mag &); - LSM303D_mag operator=(const LSM303D_mag &); +static constexpr uint8_t _checked_registers[] = { + ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_CTRL_REG6, + ADDR_CTRL_REG7 }; - -LSM303D::LSM303D(int bus, const char *path, uint32_t device, enum Rotation rotation) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, - 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), +LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) : + SPI("LSM303D", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000), ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _mag(new LSM303D_mag(this)), - _call_accel_interval(0), - _call_mag_interval(0), - _accel_reports(nullptr), - _mag_reports(nullptr), - _accel_scale{}, - _accel_range_m_s2(0.0f), - _accel_range_scale(0.0f), - _accel_samplerate(0), - _accel_onchip_filter_bandwith(0), - _mag_scale{}, - _mag_range_ga(0.0f), - _mag_range_scale(0.0f), - _mag_samplerate(0), - _accel_topic(nullptr), - _accel_orb_class_instance(-1), - _accel_class_instance(-1), - _accel_read(0), - _mag_read(0), - _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_acc_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_reg")), - _bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_val")), - _accel_duplicates(perf_alloc(PC_COUNT, "lsm303d_acc_dupe")), - _register_wait(0), - _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_int(1000000 / LSM303D_ACCEL_MAX_OUTPUT_RATE, true), - _rotation(rotation), - _constant_accel_count(0), - _last_temperature(0), - _checked_next(0) + _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), + _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")), + _accel_sample_interval_perf(perf_alloc(PC_INTERVAL, "lsm303d: acc interval")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: mag_read")), + _mag_sample_interval_perf(perf_alloc(PC_INTERVAL, "lsm303d: mag interval")), + _bad_registers(perf_alloc(PC_COUNT, "lsm303d: bad_reg")), + _bad_values(perf_alloc(PC_COUNT, "lsm303d: bad_val")), + _accel_duplicates(perf_alloc(PC_COUNT, "lsm303d: acc_dupe")) { - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D; - - /* Prime _mag with parents devid. */ - _mag->_device_id.devid = _device_id.devid; - _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; - - // default scale factors - _accel_scale.x_offset = 0.0f; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0.0f; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0.0f; - _accel_scale.z_scale = 1.0f; - - _mag_scale.x_offset = 0.0f; - _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0.0f; - _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0.0f; - _mag_scale.z_scale = 1.0f; + _px4_accel.set_device_type(DRV_ACC_DEVTYPE_LSM303D); + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_LSM303D); } LSM303D::~LSM303D() { - /* make sure we are truly inactive */ + // make sure we are truly inactive stop(); - /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; - } - - if (_mag_reports != nullptr) { - delete _mag_reports; - } - - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - - delete _mag; - - /* delete the perf counter */ + // delete the perf counter perf_free(_accel_sample_perf); + perf_free(_accel_sample_interval_perf); perf_free(_mag_sample_perf); + perf_free(_mag_sample_interval_perf); perf_free(_bad_registers); perf_free(_bad_values); perf_free(_accel_duplicates); @@ -570,55 +397,13 @@ LSM303D::init() if (ret != OK) { PX4_ERR("SPI init failed (%i)", ret); - goto out; - } - - /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - - if (_accel_reports == nullptr) { - goto out; - } - - _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - - if (_mag_reports == nullptr) { - goto out; + return PX4_ERROR; } reset(); - /* do CDev init for the mag device node */ - ret = _mag->init(); + start(); - if (ret != OK) { - PX4_ERR("MAG init failed (%i)", ret); - goto out; - } - - /* fill report structures */ - measure(); - - /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; - _mag_reports->get(&mrp); - - /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); - - - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - - /* advertise sensor topic, measure manually to initialize valid report */ - sensor_accel_s arp; - _accel_reports->get(&arp); - - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); - -out: return ret; } @@ -641,11 +426,11 @@ LSM303D::reset() // ensure the chip doesn't interpret any other bus traffic as I2C disable_i2c(); - /* enable accel*/ - write_checked_reg(ADDR_CTRL_REG1, - REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); + // enable accel + write_checked_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | + REG1_RATE_800HZ_A); - /* enable mag */ + // enable mag write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 @@ -653,7 +438,6 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); - accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); // we setup the anti-alias on-chip filter as 50Hz. We believe // this operates in the analog domain, and is critical for @@ -663,21 +447,16 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); - - _accel_read = 0; - _mag_read = 0; } int LSM303D::probe() { - /* read dummy value to void to clear SPI statemachine on sensor */ - (void)read_reg(ADDR_WHO_AM_I); + // read dummy value to void to clear SPI statemachine on sensor + read_reg(ADDR_WHO_AM_I); - /* verify that the device is attached and functioning */ - bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - - if (success) { + // verify that the device is attached and functioning + if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { _checked_values[0] = WHO_I_AM; return OK; } @@ -685,234 +464,11 @@ LSM303D::probe() return -EIO; } -ssize_t -LSM303D::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_accel_s); - sensor_accel_s *arb = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_call_accel_interval > 0) { - /* - * While there is space in the caller's buffer, and reports, copy them. - */ - while (count--) { - if (_accel_reports->get(arb)) { - ret += sizeof(*arb); - arb++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - measure(); - - /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) { - ret = sizeof(*arb); - } - - return ret; -} - -ssize_t -LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct mag_report); - mag_report *mrb = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_call_mag_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - */ - while (count--) { - if (_mag_reports->get(mrb)) { - ret += sizeof(*mrb); - mrb++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _mag_reports->flush(); - _mag->measure(); - - /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) { - ret = sizeof(*mrb); - } - - return ret; -} - -int -LSM303D::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: - return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / arg; - - /* check against maximum sane rate */ - if (interval < 500) { - return -EINVAL; - } - - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_accel_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - reset(); - return OK; - - case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - - } else { - return -EINVAL; - } - } - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -int -LSM303D::mag_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: - /* 100 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_mag_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / arg; - - /* check against maximum sane rate */ - if (interval < 1000) { - return -EINVAL; - } - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_mag_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - reset(); - return OK; - - case MAGIOCSSCALE: - /* copy scale in */ - memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale)); - return OK; - - case MAGIOCGSCALE: - /* copy scale out */ - memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale)); - return OK; - - case MAGIOCGEXTERNAL: - /* Even if this sensor is on the "external" SPI bus - * it is still fixed to the autopilot assembly, - * so always return 0. - */ - return 0; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - uint8_t LSM303D::read_reg(unsigned reg) { - uint8_t cmd[2]; - + uint8_t cmd[2] {}; cmd[0] = reg | DIR_READ; - cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); @@ -922,7 +478,7 @@ LSM303D::read_reg(unsigned reg) void LSM303D::write_reg(unsigned reg, uint8_t value) { - uint8_t cmd[2]; + uint8_t cmd[2] {}; cmd[0] = reg | DIR_WRITE; cmd[1] = value; @@ -945,9 +501,7 @@ LSM303D::write_checked_reg(unsigned reg, uint8_t value) void LSM303D::modify_reg(unsigned 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_checked_reg(reg, val); @@ -965,27 +519,27 @@ LSM303D::accel_set_range(unsigned max_g) } if (max_g <= 2) { - _accel_range_m_s2 = 2.0f * CONSTANTS_ONE_G; + // accel_range_m_s2 = 2.0f * CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - _accel_range_m_s2 = 4.0f * CONSTANTS_ONE_G; + // accel_range_m_s2 = 4.0f * CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - _accel_range_m_s2 = 6.0f * CONSTANTS_ONE_G; + // accel_range_m_s2 = 6.0f * CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; + // accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - _accel_range_m_s2 = 16.0f * CONSTANTS_ONE_G; + // accel_range_m_s2 = 16.0f * CONSTANTS_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -993,8 +547,9 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; + float accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; + _px4_accel.set_scale(accel_range_scale); modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -1013,22 +568,22 @@ LSM303D::mag_set_range(unsigned max_ga) } if (max_ga <= 2) { - _mag_range_ga = 2; + // mag_range_ga = 2; setbits |= REG6_FULL_SCALE_2GA_M; new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { - _mag_range_ga = 4; + // mag_range_ga = 4; setbits |= REG6_FULL_SCALE_4GA_M; new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { - _mag_range_ga = 8; + // mag_range_ga = 8; setbits |= REG6_FULL_SCALE_8GA_M; new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { - _mag_range_ga = 12; + // mag_range_ga = 12; setbits |= REG6_FULL_SCALE_12GA_M; new_scale_ga_digit = 0.479e-3f; @@ -1036,7 +591,7 @@ LSM303D::mag_set_range(unsigned max_ga) return -EINVAL; } - _mag_range_scale = new_scale_ga_digit; + _px4_mag.set_scale(new_scale_ga_digit); modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -1054,20 +609,20 @@ LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) } if (bandwidth <= 50) { + // accel_onchip_filter_bandwith = 50; setbits |= REG2_AA_FILTER_BW_50HZ_A; - _accel_onchip_filter_bandwith = 50; } else if (bandwidth <= 194) { + // accel_onchip_filter_bandwith = 194; setbits |= REG2_AA_FILTER_BW_194HZ_A; - _accel_onchip_filter_bandwith = 194; } else if (bandwidth <= 362) { + // accel_onchip_filter_bandwith = 362; setbits |= REG2_AA_FILTER_BW_362HZ_A; - _accel_onchip_filter_bandwith = 362; } else if (bandwidth <= 773) { + // accel_onchip_filter_bandwith = 773; setbits |= REG2_AA_FILTER_BW_773HZ_A; - _accel_onchip_filter_bandwith = 773; } else { return -EINVAL; @@ -1078,16 +633,6 @@ LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) return OK; } -int -LSM303D::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) -{ - _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); - _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); - _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); - - return OK; -} - int LSM303D::accel_set_samplerate(unsigned frequency) { @@ -1098,30 +643,35 @@ LSM303D::accel_set_samplerate(unsigned frequency) frequency = 1600; } + int accel_samplerate = 100; + if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; - _accel_samplerate = 100; + accel_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; - _accel_samplerate = 200; + accel_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; - _accel_samplerate = 400; + accel_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; - _accel_samplerate = 800; + accel_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; - _accel_samplerate = 1600; + accel_samplerate = 1600; } else { return -EINVAL; } + _call_accel_interval = 1000000 / accel_samplerate; + _px4_accel.set_sample_rate(accel_samplerate); + modify_reg(ADDR_CTRL_REG1, clearbits, setbits); return OK; @@ -1137,22 +687,26 @@ LSM303D::mag_set_samplerate(unsigned frequency) frequency = 100; } + int mag_samplerate = 100; + if (frequency <= 25) { setbits |= REG5_RATE_25HZ_M; - _mag_samplerate = 25; + mag_samplerate = 25; } else if (frequency <= 50) { setbits |= REG5_RATE_50HZ_M; - _mag_samplerate = 50; + mag_samplerate = 50; } else if (frequency <= 100) { setbits |= REG5_RATE_100HZ_M; - _mag_samplerate = 100; + mag_samplerate = 100; } else { return -EINVAL; } + _call_mag_interval = 1000000 / mag_samplerate; + modify_reg(ADDR_CTRL_REG5, clearbits, setbits); return OK; @@ -1161,45 +715,34 @@ LSM303D::mag_set_samplerate(unsigned frequency) void LSM303D::start() { - /* make sure we are stopped first */ + // make sure we are stopped first stop(); - /* reset the report ring */ - _accel_reports->flush(); - _mag_reports->flush(); - - /* start polling at the specified rate */ - ScheduleOnInterval(_call_accel_interval - LSM303D_TIMER_REDUCTION, 10000); + // start polling at the specified rate + ScheduleOnInterval(_call_accel_interval - LSM303D_TIMER_REDUCTION); } void LSM303D::stop() { ScheduleClear(); - - /* reset internal states */ - memset(_last_accel, 0, sizeof(_last_accel)); - - /* discard unread data in the buffers */ - _accel_reports->flush(); - _mag_reports->flush(); } void LSM303D::Run() { - /* make another measurement */ - measure(); + // make another accel measurement + measureAccelerometer(); if (hrt_elapsed_time(&_mag_last_measure) >= _call_mag_interval) { - mag_measure(); + measureMagnetometer(); } } void LSM303D::check_registers(void) { - uint8_t v; + uint8_t v = 0; if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { /* @@ -1227,10 +770,12 @@ LSM303D::check_registers(void) } void -LSM303D::measure() +LSM303D::measureAccelerometer() { - /* status register and data as read back from the device */ + perf_begin(_accel_sample_perf); + perf_count(_accel_sample_interval_perf); + // status register and data as read back from the device #pragma pack(push, 1) struct { uint8_t cmd; @@ -1238,14 +783,9 @@ LSM303D::measure() int16_t x; int16_t y; int16_t z; - } raw_accel_report; + } raw_accel_report{}; #pragma pack(pop) - sensor_accel_s accel_report; - - /* start the performance counter */ - perf_begin(_accel_sample_perf); - check_registers(); if (_register_wait != 0) { @@ -1257,7 +797,7 @@ LSM303D::measure() } /* fetch data from the sensor */ - memset(&raw_accel_report, 0, sizeof(raw_accel_report)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); @@ -1267,59 +807,16 @@ LSM303D::measure() return; } - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - accel_report.timestamp = hrt_absolute_time(); - - // use the temperature from the last mag reading - accel_report.temperature = _last_temperature; - - // report the error count as the sum of the number of bad - // register reads and bad values. This allows the higher level - // code to decide if it should use this sensor based on - // whether it has had failures - accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - - accel_report.x_raw = raw_accel_report.x; - accel_report.y_raw = raw_accel_report.y; - accel_report.z_raw = raw_accel_report.z; - - float xraw_f = raw_accel_report.x; - float yraw_f = raw_accel_report.y; - float zraw_f = raw_accel_report.z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - /* we have logs where the accelerometers get stuck at a fixed large value. We want to detect this and mark the sensor as being faulty */ - if (fabsf(_last_accel[0] - x_in_new) < 0.001f && - fabsf(_last_accel[1] - y_in_new) < 0.001f && - fabsf(_last_accel[2] - z_in_new) < 0.001f && - fabsf(x_in_new) > 20 && - fabsf(y_in_new) > 20 && - fabsf(z_in_new) > 20) { - _constant_accel_count += 1; + if (((_last_accel[0] - raw_accel_report.x) == 0) && + ((_last_accel[1] - raw_accel_report.y) == 0) && + ((_last_accel[2] - raw_accel_report.z) == 0)) { + + _constant_accel_count++; } else { _constant_accel_count = 0; @@ -1333,52 +830,34 @@ LSM303D::measure() // we'll still give the data so that it can be logged // and viewed perf_count(_bad_values); + _constant_accel_count = 0; + + perf_end(_accel_sample_perf); + return; } - _last_accel[0] = x_in_new; - _last_accel[1] = y_in_new; - _last_accel[2] = z_in_new; + // report the error count as the sum of the number of bad + // register reads and bad values. This allows the higher level + // code to decide if it should use this sensor based on + // whether it has had failures + _px4_accel.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values)); + _px4_accel.update(timestamp_sample, raw_accel_report.x, raw_accel_report.y, raw_accel_report.z); - accel_report.x = _accel_filter_x.apply(x_in_new); - accel_report.y = _accel_filter_y.apply(y_in_new); - accel_report.z = _accel_filter_z.apply(z_in_new); + _last_accel[0] = raw_accel_report.x; + _last_accel[1] = raw_accel_report.y; + _last_accel[2] = raw_accel_report.z; - matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); - matrix::Vector3f aval_integrated; - - bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); - accel_report.x_integral = aval_integrated(0); - accel_report.y_integral = aval_integrated(1); - accel_report.z_integral = aval_integrated(2); - - accel_report.scaling = _accel_range_scale; - - /* return device ID */ - accel_report.device_id = _device_id.devid; - - _accel_reports->force(&accel_report); - - /* notify anyone waiting for data */ - if (accel_notify) { - poll_notify(POLLIN); - - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); - } - } - - _accel_read++; - - /* stop the perf counter */ perf_end(_accel_sample_perf); } void -LSM303D::mag_measure() +LSM303D::measureMagnetometer() { - /* status register and data as read back from the device */ + perf_begin(_mag_sample_perf); + perf_count(_mag_sample_interval_perf); + + // status register and data as read back from the device #pragma pack(push, 1) struct { uint8_t cmd; @@ -1387,92 +866,41 @@ LSM303D::mag_measure() int16_t x; int16_t y; int16_t z; - } raw_mag_report; + } raw_mag_report{}; #pragma pack(pop) - mag_report mag_report {}; - - /* start the performance counter */ - perf_begin(_mag_sample_perf); - - /* fetch data from the sensor */ - memset(&raw_mag_report, 0, sizeof(raw_mag_report)); + // fetch data from the sensor + const hrt_abstime timestamp_sample = hrt_absolute_time(); raw_mag_report.cmd = ADDR_OUT_TEMP_L | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - mag_report.timestamp = hrt_absolute_time(); - - _mag_last_measure = mag_report.timestamp; - - mag_report.is_external = external(); - - mag_report.x_raw = raw_mag_report.x; - mag_report.y_raw = raw_mag_report.y; - mag_report.z_raw = raw_mag_report.z; - - float xraw_f = mag_report.x_raw; - float yraw_f = mag_report.y_raw; - float zraw_f = mag_report.z_raw; - - /* apply user specified rotation */ - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.scaling = _mag_range_scale; - mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - /* remember the temperature. The datasheet isn't clear, but it * seems to be a signed offset from 25 degrees C in units of 0.125C */ - _last_temperature = 25 + (raw_mag_report.temperature * 0.125f); - mag_report.temperature = _last_temperature; - mag_report.device_id = _mag->_device_id.devid; + _last_temperature = 25.0f + (raw_mag_report.temperature * 0.125f); + _px4_accel.set_temperature(_last_temperature); + _px4_mag.set_temperature(_last_temperature); - _mag_reports->force(&mag_report); + _px4_mag.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values)); + _px4_mag.set_external(external()); + _px4_mag.update(timestamp_sample, raw_mag_report.x, raw_mag_report.y, raw_mag_report.z); - /* notify anyone waiting for data */ - poll_notify(POLLIN); + _mag_last_measure = timestamp_sample; - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); - } - - _mag_read++; - - /* stop the perf counter */ perf_end(_mag_sample_perf); } void LSM303D::print_info() { - printf("accel reads: %u\n", _accel_read); - printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_accel_sample_interval_perf); perf_print_counter(_mag_sample_perf); + perf_print_counter(_mag_sample_interval_perf); perf_print_counter(_bad_registers); perf_print_counter(_bad_values); perf_print_counter(_accel_duplicates); - _accel_reports->print_info("accel reports"); - _mag_reports->print_info("mag reports"); + ::printf("checked_next: %u\n", _checked_next); for (uint8_t i = 0; i < LSM303D_NUM_CHECKED_REGISTERS; i++) { @@ -1485,137 +913,6 @@ LSM303D::print_info() (unsigned)_checked_values[i]); } } - - ::printf("temperature: %.2f\n", (double)_last_temperature); -} - -void -LSM303D::print_registers() -{ - const struct { - uint8_t reg; - const char *name; - } regmap[] = { - { ADDR_WHO_AM_I, "WHO_AM_I" }, - { 0x02, "I2C_CONTROL1" }, - { 0x15, "I2C_CONTROL2" }, - { ADDR_STATUS_A, "STATUS_A" }, - { ADDR_STATUS_M, "STATUS_M" }, - { ADDR_CTRL_REG0, "CTRL_REG0" }, - { ADDR_CTRL_REG1, "CTRL_REG1" }, - { ADDR_CTRL_REG2, "CTRL_REG2" }, - { ADDR_CTRL_REG3, "CTRL_REG3" }, - { ADDR_CTRL_REG4, "CTRL_REG4" }, - { ADDR_CTRL_REG5, "CTRL_REG5" }, - { ADDR_CTRL_REG6, "CTRL_REG6" }, - { ADDR_CTRL_REG7, "CTRL_REG7" }, - { ADDR_OUT_TEMP_L, "TEMP_L" }, - { ADDR_OUT_TEMP_H, "TEMP_H" }, - { ADDR_INT_CTRL_M, "INT_CTRL_M" }, - { ADDR_INT_SRC_M, "INT_SRC_M" }, - { ADDR_REFERENCE_X, "REFERENCE_X" }, - { ADDR_REFERENCE_Y, "REFERENCE_Y" }, - { ADDR_REFERENCE_Z, "REFERENCE_Z" }, - { ADDR_OUT_X_L_A, "ACCEL_XL" }, - { ADDR_OUT_X_H_A, "ACCEL_XH" }, - { ADDR_OUT_Y_L_A, "ACCEL_YL" }, - { ADDR_OUT_Y_H_A, "ACCEL_YH" }, - { ADDR_OUT_Z_L_A, "ACCEL_ZL" }, - { ADDR_OUT_Z_H_A, "ACCEL_ZH" }, - { ADDR_FIFO_CTRL, "FIFO_CTRL" }, - { ADDR_FIFO_SRC, "FIFO_SRC" }, - { ADDR_IG_CFG1, "IG_CFG1" }, - { ADDR_IG_SRC1, "IG_SRC1" }, - { ADDR_IG_THS1, "IG_THS1" }, - { ADDR_IG_DUR1, "IG_DUR1" }, - { ADDR_IG_CFG2, "IG_CFG2" }, - { ADDR_IG_SRC2, "IG_SRC2" }, - { ADDR_IG_THS2, "IG_THS2" }, - { ADDR_IG_DUR2, "IG_DUR2" }, - { ADDR_CLICK_CFG, "CLICK_CFG" }, - { ADDR_CLICK_SRC, "CLICK_SRC" }, - { ADDR_CLICK_THS, "CLICK_THS" }, - { ADDR_TIME_LIMIT, "TIME_LIMIT" }, - { ADDR_TIME_LATENCY, "TIME_LATENCY" }, - { ADDR_TIME_WINDOW, "TIME_WINDOW" }, - { ADDR_ACT_THS, "ACT_THS" }, - { ADDR_ACT_DUR, "ACT_DUR" } - }; - - for (uint8_t i = 0; i < sizeof(regmap) / sizeof(regmap[0]); i++) { - printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name); - } -} - -void -LSM303D::test_error() -{ - // trigger an error - write_reg(ADDR_CTRL_REG3, 0); -} - -LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), - _parent(parent), - _mag_topic(nullptr), - _mag_orb_class_instance(-1), - _mag_class_instance(-1) -{ -} - -LSM303D_mag::~LSM303D_mag() -{ - if (_mag_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); - } -} - -int -LSM303D_mag::init() -{ - int ret; - - ret = CDev::init(); - - if (ret != OK) { - goto out; - } - - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - -out: - return ret; -} - -void -LSM303D_mag::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -ssize_t -LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen) -{ - return _parent->mag_read(filp, buffer, buflen); -} - -int -LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case DEVIOCGDEVICEID: - return (int)CDev::ioctl(filp, cmd, arg); - break; - - default: - return _parent->mag_ioctl(filp, cmd, arg); - } -} - -void -LSM303D_mag::measure() -{ - _parent->mag_measure(); } /** @@ -1627,12 +924,8 @@ namespace lsm303d LSM303D *g_dev; void start(bool external_bus, enum Rotation rotation, unsigned range); -void test(); -void reset(); void info(); -void regdump(); void usage(); -void test_error(); /** * Start the driver. @@ -1643,8 +936,6 @@ void test_error(); void start(bool external_bus, enum Rotation rotation, unsigned range) { - int fd, fd_mag; - if (g_dev != nullptr) { errx(0, "already started"); } @@ -1652,13 +943,13 @@ start(bool external_bus, enum Rotation rotation, unsigned range) /* create the driver */ if (external_bus) { #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) - g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); + g_dev = new LSM303D(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); #else errx(0, "External SPI not available"); #endif } else { - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, PX4_SPIDEV_ACCEL_MAG, rotation); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation); } if (g_dev == nullptr) { @@ -1670,29 +961,6 @@ start(bool external_bus, enum Rotation rotation, unsigned range) goto fail; } - /* set the poll rate to default, starts automatic data collection */ - fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); - - /* don't fail if open cannot be opened */ - if (0 <= fd_mag) { - if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - } - - close(fd); - close(fd_mag); - exit(0); fail: @@ -1704,112 +972,6 @@ fail: errx(1, "driver start failed"); } -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - int fd_accel = -1; - sensor_accel_s accel_report; - ssize_t sz; - int ret; - - /* get the driver */ - fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd_accel < 0) { - err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); - } - - /* do a simple demand read */ - sz = read(fd_accel, &accel_report, sizeof(sensor_accel_s)); - - if (sz != sizeof(sensor_accel_s)) { - err(1, "immediate read failed"); - } - - print_message(accel_report); - - int fd_mag = -1; - struct mag_report m_report; - - /* get the driver */ - fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); - - if (fd_mag < 0) { - err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); - } - - /* check if mag is onboard or external */ - if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) { - errx(1, "failed to get if mag is onboard or external"); - } - - PX4_INFO("mag device active: %s", ret ? "external" : "onboard"); - - /* do a simple demand read */ - sz = read(fd_mag, &m_report, sizeof(m_report)); - - if (sz != sizeof(m_report)) { - err(1, "immediate read failed"); - } - - print_message(m_report); - - /* reset to default polling */ - if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "reset to default polling"); - } - - close(fd_accel); - close(fd_mag); - - reset(); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd < 0) { - err(1, "failed "); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "accel pollrate reset failed"); - } - - close(fd); - - fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); - - if (fd < 0) { - PX4_WARN("mag could not be opened, external mag might be used"); - - } else { - /* no need to reset the mag as well, the reset() is the same */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "mag pollrate reset failed"); - } - } - - close(fd); - - exit(0); -} - /** * Print a little info about the driver. */ @@ -1826,41 +988,10 @@ info() exit(0); } -/** - * dump registers from device - */ -void -regdump() -{ - if (g_dev == nullptr) { - errx(1, "driver not running\n"); - } - - printf("regdump @ %p\n", g_dev); - g_dev->print_registers(); - - exit(0); -} - -/** - * trigger an error - */ -void -test_error() -{ - if (g_dev == nullptr) { - errx(1, "driver not running\n"); - } - - g_dev->test_error(); - - exit(0); -} - void usage() { - PX4_INFO("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); + PX4_INFO("missing command: try 'start', 'info'"); PX4_INFO("options:"); PX4_INFO(" -X (external bus)"); PX4_INFO(" -R rotation"); @@ -1915,20 +1046,6 @@ lsm303d_main(int argc, char *argv[]) lsm303d::start(external_bus, rotation, accel_range); } - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - lsm303d::test(); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - lsm303d::reset(); - } - /* * Print driver information. */ @@ -1936,19 +1053,5 @@ lsm303d_main(int argc, char *argv[]) lsm303d::info(); } - /* - * dump device registers - */ - if (!strcmp(verb, "regdump")) { - lsm303d::regdump(); - } - - /* - * trigger an error - */ - if (!strcmp(verb, "testerror")) { - lsm303d::test_error(); - } - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); + errx(1, "unrecognized command, try 'start', info'"); }