diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4235ecc6f5..1252f57c09 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -236,7 +236,7 @@ class LSM303D_mag; class LSM303D : public device::SPI { public: - LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation); + LSM303D(int bus, const char *path, spi_dev_e device, enum Rotation rotation); virtual ~LSM303D(); virtual int init(); @@ -497,8 +497,8 @@ private: int mag_set_samplerate(unsigned frequency); /* this class cannot be copied */ - LSM303D(const LSM303D&); - LSM303D operator=(const LSM303D&); + LSM303D(const LSM303D &); + LSM303D operator=(const LSM303D &); }; /* @@ -512,7 +512,8 @@ const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADD ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7 }; + ADDR_CTRL_REG7 + }; /** * Helper class implementing the mag driver node. @@ -544,13 +545,14 @@ private: void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ - LSM303D_mag(const LSM303D_mag&); - LSM303D_mag operator=(const LSM303D_mag&); + LSM303D_mag(const LSM303D_mag &); + LSM303D_mag operator=(const LSM303D_mag &); }; -LSM303D::LSM303D(int bus, const char* path, spi_dev_e 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, const char *path, spi_dev_e 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 */), _mag(new LSM303D_mag(this)), _accel_call{}, _mag_call{}, @@ -621,13 +623,17 @@ LSM303D::~LSM303D() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_mag_reports != nullptr) - delete _mag_reports; + } - if (_accel_class_instance != -1) + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } delete _mag; @@ -653,18 +659,21 @@ LSM303D::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + if (_accel_reports == nullptr) { goto out; + } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_mag_reports == nullptr) + if (_mag_reports == nullptr) { goto out; + } reset(); /* do CDev init for the mag device node */ ret = _mag->init(); + if (ret != OK) { warnx("MAG init failed"); goto out; @@ -679,7 +688,7 @@ LSM303D::init() /* 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); + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { warnx("ADVERT ERR"); @@ -694,7 +703,7 @@ LSM303D::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { warnx("ADVERT ERR"); @@ -762,7 +771,7 @@ LSM303D::probe() if (success) { _checked_values[0] = WHO_I_AM; return OK; - } + } return -EIO; } @@ -775,8 +784,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { @@ -798,8 +808,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) + if (_accel_reports->get(arb)) { ret = sizeof(*arb); + } return ret; } @@ -812,8 +823,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { @@ -837,8 +849,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) _mag->measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) + if (_mag_reports->get(mrb)) { ret = sizeof(*mrb); + } return ret; } @@ -849,7 +862,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -871,56 +884,62 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 500) - return -EINVAL; + /* check against maximum sane rate */ + if (ticks < 500) { + return -EINVAL; + } - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + /* 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 = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_accel_interval = ticks; - _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; + _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) + if (_call_accel_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -936,23 +955,25 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } case ACCELIOCGLOWPASS: return static_cast(_accel_filter_x.get_cutoff_freq()); case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) 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; + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) 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; + } } - } case ACCELIOCSRANGE: /* arg needs to be in G */ @@ -960,7 +981,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / LSM303D_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -982,7 +1003,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -1012,16 +1033,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1029,25 +1052,29 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) + if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_mag_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); @@ -1102,24 +1129,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) int LSM303D::accel_self_test() { - if (_accel_read == 0) + if (_accel_read == 0) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -1127,21 +1164,25 @@ LSM303D::accel_self_test() int LSM303D::mag_self_test() { - if (_mag_read == 0) + if (_mag_read == 0) { return 1; + } /** * inspect mag offsets * don't check mag scale because it seems this is calibrated on chip */ - if (fabsf(_mag_scale.x_offset) < 0.000001f) + if (fabsf(_mag_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_mag_scale.y_offset) < 0.000001f) + if (fabsf(_mag_scale.y_offset) < 0.000001f) { return 1; + } - if (fabsf(_mag_scale.z_offset) < 0.000001f) + if (fabsf(_mag_scale.z_offset) < 0.000001f) { return 1; + } return 0; } @@ -1174,7 +1215,8 @@ void LSM303D::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 20 && fabsf(z_in_new) > 20) { _constant_accel_count += 1; + } else { _constant_accel_count = 0; } + if (_constant_accel_count > 100) { // we've had 100 constant accel readings with large // values. The sensor is almost certainly dead. We @@ -1712,16 +1763,19 @@ LSM303D::print_info() 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; imag_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); } } @@ -1881,17 +1940,19 @@ void start(bool external_bus, enum Rotation rotation, unsigned range) { int fd, fd_mag; + if (g_dev != nullptr) { errx(0, "already started"); } /* create the driver */ - if (external_bus) { - #ifdef PX4_SPI_BUS_EXT + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); - #else +#else errx(0, "External SPI not available"); - #endif +#endif + } else { g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } @@ -1901,8 +1962,9 @@ start(bool external_bus, enum Rotation rotation, unsigned range) goto fail; } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); @@ -1928,8 +1990,8 @@ start(bool external_bus, enum Rotation rotation, unsigned range) } } - close(fd); - close(fd_mag); + close(fd); + close(fd_mag); exit(0); fail: @@ -1958,14 +2020,16 @@ test() /* get the driver */ fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd_accel < 0) + 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(accel_report)); - if (sz != sizeof(accel_report)) + if (sz != sizeof(accel_report)) { err(1, "immediate read failed"); + } warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); @@ -1976,10 +2040,13 @@ test() warnx("accel z: \t%d\traw", (int)accel_report.z_raw); warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); - if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) { warnx("accel antialias filter bandwidth: fail"); - else + + } else { warnx("accel antialias filter bandwidth: %d Hz", ret); + } int fd_mag = -1; struct mag_report m_report; @@ -1987,19 +2054,23 @@ test() /* get the driver */ fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); - if (fd_mag < 0) + 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) + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); + } + warnx("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)) + if (sz != sizeof(m_report)) { err(1, "immediate read failed"); + } warnx("mag x: \t% 9.5f\tga", (double)m_report.x); warnx("mag y: \t% 9.5f\tga", (double)m_report.y); @@ -2014,8 +2085,8 @@ test() err(1, "reset to default polling"); } - close(fd_accel); - close(fd_mag); + close(fd_accel); + close(fd_mag); reset(); errx(0, "PASS"); @@ -2029,28 +2100,33 @@ reset() { int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "accel pollrate reset failed"); + } - close(fd); + close(fd); fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("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) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "mag pollrate reset failed"); + } } - close(fd); + close(fd); exit(0); } @@ -2061,8 +2137,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -2076,8 +2153,9 @@ info() void regdump() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("regdump @ %p\n", g_dev); g_dev->print_registers(); @@ -2091,8 +2169,9 @@ regdump() void test_error() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } g_dev->test_error(); @@ -2124,12 +2203,15 @@ lsm303d_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': accel_range = atoi(optarg); break; + default: lsm303d::usage(); exit(0); @@ -2142,38 +2224,44 @@ lsm303d_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { lsm303d::start(external_bus, rotation, accel_range); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { lsm303d::test(); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { lsm303d::reset(); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { lsm303d::info(); + } /* * dump device registers */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { lsm303d::regdump(); + } /* * trigger an error */ - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { lsm303d::test_error(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); }