.. _program_listing_file_src_iio_wrapper.cpp: Program Listing for File iio_wrapper.cpp ======================================== |exhale_lsh| :ref:`Return to documentation for file ` (``src/iio_wrapper.cpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /***************************************************************************/ #include "adi_imu/iio_wrapper.h" #include #define MAX_NO_OF_SAMPLES 1 enum { CHAN_GYRO_X, CHAN_GYRO_Y, CHAN_GYRO_Z, CHAN_ACCEL_X, CHAN_ACCEL_Y, CHAN_ACCEL_Z, CHAN_TEMP, #ifdef ADIS_HAS_DELTA_BURST CHAN_DELTA_ANGL_X, CHAN_DELTA_ANGL_Y, CHAN_DELTA_ANGL_Z, CHAN_DELTA_VEL_X, CHAN_DELTA_VEL_Y, CHAN_DELTA_VEL_Z, #endif CHAN_DATA_TIMESTAMP, NO_OF_CHANS, }; struct iio_context * IIOWrapper::m_iio_context = nullptr; struct iio_device * IIOWrapper::m_dev = nullptr; struct iio_device * IIOWrapper::m_dev_trigger = nullptr; struct iio_buffer * IIOWrapper::m_dev_buffer = nullptr; struct iio_channel * IIOWrapper::m_channel_accel_x = nullptr; struct iio_channel * IIOWrapper::m_channel_accel_y = nullptr; struct iio_channel * IIOWrapper::m_channel_accel_z = nullptr; struct iio_channel * IIOWrapper::m_channel_anglvel_x = nullptr; struct iio_channel * IIOWrapper::m_channel_anglvel_y = nullptr; struct iio_channel * IIOWrapper::m_channel_anglvel_z = nullptr; struct iio_channel * IIOWrapper::m_channel_deltaangl_x = nullptr; struct iio_channel * IIOWrapper::m_channel_deltaangl_y = nullptr; struct iio_channel * IIOWrapper::m_channel_deltaangl_z = nullptr; struct iio_channel * IIOWrapper::m_channel_deltavelocity_x = nullptr; struct iio_channel * IIOWrapper::m_channel_deltavelocity_y = nullptr; struct iio_channel * IIOWrapper::m_channel_deltavelocity_z = nullptr; struct iio_channel * IIOWrapper::m_channel_temp = nullptr; struct iio_channel * IIOWrapper::m_channel_timestamp = nullptr; double IIOWrapper::m_scale_accel_x = 0; double IIOWrapper::m_scale_accel_y = 0; double IIOWrapper::m_scale_accel_z = 0; double IIOWrapper::m_scale_anglvel_x = 0; double IIOWrapper::m_scale_anglvel_y = 0; double IIOWrapper::m_scale_anglvel_z = 0; double IIOWrapper::m_scale_deltaangl_x = 0; double IIOWrapper::m_scale_deltaangl_y = 0; double IIOWrapper::m_scale_deltaangl_z = 0; double IIOWrapper::m_scale_deltavelocity_x = 0; double IIOWrapper::m_scale_deltavelocity_y = 0; double IIOWrapper::m_scale_deltavelocity_z = 0; double IIOWrapper::m_scale_temp = 0; uint32_t buff_write_idx = 0; uint32_t buff_read_idx = MAX_NO_OF_SAMPLES; uint32_t buff_data[NO_OF_CHANS + 1][MAX_NO_OF_SAMPLES]; double samp_freq = 2000.0; uint32_t no_of_samp = MAX_NO_OF_SAMPLES; uint32_t current_data_selection = FULL_MEASURED_DATA; bool has_delta_channels = true; bool has_timestamp_channel = false; IIOWrapper::IIOWrapper() {} IIOWrapper::~IIOWrapper() { if (m_dev_buffer != nullptr) { iio_buffer_destroy(m_dev_buffer); m_dev_buffer = nullptr; } if (m_iio_context != nullptr) { iio_context_destroy(m_iio_context); m_iio_context = nullptr; } } int IIOWrapper::createContext(const char * context) { if (m_iio_context) { RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "IIO context already exists."); return IIO_CONTEXT_ERROR; } if (!strcmp(context, "local:")) m_iio_context = iio_create_local_context(); else m_iio_context = iio_create_context_from_uri(context); if (!m_iio_context) { RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "IIO context is null"); return IIO_CONTEXT_ERROR; } iio_context_set_timeout(m_iio_context, 5000); std::list supported_devices{ "adis16465-1", "adis16465-2", "adis16465-3", "adis16467-1", "adis16467-2", "adis16467-3", "adis16470", "adis16475-1", "adis16475-2", "adis16475-3", "adis16477-1", "adis16477-2", "adis16477-3", "adis16500", "adis16501", "adis16505-1", "adis16505-2", "adis16505-3", "adis16507-1", "adis16507-2", "adis16507-3", "adis16545-1", "adis16545-2", "adis16545-3", "adis16547-1", "adis16547-2", "adis16547-3", "adis16550", "adis16575-2", "adis16575-3", "adis16576-2", "adis16576-3", "adis16577-2", "adis16577-3"}; uint8_t dev_id = 0; for (std::string const & devname : supported_devices) { m_dev = iio_context_find_device(m_iio_context, devname.c_str()); if (m_dev) { RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "Found device: %s", devname.c_str()); if (m_dev_trigger == nullptr) { const std::string devid = iio_device_get_id(m_dev); /* Get device number in the form of 0, 1, etc. to append to the * trigger name. */ std::string devnb; for (long unsigned int i = 0; i < devid.std::string::length(); i++) if (isdigit(devid.c_str()[i])) devnb.std::string::append(&devid.c_str()[i]); std::string triggerName = devname + "-dev" + devnb; m_dev_trigger = iio_context_find_device(m_iio_context, triggerName.c_str()); RCLCPP_INFO( rclcpp::get_logger("rclcpp_iiowrapper"), "Found trigger: %s", triggerName.c_str()); if (!m_dev_trigger) { RCLCPP_INFO( rclcpp::get_logger("rclcpp_iiowrapper"), "Did not find a trigger for device %s.", devname.c_str()); return IIO_CONTEXT_ERROR; } } break; } dev_id++; } if (!m_dev) { iio_context_destroy(m_iio_context); m_iio_context = nullptr; RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "No supported IIO device found."); return IIO_CONTEXT_ERROR; } iio_device_set_trigger(m_dev, m_dev_trigger); if (m_channel_accel_x == nullptr) m_channel_accel_x = iio_device_find_channel(m_dev, "accel_x", false); if (m_channel_accel_y == nullptr) m_channel_accel_y = iio_device_find_channel(m_dev, "accel_y", false); if (m_channel_accel_z == nullptr) m_channel_accel_z = iio_device_find_channel(m_dev, "accel_z", false); if (m_channel_anglvel_x == nullptr) m_channel_anglvel_x = iio_device_find_channel(m_dev, "anglvel_x", false); if (m_channel_anglvel_y == nullptr) m_channel_anglvel_y = iio_device_find_channel(m_dev, "anglvel_y", false); if (m_channel_anglvel_z == nullptr) m_channel_anglvel_z = iio_device_find_channel(m_dev, "anglvel_z", false); if (m_channel_deltaangl_x == nullptr) { m_channel_deltaangl_x = iio_device_find_channel(m_dev, "deltaangl_x", false); if (m_channel_deltaangl_x == nullptr) has_delta_channels = false; } if (has_delta_channels) { if (m_channel_deltaangl_y == nullptr) m_channel_deltaangl_y = iio_device_find_channel(m_dev, "deltaangl_y", false); if (m_channel_deltaangl_z == nullptr) m_channel_deltaangl_z = iio_device_find_channel(m_dev, "deltaangl_z", false); if (m_channel_deltavelocity_x == nullptr) m_channel_deltavelocity_x = iio_device_find_channel(m_dev, "deltavelocity_x", false); if (m_channel_deltavelocity_y == nullptr) m_channel_deltavelocity_y = iio_device_find_channel(m_dev, "deltavelocity_y", false); if (m_channel_deltavelocity_z == nullptr) m_channel_deltavelocity_z = iio_device_find_channel(m_dev, "deltavelocity_z", false); } if (m_channel_temp == nullptr) m_channel_temp = iio_device_find_channel(m_dev, "temp0", false); if (m_channel_timestamp == nullptr) m_channel_timestamp = iio_device_find_channel(m_dev, "timestamp", false); if (m_channel_temp) iio_channel_enable(m_channel_temp); if (m_channel_timestamp) { iio_channel_enable(m_channel_timestamp); has_timestamp_channel = true; } if (m_channel_accel_x) iio_channel_attr_read_double(m_channel_accel_x, "scale", &m_scale_accel_x); if (m_channel_accel_y) iio_channel_attr_read_double(m_channel_accel_y, "scale", &m_scale_accel_y); if (m_channel_accel_z) iio_channel_attr_read_double(m_channel_accel_z, "scale", &m_scale_accel_z); if (m_channel_anglvel_x) iio_channel_attr_read_double(m_channel_anglvel_x, "scale", &m_scale_anglvel_x); if (m_channel_anglvel_y) iio_channel_attr_read_double(m_channel_anglvel_y, "scale", &m_scale_anglvel_y); if (m_channel_anglvel_z) iio_channel_attr_read_double(m_channel_anglvel_z, "scale", &m_scale_anglvel_z); if (has_delta_channels) { iio_channel_attr_read_double(m_channel_deltaangl_x, "scale", &m_scale_deltaangl_x); iio_channel_attr_read_double(m_channel_deltaangl_y, "scale", &m_scale_deltaangl_y); iio_channel_attr_read_double(m_channel_deltaangl_z, "scale", &m_scale_deltaangl_z); iio_channel_attr_read_double(m_channel_deltavelocity_x, "scale", &m_scale_deltavelocity_x); iio_channel_attr_read_double(m_channel_deltavelocity_y, "scale", &m_scale_deltavelocity_y); iio_channel_attr_read_double(m_channel_deltavelocity_z, "scale", &m_scale_deltavelocity_z); } else { /* Set scale manually in case delta channels are not available in the * linux driver. */ setDeltaAngleScales((enum adis_device_id)dev_id); setDeltaVelocityScales((enum adis_device_id)dev_id); } if (m_channel_temp) iio_channel_attr_read_double(m_channel_temp, "scale", &m_scale_temp); return 0; } bool IIOWrapper::updateField(uint32_t reg, uint32_t val, uint32_t mask) { int ret; uint32_t __val; if (!m_dev) return false; ret = iio_device_reg_read(m_dev, reg, &__val); if (ret) return false; __val = (__val & ~mask) | (val & mask); return (iio_device_reg_write(m_dev, reg, __val) == 0); } void IIOWrapper::setDeltaAngleScales(enum adis_device_id id) { switch (id) { case ADIS16465_1: case ADIS16467_1: case ADIS16475_1: case ADIS16477_1: case ADIS16505_1: case ADIS16507_1: m_scale_deltaangl_x = 0.000000002; m_scale_deltaangl_y = 0.000000002; m_scale_deltaangl_z = 0.000000002; return; case ADIS16465_2: case ADIS16467_2: case ADIS16475_2: case ADIS16477_2: case ADIS16501: case ADIS16505_2: case ADIS16507_2: case ADIS16550: m_scale_deltaangl_x = 0.000000006; m_scale_deltaangl_y = 0.000000006; m_scale_deltaangl_z = 0.000000006; return; case ADIS16465_3: case ADIS16467_3: case ADIS16470: case ADIS16475_3: case ADIS16477_3: case ADIS16500: case ADIS16505_3: case ADIS16507_3: case ADIS16545_1: case ADIS16545_2: case ADIS16545_3: case ADIS16547_1: case ADIS16547_2: case ADIS16547_3: m_scale_deltaangl_x = 0.000000017; m_scale_deltaangl_y = 0.000000017; m_scale_deltaangl_z = 0.000000017; return; case ADIS16575_2: case ADIS16576_2: case ADIS16577_2: m_scale_deltaangl_x = 0.000000003; m_scale_deltaangl_y = 0.000000003; m_scale_deltaangl_z = 0.000000003; return; case ADIS16575_3: case ADIS16576_3: case ADIS16577_3: m_scale_deltaangl_x = 0.000000016; m_scale_deltaangl_y = 0.000000016; m_scale_deltaangl_z = 0.000000016; return; default: return; } } void IIOWrapper::setDeltaVelocityScales(enum adis_device_id id) { switch (id) { case ADIS16467_1: case ADIS16467_2: case ADIS16467_3: case ADIS16470: case ADIS16477_1: case ADIS16477_2: case ADIS16477_3: case ADIS16500: case ADIS16507_1: case ADIS16507_2: case ADIS16507_3: m_scale_deltavelocity_x = 0.000000186; m_scale_deltavelocity_y = 0.000000186; m_scale_deltavelocity_z = 0.000000186; return; case ADIS16465_1: case ADIS16465_2: case ADIS16465_3: case ADIS16475_1: case ADIS16475_2: case ADIS16475_3: case ADIS16505_1: case ADIS16505_2: case ADIS16505_3: case ADIS16545_1: case ADIS16545_2: case ADIS16545_3: case ADIS16547_1: case ADIS16547_2: case ADIS16547_3: case ADIS16575_2: case ADIS16575_3: case ADIS16576_2: case ADIS16576_3: case ADIS16577_2: case ADIS16577_3: m_scale_deltavelocity_x = 0.000000046; m_scale_deltavelocity_y = 0.000000046; m_scale_deltavelocity_z = 0.000000046; return; case ADIS16501: case ADIS16550: m_scale_deltavelocity_x = 0.000000058; m_scale_deltavelocity_y = 0.000000058; m_scale_deltavelocity_z = 0.000000058; return; default: return; } } void IIOWrapper::stopBufferAcquisition() { if (m_dev_buffer != nullptr) { iio_buffer_destroy(m_dev_buffer); m_dev_buffer = nullptr; } } static ssize_t demux_sample( const struct iio_channel * chn, void * sample, size_t size, __attribute__((unused)) void * d) { if (size == 2) { int16_t val; iio_channel_convert(chn, &val, sample); buff_data[iio_channel_get_index(chn)][buff_write_idx] = val; if (!has_timestamp_channel) { /* timestamp channel is not available, have to update buff_write_idx */ buff_write_idx++; } } else if (size == 4) { int32_t val; iio_channel_convert(chn, &val, sample); buff_data[iio_channel_get_index(chn)][buff_write_idx] = val; if (!has_timestamp_channel) { /* timestamp channel is not available, have to update buff_write_idx for last * read channel */ #ifdef ADIS_HAS_DELTA_BURST if (iio_channel_get_index(chn) == CHAN_DELTA_VEL_Z) buff_write_idx++; #endif } } else { int64_t val; iio_channel_convert(chn, &val, sample); buff_data[CHAN_DATA_TIMESTAMP][buff_write_idx] = val; buff_data[CHAN_DATA_TIMESTAMP + 1][buff_write_idx++] = val >> 32; } if (buff_write_idx == no_of_samp) buff_write_idx = 0; return size; } bool IIOWrapper::updateBuffer(uint32_t data_selection) { ssize_t ret; if (current_data_selection != data_selection) { stopBufferAcquisition(); if (data_selection == ACCEL_GYRO_BUFFERED_DATA) { #ifdef ADIS_HAS_DELTA_BURST if (has_delta_channels) { iio_channel_disable(m_channel_deltaangl_x); iio_channel_disable(m_channel_deltaangl_y); iio_channel_disable(m_channel_deltaangl_z); iio_channel_disable(m_channel_deltavelocity_x); iio_channel_disable(m_channel_deltavelocity_y); iio_channel_disable(m_channel_deltavelocity_z); } #endif iio_channel_enable(m_channel_accel_x); iio_channel_enable(m_channel_accel_y); iio_channel_enable(m_channel_accel_z); iio_channel_enable(m_channel_anglvel_x); iio_channel_enable(m_channel_anglvel_y); iio_channel_enable(m_channel_anglvel_z); } else { iio_channel_disable(m_channel_accel_x); iio_channel_disable(m_channel_accel_y); iio_channel_disable(m_channel_accel_z); iio_channel_disable(m_channel_anglvel_x); iio_channel_disable(m_channel_anglvel_y); iio_channel_disable(m_channel_anglvel_z); #ifdef ADIS_HAS_DELTA_BURST if (has_delta_channels) { iio_channel_enable(m_channel_deltaangl_x); iio_channel_enable(m_channel_deltaangl_y); iio_channel_enable(m_channel_deltaangl_z); iio_channel_enable(m_channel_deltavelocity_x); iio_channel_enable(m_channel_deltavelocity_y); iio_channel_enable(m_channel_deltavelocity_z); } else { stopBufferAcquisition(); } #endif } current_data_selection = data_selection; } if (m_dev_buffer == nullptr) { sampling_frequency(&samp_freq); no_of_samp = MAX_NO_OF_SAMPLES; if (no_of_samp > samp_freq) /* Overwrite number of samples based on sampling frequency, to avoid * timeout errors from LibIIO */ no_of_samp = samp_freq; m_dev_buffer = iio_device_create_buffer(m_dev, no_of_samp, false); if (!m_dev_buffer) throw std::runtime_error("Exception: device buffer is null"); buff_read_idx = 0; buff_write_idx = 0; } else { buff_read_idx++; if (buff_read_idx < no_of_samp) return true; } ret = iio_buffer_refill(m_dev_buffer); if ((ret == 0) || (ret == -110)) { RCLCPP_INFO( rclcpp::get_logger("rclcpp_iiowrapper"), "no samples available yet, retrying ret = %ld", ret); return false; } if (ret < 0) { RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "buffer refill error status %ld", ret); stopBufferAcquisition(); return false; } iio_buffer_foreach_sample(m_dev_buffer, demux_sample, NULL); buff_read_idx = 0; return true; } double IIOWrapper::getBuffLinearAccelerationX() { return (int32_t)buff_data[CHAN_ACCEL_X][buff_read_idx] * m_scale_accel_x; } double IIOWrapper::getBuffLinearAccelerationY() { return (int32_t)buff_data[CHAN_ACCEL_Y][buff_read_idx] * m_scale_accel_y; } double IIOWrapper::getBuffLinearAccelerationZ() { return (int32_t)buff_data[CHAN_ACCEL_Z][buff_read_idx] * m_scale_accel_z; } double IIOWrapper::getBuffAngularVelocityX() { return (int32_t)buff_data[CHAN_GYRO_X][buff_read_idx] * m_scale_anglvel_x; } double IIOWrapper::getBuffAngularVelocityY() { return (int32_t)buff_data[CHAN_GYRO_Y][buff_read_idx] * m_scale_anglvel_y; } double IIOWrapper::getBuffAngularVelocityZ() { return (int32_t)buff_data[CHAN_GYRO_Z][buff_read_idx] * m_scale_anglvel_z; } #ifdef ADIS_HAS_DELTA_BURST double IIOWrapper::getBuffDeltaAngleX() { if (!has_delta_channels) { double result; if (getConvertedDeltaAngleXFromDebug(result)) return result; else return 0.0; } return (int32_t)buff_data[CHAN_DELTA_ANGL_X][buff_read_idx] * m_scale_deltaangl_x; } double IIOWrapper::getBuffDeltaAngleY() { if (!has_delta_channels) { double result; if (getConvertedDeltaAngleYFromDebug(result)) return result; else return 0.0; } return (int32_t)buff_data[CHAN_DELTA_ANGL_Y][buff_read_idx] * m_scale_deltaangl_y; } double IIOWrapper::getBuffDeltaAngleZ() { if (!has_delta_channels) { double result; if (getConvertedDeltaAngleZFromDebug(result)) return result; else return 0.0; } return (int32_t)buff_data[CHAN_DELTA_ANGL_Z][buff_read_idx] * m_scale_deltaangl_z; } double IIOWrapper::getBuffDeltaVelocityX() { if (!has_delta_channels) { double result; if (getConvertedDeltaVelocityXFromDebug(result)) return result; else return 0.0; } return (int32_t)buff_data[CHAN_DELTA_VEL_X][buff_read_idx] * m_scale_deltavelocity_x; } double IIOWrapper::getBuffDeltaVelocityY() { if (!has_delta_channels) { double result; if (getConvertedDeltaVelocityYFromDebug(result)) return result; else return 0.0; } return (int32_t)buff_data[CHAN_DELTA_VEL_Y][buff_read_idx] * m_scale_deltavelocity_y; } double IIOWrapper::getBuffDeltaVelocityZ() { if (!has_delta_channels) { double result; if (getConvertedDeltaVelocityZFromDebug(result)) return result; else return 0.0; } return (int32_t)buff_data[CHAN_DELTA_VEL_Z][buff_read_idx] * m_scale_deltavelocity_z; } #endif double IIOWrapper::getBuffTemperature() { return (int32_t)buff_data[CHAN_TEMP][buff_read_idx] * m_scale_temp / 1000.0; } void IIOWrapper::getBuffSampleTimestamp(int32_t & sec, uint32_t & nanosec) { if (has_timestamp_channel) { uint16_t timestamp_0_15 = buff_data[CHAN_DATA_TIMESTAMP][buff_read_idx]; uint16_t timestamp_16_31 = buff_data[CHAN_DATA_TIMESTAMP][buff_read_idx] >> 16; uint16_t timestamp_32_47 = buff_data[CHAN_DATA_TIMESTAMP + 1][buff_read_idx]; uint16_t timestamp_48_63 = buff_data[CHAN_DATA_TIMESTAMP + 1][buff_read_idx] >> 16; uint64_t timestamp = ((uint64_t)timestamp_48_63 << 48) | ((uint64_t)timestamp_32_47 << 32) | ((uint64_t)timestamp_16_31 << 16) | timestamp_0_15; sec = timestamp / 1000000000; nanosec = timestamp % 1000000000; } else { sec = 0; nanosec = 0; } } bool IIOWrapper::getConvertedLinearAccelerationX(double & result) { long long valueRaw; if (!m_channel_accel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_x, "raw", &valueRaw); result = valueRaw * m_scale_accel_x; return (ret == 0); } bool IIOWrapper::getConvertedLinearAccelerationY(double & result) { long long valueRaw; if (!m_channel_accel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_y, "raw", &valueRaw); result = valueRaw * m_scale_accel_y; return (ret == 0); } bool IIOWrapper::getConvertedLinearAccelerationZ(double & result) { long long valueRaw; if (!m_channel_accel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_z, "raw", &valueRaw); result = valueRaw * m_scale_accel_z; return (ret == 0); } bool IIOWrapper::getConvertedAngularVelocityX(double & result) { long long valueRaw; if (!m_channel_anglvel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_x, "raw", &valueRaw); result = valueRaw * m_scale_anglvel_x; return (ret == 0); } bool IIOWrapper::getConvertedAngularVelocityY(double & result) { long long valueRaw; if (!m_channel_anglvel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_y, "raw", &valueRaw); result = valueRaw * m_scale_anglvel_y; return (ret == 0); } bool IIOWrapper::getConvertedAngularVelocityZ(double & result) { long long valueRaw; if (!m_channel_anglvel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_z, "raw", &valueRaw); result = valueRaw * m_scale_anglvel_z; return (ret == 0); } bool IIOWrapper::getRawDeltaAngleXFromDebug(int32_t & result) { uint32_t reg_low; uint32_t reg_high; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DELTANG_X_LOW_REG, ®_low); if (ret) return false; ret = iio_device_reg_read(m_dev, ADIS_DELTANG_X_OUT_REG, ®_high); if (ret) return false; result = reg_high << 16 | reg_low; return true; } bool IIOWrapper::getConvertedDeltaAngleXFromDebug(double & result) { int32_t reg_val = 0; bool ret = getRawDeltaAngleXFromDebug(reg_val); result = reg_val * m_scale_deltaangl_x; return ret; } bool IIOWrapper::getConvertedDeltaAngleX(double & result) { long long valueRaw; if (!m_channel_deltaangl_x) return getConvertedDeltaAngleXFromDebug(result); int ret = iio_channel_attr_read_longlong(m_channel_deltaangl_x, "raw", &valueRaw); result = valueRaw * m_scale_deltaangl_x; return (ret == 0); } bool IIOWrapper::getRawDeltaAngleYFromDebug(int32_t & result) { uint32_t reg_low; uint32_t reg_high; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DELTANG_Y_LOW_REG, ®_low); if (ret) return false; ret = iio_device_reg_read(m_dev, ADIS_DELTANG_Y_OUT_REG, ®_high); if (ret) return false; result = reg_high << 16 | reg_low; return true; } bool IIOWrapper::getConvertedDeltaAngleYFromDebug(double & result) { int32_t reg_val = 0; bool ret = getRawDeltaAngleYFromDebug(reg_val); result = reg_val * m_scale_deltaangl_y; return ret; } bool IIOWrapper::getConvertedDeltaAngleY(double & result) { long long valueRaw; if (!m_channel_deltaangl_y) return getConvertedDeltaAngleYFromDebug(result); int ret = iio_channel_attr_read_longlong(m_channel_deltaangl_y, "raw", &valueRaw); result = valueRaw * m_scale_deltaangl_y; return (ret == 0); } bool IIOWrapper::getRawDeltaAngleZFromDebug(int32_t & result) { uint32_t reg_low; uint32_t reg_high; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DELTANG_Z_LOW_REG, ®_low); if (ret) return false; ret = iio_device_reg_read(m_dev, ADIS_DELTANG_Z_OUT_REG, ®_high); if (ret) return false; result = reg_high << 16 | reg_low; return true; } bool IIOWrapper::getConvertedDeltaAngleZFromDebug(double & result) { int32_t reg_val = 0; bool ret = getRawDeltaAngleZFromDebug(reg_val); result = reg_val * m_scale_deltaangl_z; return ret; } bool IIOWrapper::getConvertedDeltaAngleZ(double & result) { long long valueRaw; if (!m_channel_deltaangl_z) return getConvertedDeltaAngleZFromDebug(result); int ret = iio_channel_attr_read_longlong(m_channel_deltaangl_z, "raw", &valueRaw); result = valueRaw * m_scale_deltaangl_z; return (ret == 0); } bool IIOWrapper::getRawDeltaVelocityXFromDebug(int32_t & result) { uint32_t reg_low; uint32_t reg_high; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DELTVEL_X_LOW_REG, ®_low); if (ret) return false; ret = iio_device_reg_read(m_dev, ADIS_DELTVEL_X_OUT_REG, ®_high); if (ret) return false; result = reg_high << 16 | reg_low; return true; } bool IIOWrapper::getConvertedDeltaVelocityXFromDebug(double & result) { int32_t reg_val = 0; bool ret = getRawDeltaVelocityXFromDebug(reg_val); result = reg_val * m_scale_deltavelocity_x; return ret; } bool IIOWrapper::getConvertedDeltaVelocityX(double & result) { long long valueRaw; if (!m_channel_deltavelocity_x) return getConvertedDeltaVelocityXFromDebug(result); int ret = iio_channel_attr_read_longlong(m_channel_deltavelocity_x, "raw", &valueRaw); result = valueRaw * m_scale_deltavelocity_x; return (ret == 0); } bool IIOWrapper::getRawDeltaVelocityYFromDebug(int32_t & result) { uint32_t reg_low; uint32_t reg_high; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DELTVEL_Y_LOW_REG, ®_low); if (ret) return false; ret = iio_device_reg_read(m_dev, ADIS_DELTVEL_Y_OUT_REG, ®_high); if (ret) return false; result = reg_high << 16 | reg_low; return true; } bool IIOWrapper::getConvertedDeltaVelocityYFromDebug(double & result) { int32_t reg_val = 0; bool ret = getRawDeltaVelocityYFromDebug(reg_val); result = reg_val * m_scale_deltavelocity_y; return ret; } bool IIOWrapper::getConvertedDeltaVelocityY(double & result) { long long valueRaw; if (!m_channel_deltavelocity_y) return getConvertedDeltaVelocityYFromDebug(result); int ret = iio_channel_attr_read_longlong(m_channel_deltavelocity_y, "raw", &valueRaw); result = valueRaw * m_scale_deltavelocity_y; return (ret == 0); } bool IIOWrapper::getRawDeltaVelocityZFromDebug(int32_t & result) { uint32_t reg_low; uint32_t reg_high; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DELTVEL_Z_LOW_REG, ®_low); if (ret) return false; ret = iio_device_reg_read(m_dev, ADIS_DELTVEL_Z_OUT_REG, ®_high); if (ret) return false; result = reg_high << 16 | reg_low; return true; } bool IIOWrapper::getConvertedDeltaVelocityZFromDebug(double & result) { int32_t reg_val = 0; bool ret = getRawDeltaVelocityZFromDebug(reg_val); result = reg_val * m_scale_deltavelocity_z; return ret; } bool IIOWrapper::getConvertedDeltaVelocityZ(double & result) { long long valueRaw; if (!m_channel_deltavelocity_z) return getConvertedDeltaVelocityZFromDebug(result); int ret = iio_channel_attr_read_longlong(m_channel_deltavelocity_z, "raw", &valueRaw); result = valueRaw * m_scale_deltavelocity_z; return (ret == 0); } bool IIOWrapper::getConvertedTemperature(double & result) { long long valueRaw; if (!m_channel_temp) return false; int ret = iio_channel_attr_read_longlong(m_channel_temp, "raw", &valueRaw); result = valueRaw * m_scale_temp / 1000.0; return (ret == 0); } bool IIOWrapper::anglvel_x_calibbias(int32_t & result) { long long valuel; if (!m_channel_anglvel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_x, "calibbias", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_anglvel_calibbias_x(int32_t val) { if (!m_channel_anglvel_x) return false; return (iio_channel_attr_write_longlong(m_channel_anglvel_x, "calibbias", val) == 0); } bool IIOWrapper::anglvel_y_calibbias(int32_t & result) { long long valuel; if (!m_channel_anglvel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_y, "calibbias", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_anglvel_calibbias_y(int32_t val) { if (!m_channel_anglvel_y) return false; return (iio_channel_attr_write_longlong(m_channel_anglvel_y, "calibbias", val) == 0); } bool IIOWrapper::anglvel_z_calibbias(int32_t & result) { long long valuel; if (!m_channel_anglvel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_z, "calibbias", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_anglvel_calibbias_z(int32_t val) { if (!m_channel_anglvel_z) return false; return (iio_channel_attr_write_longlong(m_channel_anglvel_z, "calibbias", val) == 0); } bool IIOWrapper::accel_x_calibbias(int32_t & result) { long long valuel; if (!m_channel_accel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_x, "calibbias", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_accel_calibbias_x(int32_t val) { if (!m_channel_accel_x) return false; return (iio_channel_attr_write_longlong(m_channel_accel_x, "calibbias", val) == 0); } bool IIOWrapper::accel_y_calibbias(int32_t & result) { long long valuel; if (!m_channel_accel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_y, "calibbias", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_accel_calibbias_y(int32_t val) { if (!m_channel_accel_y) return false; return (iio_channel_attr_write_longlong(m_channel_accel_y, "calibbias", val) == 0); } bool IIOWrapper::accel_z_calibbias(int32_t & result) { long long valuel; if (!m_channel_accel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_z, "calibbias", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_accel_calibbias_z(int32_t val) { if (!m_channel_accel_z) return false; return (iio_channel_attr_write_longlong(m_channel_accel_z, "calibbias", val) == 0); } bool IIOWrapper::sampling_frequency(double * result) { int ret; if (!m_dev) return false; ret = iio_device_attr_read_double(m_dev, "sampling_frequency", result); if (ret) return false; samp_freq = *result; return true; } bool IIOWrapper::update_sampling_frequency(double val) { int ret; if (!m_dev) return false; ret = iio_device_attr_write_double(m_dev, "sampling_frequency", val); if (ret) return false; samp_freq = val; return true; } #if defined(ADIS1654X) || defined(ADIS1655X) bool IIOWrapper::angvel_x_filter_low_pass_3db(uint32_t & result) { long long valuel; if (!m_channel_anglvel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_x, "filter_low_pass_3db_frequency", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_angvel_x_filter_low_pass_3db(uint32_t val) { if (!m_channel_anglvel_x) return false; return ( iio_channel_attr_write_longlong(m_channel_anglvel_x, "filter_low_pass_3db_frequency", val) == 0); } bool IIOWrapper::angvel_y_filter_low_pass_3db(uint32_t & result) { long long valuel; if (!m_channel_anglvel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_y, "filter_low_pass_3db_frequency", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_angvel_y_filter_low_pass_3db(uint32_t val) { if (!m_channel_anglvel_y) return false; return ( iio_channel_attr_write_longlong(m_channel_anglvel_y, "filter_low_pass_3db_frequency", val) == 0); } bool IIOWrapper::angvel_z_filter_low_pass_3db(uint32_t & result) { long long valuel; if (!m_channel_anglvel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_z, "filter_low_pass_3db_frequency", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_angvel_z_filter_low_pass_3db(uint32_t val) { if (!m_channel_anglvel_z) return false; return ( iio_channel_attr_write_longlong(m_channel_anglvel_z, "filter_low_pass_3db_frequency", val) == 0); } bool IIOWrapper::accel_x_filter_low_pass_3db(uint32_t & result) { long long valuel; if (!m_channel_accel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_x, "filter_low_pass_3db_frequency", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_accel_x_filter_low_pass_3db(uint32_t val) { if (!m_channel_accel_x) return false; return ( iio_channel_attr_write_longlong(m_channel_accel_x, "filter_low_pass_3db_frequency", val) == 0); } bool IIOWrapper::accel_y_filter_low_pass_3db(uint32_t & result) { long long valuel; if (!m_channel_accel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_y, "filter_low_pass_3db_frequency", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_accel_y_filter_low_pass_3db(uint32_t val) { if (!m_channel_accel_y) return false; return ( iio_channel_attr_write_longlong(m_channel_accel_y, "filter_low_pass_3db_frequency", val) == 0); } bool IIOWrapper::accel_z_filter_low_pass_3db(uint32_t & result) { long long valuel; if (!m_channel_accel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_z, "filter_low_pass_3db_frequency", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_accel_z_filter_low_pass_3db(uint32_t val) { if (!m_channel_accel_z) return false; return ( iio_channel_attr_write_longlong(m_channel_accel_z, "filter_low_pass_3db_frequency", val) == 0); } #else bool IIOWrapper::filter_low_pass_3db_frequency(uint32_t & result) { long long valuel; if (!m_dev) return false; int ret = iio_device_attr_read_longlong(m_dev, "filter_low_pass_3db_frequency", &valuel); if (ret) return false; result = valuel; return true; } bool IIOWrapper::update_filter_low_pass_3db_frequency(uint32_t val) { if (!m_dev) return false; return (iio_device_attr_write_longlong(m_dev, "filter_low_pass_3db_frequency", val) == 0); } #endif #ifdef ADIS_HAS_CALIB_SCALE bool IIOWrapper::accel_x_calibscale(int32_t & result) { long long valuel; if (!m_channel_accel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_x, "calibscale", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::accel_y_calibscale(int32_t & result) { long long valuel; if (!m_channel_accel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_y, "calibscale", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::accel_z_calibscale(int32_t & result) { long long valuel; if (!m_channel_accel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_accel_z, "calibscale", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::anglvel_x_calibscale(int32_t & result) { long long valuel; if (!m_channel_anglvel_x) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_x, "calibscale", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::anglvel_y_calibscale(int32_t & result) { long long valuel; if (!m_channel_anglvel_y) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_y, "calibscale", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::anglvel_z_calibscale(int32_t & result) { long long valuel; if (!m_channel_anglvel_z) return false; int ret = iio_channel_attr_read_longlong(m_channel_anglvel_z, "calibscale", &valuel); result = valuel; return (ret == 0); } bool IIOWrapper::update_accel_calibscale_x(int32_t val) { if (!m_channel_accel_x) return false; return (iio_channel_attr_write_longlong(m_channel_accel_x, "calibscale", val) == 0); } bool IIOWrapper::update_accel_calibscale_y(int32_t val) { if (!m_channel_accel_y) return false; return (iio_channel_attr_write_longlong(m_channel_accel_y, "calibscale", val) == 0); } bool IIOWrapper::update_accel_calibscale_z(int32_t val) { if (!m_channel_accel_z) return false; return (iio_channel_attr_write_longlong(m_channel_accel_z, "calibscale", val) == 0); } bool IIOWrapper::update_anglvel_calibscale_x(int32_t val) { if (!m_channel_anglvel_x) return false; return (iio_channel_attr_write_longlong(m_channel_anglvel_x, "calibscale", val) == 0); } bool IIOWrapper::update_anglvel_calibscale_y(int32_t val) { if (!m_channel_anglvel_y) return false; return (iio_channel_attr_write_longlong(m_channel_anglvel_y, "calibscale", val) == 0); } bool IIOWrapper::update_anglvel_calibscale_z(int32_t val) { if (!m_channel_anglvel_z) return false; return (iio_channel_attr_write_longlong(m_channel_anglvel_z, "calibscale", val) == 0); } #endif #ifdef ADIS_SNSR_INIT_FAIL bool IIOWrapper::diag_sensor_initialization_failure(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_SNSR_INIT_FAIL) >> ADIS_SNSR_INIT_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_DATA_PATH_OVERRUN bool IIOWrapper::diag_data_path_overrun(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_DATA_PATH_OVERRUN) >> ADIS_DATA_PATH_OVERRUN_POS; result = reg_val; return true; } #endif #ifdef ADIS_WDG_TIMER_FLAG bool IIOWrapper::diag_automatic_reset(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_WDG_TIMER_FLAG) >> ADIS_WDG_TIMER_FLAG_POS; result = reg_val; return true; } #endif #ifdef ADIS_FLS_MEM_UPDATE_FAIL bool IIOWrapper::diag_flash_memory_update_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_FLS_MEM_UPDATE_FAIL) >> ADIS_FLS_MEM_UPDATE_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_SPI_COMM_ERR bool IIOWrapper::diag_spi_communication_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_SPI_COMM_ERR) >> ADIS_SPI_COMM_ERR_POS; result = reg_val; return true; } #endif #ifdef ADIS_CRC_ERROR_POS bool IIOWrapper::diag_crc_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_CRC_ERROR) >> ADIS_CRC_ERROR_POS; result = reg_val; return true; } #endif #ifdef ADIS_STDBY_MODE bool IIOWrapper::diag_standby_mode(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_STDBY_MODE) >> ADIS_STDBY_MODE_POS; result = reg_val; return true; } #endif #ifdef ADIS_SNSR_FAIL bool IIOWrapper::diag_sensor_self_test_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_SNSR_FAIL) >> ADIS_SNSR_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_MEM_FAIL bool IIOWrapper::diag_flash_memory_test_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_MEM_FAIL) >> ADIS_MEM_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_CLK_ERR bool IIOWrapper::diag_clock_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_CLK_ERR) >> ADIS_CLK_ERR_POS; result = reg_val; return true; } #endif #ifdef ADIS_GYRO1_FAIL bool IIOWrapper::diag_gyroscope1_self_test_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO1_FAIL) >> ADIS_GYRO1_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_GYRO2_FAIL bool IIOWrapper::diag_gyroscope2_self_test_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO2_FAIL) >> ADIS_GYRO2_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_ACCEL_FAIL bool IIOWrapper::diag_acceleration_self_test_error(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ACCEL_FAIL) >> ADIS_ACCEL_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_GYRO_X_FAIL bool IIOWrapper::diag_x_axis_gyroscope_failure(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO_X_FAIL) >> ADIS_GYRO_X_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_GYRO_Y_FAIL bool IIOWrapper::diag_y_axis_gyroscope_failure(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO_Y_FAIL) >> ADIS_GYRO_Y_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_GYRO_Z_FAIL bool IIOWrapper::diag_z_axis_gyroscope_failure(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO_Z_FAIL) >> ADIS_GYRO_Z_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_ACCEL_X_FAIL bool IIOWrapper::diag_x_axis_accelerometer_failure(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ACCEL_X_FAIL) >> ADIS_ACCEL_X_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_ACCEL_Y_FAIL bool IIOWrapper::diag_y_axis_accelerometer_failure(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ACCEL_Y_FAIL) >> ADIS_ACCEL_Y_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_ACCEL_Z_FAIL bool IIOWrapper::diag_z_axis_accelerometer_failure(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ACCEL_Z_FAIL) >> ADIS_ACCEL_Z_FAIL_POS; result = reg_val; return true; } #endif #ifdef ADIS_ADUC_MCU_FAULT bool IIOWrapper::diag_aduc_mcu_fault(bool & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ADUC_MCU_FAULT) >> ADIS_ADUC_MCU_FAULT_POS; result = reg_val; return true; } #endif bool IIOWrapper::diag_flash_memory_write_count_exceeded_error(bool & result) { uint32_t reg_val; bool ret = flash_counter(reg_val); if (!ret) return false; result = reg_val > ADIS_FLS_MEM_ENDURANCE; return true; } bool IIOWrapper::gyroscope_measurement_range(std::string & result) { uint32_t reg_val; if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_RANG_MDL_ADDR, ®_val); if (ret) return false; #ifdef ADIS1655X result = "+/-300_degrees_per_sec"; return true; #else reg_val = (reg_val & ADIS_GYRO_MEAS_RANG) >> ADIS_GYRO_MEAS_RANG_POS; switch (reg_val) { case 0: result = "+/-125_degrees_per_sec"; return true; case 1: #ifdef ADIS1654X result = "+/-450_degrees_per_sec"; #else result = "+/-500_degrees_per_sec"; #endif return true; case 3: result = "+/-2000_degrees_per_sec"; return true; default: return false; } #endif } #ifdef ADIS_SENS_BW bool IIOWrapper::internal_sensor_bandwidth(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_MSC_CTRL_ADDR, &result); if (ret) return false; result = (result & ADIS_SENS_BW) >> ADIS_SENS_BW_POS; return true; } bool IIOWrapper::update_internal_sensor_bandwidth(uint32_t val) { return updateField(ADIS_MSC_CTRL_ADDR, val << ADIS_SENS_BW_POS, ADIS_SENS_BW); } #endif #ifdef ADIS_PT_OF_PERC_REG_ADDR bool IIOWrapper::point_of_percussion_alignment(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_PT_OF_PERC_REG_ADDR, &result); if (ret) return false; result = (result & ADIS_PT_OF_PERC_ALGNMNT) >> ADIS_PT_OF_PERC_ALGNMNT_POS; return true; } #endif #ifdef ADIS_PT_OF_PERC_REG_ADDR bool IIOWrapper::update_point_of_percussion_alignment(uint32_t val) { return updateField( ADIS_PT_OF_PERC_REG_ADDR, val << ADIS_PT_OF_PERC_ALGNMNT_POS, ADIS_PT_OF_PERC_ALGNMNT); } #endif #ifdef ADIS_MSC_CTRL_ADDR bool IIOWrapper::linear_acceleration_compensation(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_MSC_CTRL_ADDR, &result); if (ret) return false; result = (result & ADIS_LN_ACCL_COMP) >> ADIS_LN_ACCL_COMP_POS; return true; } #endif #ifdef ADIS_MSC_CTRL_ADDR bool IIOWrapper::update_linear_acceleration_compensation(uint32_t val) { return updateField(ADIS_MSC_CTRL_ADDR, val << ADIS_LN_ACCL_COMP_POS, ADIS_LN_ACCL_COMP); } #endif #ifdef ADIS_NULL_CNFG_ADDR bool IIOWrapper::bias_correction_time_base_control(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_NULL_CNFG_ADDR, &result); if (ret) return false; result = (result & ADIS_TIME_BASE_CONTROL) >> ADIS_TIME_BASE_CONTROL_POS; return true; } bool IIOWrapper::update_bias_correction_time_base_control(uint32_t val) { return updateField( ADIS_NULL_CNFG_ADDR, val << ADIS_TIME_BASE_CONTROL_POS, ADIS_TIME_BASE_CONTROL); } bool IIOWrapper::x_axis_gyroscope_bias_correction_enable(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_NULL_CNFG_ADDR, &result); if (ret) return false; result = (result & ADIS_X_AXIS_GYRO_BIAS_CORR_EN) >> ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS; return true; } bool IIOWrapper::update_x_axis_gyroscope_bias_correction_enable(uint32_t val) { return updateField( ADIS_NULL_CNFG_ADDR, val << ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS, ADIS_X_AXIS_GYRO_BIAS_CORR_EN); } bool IIOWrapper::y_axis_gyroscope_bias_correction_enable(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_NULL_CNFG_ADDR, &result); if (ret) return false; result = (result & ADIS_Y_AXIS_GYRO_BIAS_CORR_EN) >> ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS; return true; } bool IIOWrapper::update_y_axis_gyroscope_bias_correction_enable(uint32_t val) { return updateField( ADIS_NULL_CNFG_ADDR, val << ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS, ADIS_Y_AXIS_GYRO_BIAS_CORR_EN); } bool IIOWrapper::z_axis_gyroscope_bias_correction_enable(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_NULL_CNFG_ADDR, &result); if (ret) return false; result = (result & ADIS_Z_AXIS_GYRO_BIAS_CORR_EN) >> ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS; return true; } bool IIOWrapper::update_z_axis_gyroscope_bias_correction_enable(uint32_t val) { return updateField( ADIS_NULL_CNFG_ADDR, val << ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS, ADIS_Z_AXIS_GYRO_BIAS_CORR_EN); } bool IIOWrapper::x_axis_accelerometer_bias_correction_enable(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_NULL_CNFG_ADDR, &result); if (ret) return false; result = (result & ADIS_X_AXIS_ACCEL_BIAS_CORR_EN) >> ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS; return true; } bool IIOWrapper::update_x_axis_accelerometer_bias_correction_enable(uint32_t val) { return updateField( ADIS_NULL_CNFG_ADDR, val << ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS, ADIS_X_AXIS_ACCEL_BIAS_CORR_EN); } bool IIOWrapper::y_axis_accelerometer_bias_correction_enable(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_NULL_CNFG_ADDR, &result); if (ret) return false; result = (result & ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN) >> ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS; return true; } bool IIOWrapper::update_y_axis_accelerometer_bias_correction_enable(uint32_t val) { return updateField( ADIS_NULL_CNFG_ADDR, val << ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS, ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN); } bool IIOWrapper::z_axis_accelerometer_bias_correction_enable(uint32_t & result) { if (!m_dev) return false; int ret = iio_device_reg_read(m_dev, ADIS_NULL_CNFG_ADDR, &result); if (ret) return false; result = (result & ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN) >> ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS; return true; } bool IIOWrapper::update_z_axis_accelerometer_bias_correction_enable(uint32_t val) { return updateField( ADIS_NULL_CNFG_ADDR, val << ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS, ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN); } #endif #ifdef ADIS_BIAS_CORRECTION_UPDATE bool IIOWrapper::bias_correction_update() { if (!m_dev) return false; uint16_t cmd = ADIS_BIAS_CORRECTION_UPDATE; return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } #endif bool IIOWrapper::factory_calibration_restore() { if (!m_dev) return false; uint16_t cmd = ADIS_FACTORY_CALIBRATION_RESTORE; return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } bool IIOWrapper::sensor_self_test() { if (!m_dev) return false; uint16_t cmd = ADIS_SENSOR_SELF_TEST; return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } bool IIOWrapper::flash_memory_update() { if (!m_dev) return false; uint16_t cmd = ADIS_FLASH_MEMORY_UPDATE; return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } #ifdef ADIS_FLASH_MEMORY_TEST bool IIOWrapper::flash_memory_test() { if (!m_dev) return false; uint16_t cmd = ADIS_FLASH_MEMORY_TEST; return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } #endif bool IIOWrapper::software_reset() { if (!m_dev) return false; uint16_t cmd = ADIS_SOFTWARE_RESET_CMD; return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } bool IIOWrapper::firmware_revision(std::string & result) { if (!m_dev) return false; char valuec[32]; int ret = iio_device_debug_attr_read(m_dev, "firmware_revision", valuec, 32); if (ret < 0) return false; result = valuec; return true; } bool IIOWrapper::firmware_date(std::string & result) { if (!m_dev) return false; char valuec[32]; int ret = iio_device_debug_attr_read(m_dev, "firmware_date", valuec, 32); if (ret < 0) return false; result = valuec; return true; } bool IIOWrapper::product_id(uint32_t & result) { if (!m_dev) return false; long long valuel; int ret = iio_device_debug_attr_read_longlong(m_dev, "product_id", &valuel); if (ret) return false; result = valuel; return true; } bool IIOWrapper::serial_number(uint32_t & result) { if (!m_dev) return false; long long valuel; int ret = iio_device_debug_attr_read_longlong(m_dev, "serial_number", &valuel); if (ret) return false; result = valuel; return true; } bool IIOWrapper::flash_counter(uint32_t & result) { if (!m_dev) return false; long long valuel; int ret = iio_device_debug_attr_read_longlong(m_dev, "flash_count", &valuel); if (ret) return false; result = valuel; return true; } double IIOWrapper::get_scale_accel() { return m_scale_accel_x; } double IIOWrapper::get_scale_anglvel() { return m_scale_anglvel_x; } double IIOWrapper::get_scale_deltavelocity() { return m_scale_deltavelocity_x; } double IIOWrapper::get_scale_deltaangl() { return m_scale_deltaangl_x; } double IIOWrapper::get_scale_temp() { return m_scale_temp / 1000.0; }