diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 87d8551200..bc8791c7e2 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ADIS16477.hpp" -#include "ADIS16477_gyro.hpp" #include #include @@ -41,117 +40,52 @@ #define DIR_WRITE 0x80 // ADIS16477 registers -static constexpr uint8_t DIAG_STAT = 0x02; // Output, system error flags - -static constexpr uint8_t X_GYRO_LOW = 0x04; // Output, x-axis gyroscope, low word -static constexpr uint8_t X_GYRO_OUT = 0x06; // Output, x-axis gyroscope, high word -static constexpr uint8_t Y_GYRO_LOW = 0x08; // Output, y-axis gyroscope, low word -static constexpr uint8_t Y_GYRO_OUT = 0x0A; // Output, y-axis gyroscope, high word -static constexpr uint8_t Z_GYRO_LOW = 0x0C; // Output, z-axis gyroscope, low word -static constexpr uint8_t Z_GYRO_OUT = 0x0E; // Output, z-axis gyroscope, high word - -static constexpr uint8_t X_ACCL_LOW = 0x10; // Output, x-axis accelerometer, low word -static constexpr uint8_t X_ACCL_OUT = 0x12; // Output, x-axis accelerometer, high word -static constexpr uint8_t Y_ACCL_LOW = 0x14; // Output, y-axis accelerometer, low word -static constexpr uint8_t Y_ACCL_OUT = 0x16; // Output, y-axis accelerometer, high word -static constexpr uint8_t Z_ACCL_LOW = 0x18; // Output, z-axis accelerometer, low word -static constexpr uint8_t Z_ACCL_OUT = 0x1A; // Output, z-axis accelerometer, high word - -static constexpr uint8_t TEMP_OUT = 0x1A; // Output, temperature -static constexpr uint8_t TIME_STAMP = 0x1A; // Output, time stamp - +static constexpr uint8_t DIAG_STAT = 0x02; // Output, system error flags static constexpr uint8_t FILT_CTRL = 0x5C; +static constexpr uint8_t MSC_CTRL = 0x60; static constexpr uint8_t DEC_RATE = 0x64; - static constexpr uint8_t GLOB_CMD = 0x68; - static constexpr uint8_t PROD_ID = 0x72; -static constexpr uint16_t PROD_ID_ADIS16477 = 0x405D; /* ADIS16477 Identification, device number */ +static constexpr uint16_t PROD_ID_ADIS16477 = 0x405D; // ADIS16477 Identification, device number -static constexpr int T_STALL = 16; +// Stall time between SPI transfers +static constexpr uint8_t T_STALL = 16; -#define GYROINITIALSENSITIVITY 250 -#define ACCELINITIALSENSITIVITY (1.0f / 1200.0f) -#define ACCELDYNAMICRANGE 18.0f +static constexpr uint32_t ADIS16477_DEFAULT_RATE = 1000; using namespace time_literals; -ADIS16477::ADIS16477(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) : - SPI("ADIS16477", path_accel, bus, device, SPIDEV_MODE3, 1000000), - ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), - _gyro(new ADIS16477_gyro(this, path_gyro)), - _sample_perf(perf_alloc(PC_ELAPSED, "adis16477_read")), - _bad_transfers(perf_alloc(PC_COUNT, "adis16477_bad_transfers")), - _rotation(rotation) +ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) : + SPI("ADIS16477", nullptr, bus, device, SPIDEV_MODE3, 1000000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _px4_accel(get_device_id(), ORB_PRIO_MAX, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation), + _sample_interval_perf(perf_alloc(PC_INTERVAL, "adis16477: read interval")), + _sample_perf(perf_alloc(PC_ELAPSED, "adis16477: read")), + _bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers")) { #ifdef GPIO_SPI1_RESET_ADIS16477 - // ADIS16477 configure reset + // Configure hardware reset line px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16477); -#endif /* GPIO_SPI1_RESET_ADIS16477 */ +#endif // GPIO_SPI1_RESET_ADIS16477 - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ADIS16477; + _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16477); + _px4_accel.set_sample_rate(ADIS16477_DEFAULT_RATE); + _px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB - _gyro->_device_id.devid = _device_id.devid; - _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_ADIS16477; - - // default gyro scale factors - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; - - // default accel scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - const unsigned sample_rate = 1000; - - // set software low pass filter for controllers - param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); - float accel_cut = ADIS16477_ACCEL_DEFAULT_RATE; - - if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) { - _accel_filter_x.set_cutoff_frequency(sample_rate, accel_cut); - _accel_filter_y.set_cutoff_frequency(sample_rate, accel_cut); - _accel_filter_z.set_cutoff_frequency(sample_rate, accel_cut); - - } else { - PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); - } - - param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); - float gyro_cut = ADIS16477_GYRO_DEFAULT_RATE; - - if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) { - _gyro_filter_x.set_cutoff_frequency(sample_rate, gyro_cut); - _gyro_filter_y.set_cutoff_frequency(sample_rate, gyro_cut); - _gyro_filter_z.set_cutoff_frequency(sample_rate, gyro_cut); - - } else { - PX4_ERR("IMU_GYRO_CUTOFF param invalid"); - } + _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477); + _px4_gyro.set_sample_rate(ADIS16477_DEFAULT_RATE); + _px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB } ADIS16477::~ADIS16477() { - /* make sure we are truly inactive */ + // make sure we are truly inactive stop(); - /* delete the gyro subdriver */ - delete _gyro; - - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - - /* delete the perf counter */ + // delete the perf counters + perf_free(_sample_interval_perf); perf_free(_sample_perf); perf_free(_bad_transfers); } @@ -159,94 +93,76 @@ ADIS16477::~ADIS16477() int ADIS16477::init() { - if (hrt_absolute_time() < 252_ms) { - // power-on startup time (if needed) - up_mdelay(252); - } - - /* do SPI init (and probe) first */ + // do SPI init (and probe) first if (SPI::init() != OK) { - /* if probe/setup failed, bail now */ - DEVICE_DEBUG("SPI setup failed"); + // if probe/setup failed, bail now + PX4_DEBUG("SPI setup failed"); return PX4_ERROR; } - /* Initialize offsets and scales */ - _gyro_scale.x_offset = 0.0f; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0.0f; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0.0f; - _gyro_scale.z_scale = 1.0f; + start(); - _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; - - /* do CDev init for the gyro device node, keep it optional */ - int ret = _gyro->init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; - } - - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - - /* fetch an initial set of measurements for advertisement */ - measure(); - - /* advertise sensor topic, measure manually to initialize valid report */ - sensor_accel_s arp = {}; - - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_accel_orb_class_instance, ORB_PRIO_MAX); - - if (_accel_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - } - - sensor_gyro_s grp = {}; - _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, ORB_PRIO_MAX); - - if (_gyro->_gyro_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - } - - return ret; + return PX4_OK; } int ADIS16477::reset() { - DEVICE_DEBUG("resetting"); - #ifdef GPIO_SPI1_RESET_ADIS16477 - // ADIS16477 reset + // Hardware reset px4_arch_gpiowrite(GPIO_SPI1_RESET_ADIS16477, 0); // The RST line must be in a low state for at least 10 μs to ensure a proper reset initiation and recovery. - up_udelay(10); + usleep(10_us); px4_arch_gpiowrite(GPIO_SPI1_RESET_ADIS16477, 1); #else - // reset (global command bit 7) - uint8_t value[2] = {}; + // Software reset (global command bit 7) + uint8_t value[2] {}; value[0] = (1 << 7); write_reg16(GLOB_CMD, (uint16_t)value[0]); -#endif /* GPIO_SPI1_RESET_ADIS16477 */ +#endif // GPIO_SPI1_RESET_ADIS16477 - // Reset Recovery Time - up_mdelay(193); + // Reset recovery time + usleep(193_ms); - // configure digital filter, 16 taps - write_reg16(FILT_CTRL, 0x04); - // configure decimation rate - //write_reg16(DEC_RATE, 0x00); + // Miscellaneous Control Register (MSC_CTRL) + static constexpr uint16_t MSC_CTRL_DEFAULT = 0x00C1; + usleep(100); + // verify + const uint16_t msc_ctrl = read_reg16(MSC_CTRL); + + if (msc_ctrl != MSC_CTRL_DEFAULT) { + PX4_ERR("invalid setup, MSC_CTRL=%#X", msc_ctrl); + return PX4_ERROR; + } + + + // Bartlett Window FIR Filter + static constexpr uint16_t FILT_CTRL_SETUP = 0x0004; // (disabled: 0x0000, 2 taps: 0x0001, 16 taps: 0x0004) + write_reg16(FILT_CTRL, FILT_CTRL_SETUP); + usleep(100); + // verify + const uint16_t filt_ctrl = read_reg16(FILT_CTRL); + + if (filt_ctrl != FILT_CTRL_SETUP) { + PX4_ERR("invalid setup, FILT_CTRL=%#X", filt_ctrl); + return PX4_ERROR; + } + + + // Decimation Filter + // set for 1000 samples per second + static constexpr uint16_t DEC_RATE_SETUP = 0x0001; + write_reg16(DEC_RATE, DEC_RATE_SETUP); + usleep(100); + // verify + const uint16_t dec_rate = read_reg16(DEC_RATE); + + if (dec_rate != DEC_RATE_SETUP) { + PX4_ERR("invalid setup, DEC_RATE=%#X", dec_rate); + return PX4_ERROR; + } return OK; } @@ -254,18 +170,16 @@ int ADIS16477::reset() int ADIS16477::probe() { - DEVICE_DEBUG("probe"); - reset(); // read product id (5 attempts) for (int i = 0; i < 5; i++) { - _product = read_reg16(PROD_ID); + uint16_t product_id = read_reg16(PROD_ID); - if (_product == PROD_ID_ADIS16477) { - DEVICE_DEBUG("PRODUCT: %X", _product); + if (product_id == PROD_ID_ADIS16477) { + PX4_DEBUG("PRODUCT: %X", product_id); - if (self_test()) { + if (self_test_memory() && self_test_sensor()) { return PX4_OK; } else { @@ -282,161 +196,54 @@ ADIS16477::probe() return -EIO; } -/* set sample rate for both accel and gyro */ -void -ADIS16477::_set_sample_rate(uint16_t desired_sample_rate_hz) -{ - -} - -/* set the DLPF FIR filter tap. This affects both accelerometer and gyroscope. */ -void -ADIS16477::_set_dlpf_filter(uint16_t desired_filter_tap) -{ - //modify_reg16(ADIS16477_SENS_AVG, 0x0007, desired_filter_tap); - - /* Verify data write on the IMU */ - - //if ((read_reg16(ADIS16477_SENS_AVG) & 0x0007) != desired_filter_tap) { - // DEVICE_DEBUG("failed to set IMU filter"); - //} -} - -/* set IMU to factory defaults. */ -void -ADIS16477::_set_factory_default() -{ - //write_reg16(ADIS16477_GLOB_CMD, 0x02); -} - -/* set the gyroscope dynamic range */ -void -ADIS16477::_set_gyro_dyn_range(uint16_t desired_gyro_dyn_range) -{ -} - bool -ADIS16477::self_test() +ADIS16477::self_test_memory() { - DEVICE_DEBUG("self test"); + DEVICE_DEBUG("self test memory"); - // self test (global command bit 2) - uint8_t value[2] = {}; - value[0] = (1 << 2); + // self test (global command bit 4) + uint8_t value[2] {}; + value[0] = (1 << 4); write_reg16(GLOB_CMD, (uint16_t)value[0]); + usleep(32_ms); // Flash Memory Test Time // read DIAG_STAT to check result uint16_t diag_stat = read_reg16(DIAG_STAT); if (diag_stat != 0) { + PX4_ERR("DIAG_STAT: %#X", diag_stat); return false; } return true; } -int -ADIS16477::ioctl(struct file *filp, int cmd, unsigned long arg) +bool +ADIS16477::self_test_sensor() { - switch (cmd) { - case SENSORIOCRESET: - return reset(); + PX4_DEBUG("self test sensor"); - case SENSORIOCSPOLLRATE: { - switch (arg) { + // self test (global command bit 2) + uint8_t value[2] {}; + value[0] = (1 << 2); + write_reg16(GLOB_CMD, (uint16_t)value[0]); + usleep(14_ms); // Self Test Time - /* zero would be bad */ - case 0: - return -EINVAL; + // read DIAG_STAT to check result + uint16_t diag_stat = read_reg16(DIAG_STAT); - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, ADIS16477_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / arg; - - /* check against maximum sane rate */ - if (interval < 1000) { - return -EINVAL; - } - - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f / interval; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - - float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - 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); + if (diag_stat != 0) { + PX4_ERR("DIAG_STAT: %#X", diag_stat); + return false; } -} -int -ADIS16477::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - /* these are shared with the accel side */ - case SENSORIOCSPOLLRATE: - case SENSORIOCRESET: - return ioctl(filp, cmd, arg); - - case GYROIOCSSCALE: - /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); - return OK; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } + return true; } uint16_t ADIS16477::read_reg16(uint8_t reg) { - uint16_t cmd[1]; + uint16_t cmd[1] {}; cmd[0] = ((reg | DIR_READ) << 8) & 0xff00; transferhword(cmd, nullptr, 1); @@ -450,7 +257,7 @@ ADIS16477::read_reg16(uint8_t reg) void ADIS16477::write_reg(uint8_t reg, uint8_t val) { - uint8_t cmd[2]; + uint8_t cmd[2] {}; cmd[0] = reg | 0x8; cmd[1] = val; transfer(cmd, cmd, sizeof(cmd)); @@ -459,7 +266,7 @@ ADIS16477::write_reg(uint8_t reg, uint8_t val) void ADIS16477::write_reg16(uint8_t reg, uint16_t value) { - uint16_t cmd[2]; + uint16_t cmd[2] {}; cmd[0] = ((reg | DIR_WRITE) << 8) | (0x00ff & value); cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8); @@ -473,41 +280,64 @@ ADIS16477::write_reg16(uint8_t reg, uint16_t value) void ADIS16477::start() { - /* make sure we are stopped first */ - uint32_t last_call_interval = _call_interval; +#ifdef GPIO_SPI1_DRDY1_ADIS16477 + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, true, false, true, &ADIS16477::data_ready_interrupt, this); +#else + // Make sure we are stopped first stop(); - _call_interval = last_call_interval; - /* start polling at the specified rate */ - ScheduleOnInterval(_call_interval, 10000); + // start polling at the specified rate + ScheduleOnInterval((1_s / ADIS16477_DEFAULT_RATE), 10000); +#endif } void ADIS16477::stop() { +#ifdef GPIO_SPI1_DRDY1_ADIS16477 + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ADIS16477, false, false, false, nullptr, nullptr); +#else ScheduleClear(); +#endif +} + +int +ADIS16477::data_ready_interrupt(int irq, void *context, void *arg) +{ + ADIS16477 *dev = reinterpret_cast(arg); + + // make another measurement + dev->ScheduleNow(); + + return PX4_OK; } void ADIS16477::Run() { - /* make another measurement */ + // make another measurement measure(); } int ADIS16477::measure() { + perf_count(_sample_interval_perf); perf_begin(_sample_perf); // Fetch the full set of measurements from the ADIS16477 in one pass (burst read). - ADISReport adis_report; + ADISReport adis_report{}; adis_report.cmd = ((GLOB_CMD | DIR_READ) << 8) & 0xff00; // ADIS16477 burst report should be 176 bits static_assert(sizeof(adis_report) == (176 / 8), "ADIS16477 report not 176 bits"); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + if (OK != transferhword((uint16_t *)&adis_report, ((uint16_t *)&adis_report), sizeof(adis_report) / sizeof(uint16_t))) { + perf_count(_bad_transfers); perf_end(_sample_perf); return -EIO; } @@ -532,129 +362,42 @@ ADIS16477::measure() } if (adis_report.checksum != checksum) { - DEVICE_DEBUG("adis_report.checksum: %X vs calculated: %X", adis_report.checksum, checksum); - perf_event_count(_bad_transfers); + PX4_DEBUG("adis_report.checksum: %X vs calculated: %X", adis_report.checksum, checksum); + + perf_count(_bad_transfers); perf_end(_sample_perf); + return -EIO; } // Check all Status/Error Flag Indicators (DIAG_STAT) if (adis_report.diag_stat != 0) { - perf_event_count(_bad_transfers); + perf_count(_bad_transfers); perf_end(_sample_perf); + return -EIO; } - publish_gyro(adis_report); - publish_accel(adis_report); + // temperature 1 LSB = 0.1°C + const float temperature = adis_report.temp * 0.1f; + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); + + _px4_accel.update(timestamp_sample, adis_report.accel_x, adis_report.accel_y, adis_report.accel_z); + _px4_gyro.update(timestamp_sample, adis_report.gyro_x, adis_report.gyro_y, adis_report.gyro_z); - /* stop measuring */ perf_end(_sample_perf); + return OK; } -bool -ADIS16477::publish_accel(const ADISReport &report) -{ - sensor_accel_s arb = {}; - arb.timestamp = hrt_absolute_time(); - arb.device_id = _device_id.devid; - arb.error_count = perf_event_count(_bad_transfers); - - // raw sensor readings - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; - arb.scaling = _accel_range_scale; - - float xraw_f = report.accel_x * _accel_range_scale; - float yraw_f = report.accel_y * _accel_range_scale; - float zraw_f = report.accel_z * _accel_range_scale; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_in_new = (xraw_f - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = (yraw_f - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = (zraw_f - _accel_scale.z_offset) * _accel_scale.z_scale; - - arb.x = _accel_filter_x.apply(x_in_new); - arb.y = _accel_filter_y.apply(y_in_new); - arb.z = _accel_filter_z.apply(z_in_new); - - matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); - matrix::Vector3f aval_integrated; - - bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); - arb.x_integral = aval_integrated(0); - arb.y_integral = aval_integrated(1); - arb.z_integral = aval_integrated(2); - - /* Temperature report: */ - // temperature 1 LSB = 0.1°C - arb.temperature = report.temp * 0.1; - - if (accel_notify) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - } - - return accel_notify; -} - -bool -ADIS16477::publish_gyro(const ADISReport &report) -{ - sensor_gyro_s grb = {}; - grb.timestamp = hrt_absolute_time(); - grb.device_id = _gyro->_device_id.devid; - grb.error_count = perf_event_count(_bad_transfers); - - /* Gyro report: */ - grb.scaling = math::radians(_gyro_range_scale); - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; - - // ADIS16477-2BMLZ scale factory - float xraw_f = report.gyro_x; - float yraw_f = report.gyro_y; - float zraw_f = report.gyro_z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_gyro_in_new = (math::radians(xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = (math::radians(yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = (math::radians(zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - - grb.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.z = _gyro_filter_z.apply(z_gyro_in_new); - - matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - matrix::Vector3f gval_integrated; - - bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); - grb.x_integral = gval_integrated(0); - grb.y_integral = gval_integrated(1); - grb.z_integral = gval_integrated(2); - - /* Temperature report: */ - // temperature 1 LSB = 0.1°C - grb.temperature = report.temp * 0.1f; - - if (gyro_notify) { - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); - } - - return gyro_notify; -} - void ADIS16477::print_info() { + perf_print_counter(_sample_interval_perf); perf_print_counter(_sample_perf); perf_print_counter(_bad_transfers); - PX4_INFO("DEVICE ID:\nACCEL:\t%d\nGYRO:\t%d\n\n", _device_id.devid, _gyro->_device_id.devid); + _px4_accel.print_status(); + _px4_gyro.print_status(); } diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index ab7b0edb9e..cc271e284f 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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,99 +36,40 @@ * */ -#ifndef DRIVERS_IMU_ADIS16477_ADIS16477_HPP_ -#define DRIVERS_IMU_ADIS16477_ADIS16477_HPP_ +#pragma once -#include #include -#include -#include -#include -#include #include #include #include -#include #include - -#define ADIS16477_GYRO_DEFAULT_RATE 250 -#define ADIS16477_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define ADIS16477_ACCEL_DEFAULT_RATE 250 -#define ADIS16477_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define ADIS16477_ACCEL_MAX_OUTPUT_RATE 1221 -#define ADIS16477_GYRO_MAX_OUTPUT_RATE 1221 - -class ADIS16477_gyro; +#include +#include class ADIS16477 : public device::SPI, public px4::ScheduledWorkItem { public: - ADIS16477(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation); + ADIS16477(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); virtual ~ADIS16477(); virtual int init(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - void print_info(); protected: virtual int probe(); - friend class ADIS16477_gyro; - - virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg); - private: - ADIS16477_gyro *_gyro{nullptr}; - uint16_t _product{0}; /** product code */ - - unsigned _call_interval{0}; - - struct gyro_calibration_s _gyro_scale {}; - - // gyro 0.025 °/sec/LSB - float _gyro_range_scale{0.025f}; - float _gyro_range_rad_s{math::radians(500.0f)}; - - struct accel_calibration_s _accel_scale {}; - - // accel 1.25 mg/LSB - float _accel_range_scale{1.25f * CONSTANTS_ONE_G / 1000.0f}; - float _accel_range_m_s2{40.0f * CONSTANTS_ONE_G}; - - orb_advert_t _accel_topic{nullptr}; - - int _accel_orb_class_instance{-1}; - int _accel_class_instance{-1}; - - unsigned _sample_rate{100}; + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + perf_counter_t _sample_interval_perf; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; - math::LowPassFilter2p _gyro_filter_x{ADIS16477_GYRO_DEFAULT_RATE, ADIS16477_GYRO_DEFAULT_DRIVER_FILTER_FREQ}; - math::LowPassFilter2p _gyro_filter_y{ADIS16477_GYRO_DEFAULT_RATE, ADIS16477_GYRO_DEFAULT_DRIVER_FILTER_FREQ}; - math::LowPassFilter2p _gyro_filter_z{ADIS16477_GYRO_DEFAULT_RATE, ADIS16477_GYRO_DEFAULT_DRIVER_FILTER_FREQ}; - - math::LowPassFilter2p _accel_filter_x{ADIS16477_ACCEL_DEFAULT_RATE, ADIS16477_ACCEL_DEFAULT_DRIVER_FILTER_FREQ}; - math::LowPassFilter2p _accel_filter_y{ADIS16477_ACCEL_DEFAULT_RATE, ADIS16477_ACCEL_DEFAULT_DRIVER_FILTER_FREQ}; - math::LowPassFilter2p _accel_filter_z{ADIS16477_ACCEL_DEFAULT_RATE, ADIS16477_ACCEL_DEFAULT_DRIVER_FILTER_FREQ}; - - Integrator _accel_int{1000000 / ADIS16477_ACCEL_MAX_OUTPUT_RATE, false}; - Integrator _gyro_int{1000000 / ADIS16477_GYRO_MAX_OUTPUT_RATE, true}; - - enum Rotation _rotation; - - perf_counter_t _controller_latency_perf; - #pragma pack(push, 1) - /** - * Report conversation with in the ADIS16477, including command byte and interrupt status. - */ + // Report conversation with in the ADIS16477, including command byte and interrupt status. struct ADISReport { uint16_t cmd; uint16_t diag_stat; @@ -148,12 +89,12 @@ private: /** * Start automatic measurement. */ - void start(); + void start(); /** * Stop automatic measurement. */ - void stop(); + void stop(); /** * Reset chip. @@ -162,46 +103,22 @@ private: */ int reset(); - void Run() override; + void Run() override; + + static int data_ready_interrupt(int irq, void *context, void *arg); /** * Fetch measurements from the sensor and update the report buffers. */ int measure(); - bool publish_accel(const ADISReport &report); - bool publish_gyro(const ADISReport &report); - uint16_t read_reg16(uint8_t reg); void write_reg(uint8_t reg, uint8_t value); void write_reg16(uint8_t reg, uint16_t value); // ADIS16477 onboard self test - bool self_test(); + bool self_test_memory(); + bool self_test_sensor(); - /* - set low pass filter frequency - */ - void _set_dlpf_filter(uint16_t frequency_hz); - - /* - set IMU to factory default - */ - void _set_factory_default(); - - /* - set sample rate (approximate) - 1kHz to 5Hz - */ - void _set_sample_rate(uint16_t desired_sample_rate_hz); - - /* - set the gyroscope dynamic range - */ - void _set_gyro_dyn_range(uint16_t desired_gyro_dyn_range); - - ADIS16477(const ADIS16477 &); - ADIS16477 operator=(const ADIS16477 &); }; - -#endif /* DRIVERS_IMU_ADIS16477_ADIS16477_HPP_ */ diff --git a/src/drivers/imu/adis16477/ADIS16477_gyro.cpp b/src/drivers/imu/adis16477/ADIS16477_gyro.cpp deleted file mode 100644 index 82a6262c39..0000000000 --- a/src/drivers/imu/adis16477/ADIS16477_gyro.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "ADIS16477_gyro.hpp" - -ADIS16477_gyro::ADIS16477_gyro(ADIS16477 *parent, const char *path) : - CDev("ADIS16477_gyro", path), - _parent(parent), - _gyro_topic(nullptr), - _gyro_orb_class_instance(-1), - _gyro_class_instance(-1) -{ -} - -ADIS16477_gyro::~ADIS16477_gyro() -{ - if (_gyro_class_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); - } -} - -int -ADIS16477_gyro::init() -{ - int ret = CDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; - } - - _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - - return ret; -} - -int -ADIS16477_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case DEVIOCGDEVICEID: - return (int)CDev::ioctl(filp, cmd, arg); - break; - - default: - return _parent->gyro_ioctl(filp, cmd, arg); - } -} diff --git a/src/drivers/imu/adis16477/ADIS16477_gyro.hpp b/src/drivers/imu/adis16477/ADIS16477_gyro.hpp deleted file mode 100644 index d1fd071ab6..0000000000 --- a/src/drivers/imu/adis16477/ADIS16477_gyro.hpp +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef DRIVERS_IMU_ADIS16477_ADIS16477_GYRO_HPP_ -#define DRIVERS_IMU_ADIS16477_ADIS16477_GYRO_HPP_ - -#include "ADIS16477.hpp" - -#include -#include - -/** - * Helper class implementing the gyro driver node. - */ -class ADIS16477_gyro : public device::CDev -{ -public: - ADIS16477_gyro(ADIS16477 *parent, const char *path); - virtual ~ADIS16477_gyro(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - virtual int init(); - -protected: - friend class ADIS16477; - -private: - ADIS16477 *_parent{nullptr}; - orb_advert_t _gyro_topic{nullptr}; - - int _gyro_orb_class_instance{-1}; - int _gyro_class_instance{-1}; - - /* do not allow to copy this class due to pointer data members */ - ADIS16477_gyro(const ADIS16477_gyro &); - ADIS16477_gyro operator=(const ADIS16477_gyro &); - -}; - -#endif /* DRIVERS_IMU_ADIS16477_ADIS16477_GYRO_HPP_ */ diff --git a/src/drivers/imu/adis16477/CMakeLists.txt b/src/drivers/imu/adis16477/CMakeLists.txt index ea1448bdc6..132dd77cab 100644 --- a/src/drivers/imu/adis16477/CMakeLists.txt +++ b/src/drivers/imu/adis16477/CMakeLists.txt @@ -38,6 +38,9 @@ px4_add_module( -Wno-cast-align # TODO: fix and enable SRCS ADIS16477.cpp - ADIS16477_gyro.cpp - ADIS16477_main.cpp + adis16477_main.cpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope ) diff --git a/src/drivers/imu/adis16477/ADIS16477_main.cpp b/src/drivers/imu/adis16477/adis16477_main.cpp similarity index 55% rename from src/drivers/imu/adis16477/ADIS16477_main.cpp rename to src/drivers/imu/adis16477/adis16477_main.cpp index 28de033010..fc788018aa 100644 --- a/src/drivers/imu/adis16477/ADIS16477_main.cpp +++ b/src/drivers/imu/adis16477/adis16477_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 @@ -35,9 +35,6 @@ #include -#define ADIS16477_DEVICE_PATH_ACCEL "/dev/adis16477_accel" -#define ADIS16477_DEVICE_PATH_GYRO "/dev/adis16477_gyro" - extern "C" { __EXPORT int adis16477_main(int argc, char *argv[]); } /** @@ -46,11 +43,9 @@ extern "C" { __EXPORT int adis16477_main(int argc, char *argv[]); } namespace adis16477 { -ADIS16477 *g_dev; +ADIS16477 *g_dev{nullptr}; void start(enum Rotation rotation); -void test(); -void reset(); void info(); void usage(); /** @@ -59,8 +54,6 @@ void usage(); void start(enum Rotation rotation) { - int fd = -1; - if (g_dev != nullptr) /* if already started, the still command succeeded */ { @@ -69,8 +62,7 @@ start(enum Rotation rotation) /* create the driver */ #if defined(PX4_SPIDEV_ADIS16477) - g_dev = new ADIS16477(PX4_SPI_BUS_SENSOR1, ADIS16477_DEVICE_PATH_ACCEL, ADIS16477_DEVICE_PATH_GYRO, - PX4_SPIDEV_ADIS16477, rotation); + g_dev = new ADIS16477(PX4_SPI_BUS_SENSOR1, PX4_SPIDEV_ADIS16477, rotation); #else PX4_ERR("External SPI not available"); exit(0); @@ -80,22 +72,10 @@ start(enum Rotation rotation) 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 = px4_open(ADIS16477_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - px4_close(fd); exit(0); fail: @@ -104,86 +84,7 @@ fail: g_dev = nullptr; } - 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() -{ - sensor_accel_s a_report{}; - sensor_gyro_s g_report{}; - - ssize_t sz; - - /* get the driver */ - int fd = px4_open(ADIS16477_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd < 0) { - err(1, "%s open failed", ADIS16477_DEVICE_PATH_ACCEL); - } - - /* get the gyro driver */ - int fd_gyro = px4_open(ADIS16477_DEVICE_PATH_GYRO, O_RDONLY); - - if (fd_gyro < 0) { - err(1, "%s open failed", ADIS16477_DEVICE_PATH_GYRO); - } - - /* do a simple demand read */ - sz = read(fd, &a_report, sizeof(a_report)); - - if (sz != sizeof(a_report)) { - PX4_ERR("ret: %d, expected: %d", sz, sizeof(a_report)); - err(1, "immediate acc read failed"); - } - - print_message(a_report); - - /* do a simple demand read */ - sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); - - if (sz != sizeof(g_report)) { - warnx("ret: %d, expected: %d", sz, sizeof(g_report)); - err(1, "immediate gyro read failed"); - } - - print_message(g_report); - - px4_close(fd_gyro); - px4_close(fd); - - reset(); - errx(0, "PASS"); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = px4_open(ADIS16477_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd < 0) { - err(1, "open failed"); - } - - if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); - } - - px4_close(fd); - - exit(0); + PX4_ERR("driver start failed"); } /** @@ -193,19 +94,16 @@ void info() { if (g_dev == nullptr) { - errx(1, "driver not running"); + PX4_WARN("driver not running"); } - printf("state @ %p\n", g_dev); g_dev->print_info(); - - exit(0); } void usage() { - PX4_INFO("missing command: try 'start', 'test', 'info', 'reset'"); + PX4_INFO("missing command: try 'start', 'info'"); PX4_INFO("options:"); PX4_INFO(" -R rotation"); } @@ -217,9 +115,8 @@ int adis16477_main(int argc, char *argv[]) { enum Rotation rotation = ROTATION_NONE; - int myoptind = 1; - int ch; + int ch = 0; const char *myoptarg = nullptr; /* start options */ @@ -235,11 +132,6 @@ adis16477_main(int argc, char *argv[]) } } - if (myoptind >= argc) { - adis16477::usage(); - return -1; - } - const char *verb = argv[myoptind]; /* @@ -250,20 +142,6 @@ adis16477_main(int argc, char *argv[]) adis16477::start(rotation); } - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - adis16477::test(); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - adis16477::reset(); - } - /* * Print driver information. */ @@ -272,5 +150,6 @@ adis16477_main(int argc, char *argv[]) } adis16477::usage(); - exit(1); + + return 0; }