diff --git a/CMakeLists.txt b/CMakeLists.txt index 990efec..ce2ad62 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,95 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +string (COMPARE EQUAL "adis16465-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16465_1) +string (COMPARE EQUAL "adis16465-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16465_2) +string (COMPARE EQUAL "adis16465-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16465_3) +string (COMPARE EQUAL "adis16467-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16467_1) +string (COMPARE EQUAL "adis16467-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16467_2) +string (COMPARE EQUAL "adis16467-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16467_3) +string (COMPARE EQUAL "adis16470" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16470) +string (COMPARE EQUAL "adis16475-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16475_1) +string (COMPARE EQUAL "adis16475-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16475_2) +string (COMPARE EQUAL "adis16475-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16475_3) + +string (COMPARE EQUAL "adis16477-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16477_1) +string (COMPARE EQUAL "adis16477-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16477_2) +string (COMPARE EQUAL "adis16477-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16477_3) + +string (COMPARE EQUAL "adis16500" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16500) +string (COMPARE EQUAL "adis16505-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16505_1) +string (COMPARE EQUAL "adis16505-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16505_2) +string (COMPARE EQUAL "adis16505-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16505_3) +string (COMPARE EQUAL "adis16507-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16507_1) +string (COMPARE EQUAL "adis16507-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16507_2) +string (COMPARE EQUAL "adis16507-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16507_3) + +string (COMPARE EQUAL "adis16575-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16575_2) +string (COMPARE EQUAL "adis16575-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16575_3) +string (COMPARE EQUAL "adis16576-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16576_2) +string (COMPARE EQUAL "adis16576-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16576_3) +string (COMPARE EQUAL "adis16577-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16577_2) +string (COMPARE EQUAL "adis16577-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16577_3) + +if (${DEVICE_FOUND_ADIS16500} OR + ${DEVICE_FOUND_ADIS16505_1} OR + ${DEVICE_FOUND_ADIS16505_2} OR + ${DEVICE_FOUND_ADIS16505_3} OR + ${DEVICE_FOUND_ADIS16507_1} OR + ${DEVICE_FOUND_ADIS16507_2} OR + ${DEVICE_FOUND_ADIS16507_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1650x) + add_compile_definitions(ADIS1650X) + +elseif (${DEVICE_FOUND_ADIS16465_1} OR + ${DEVICE_FOUND_ADIS16465_2} OR + ${DEVICE_FOUND_ADIS16465_3} OR + ${DEVICE_FOUND_ADIS16467_1} OR + ${DEVICE_FOUND_ADIS16467_2} OR + ${DEVICE_FOUND_ADIS16467_3} OR + ${DEVICE_FOUND_ADIS16470} OR + ${DEVICE_FOUND_ADIS16475_1} OR + ${DEVICE_FOUND_ADIS16475_2} OR + ${DEVICE_FOUND_ADIS16475_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1646x) + add_compile_definitions(ADIS1646X) + +elseif (${DEVICE_FOUND_ADIS16565_2} OR + ${DEVICE_FOUND_ADIS16565_3} OR + ${DEVICE_FOUND_ADIS16576_2} OR + ${DEVICE_FOUND_ADIS16576_3} OR + ${DEVICE_FOUND_ADIS16577_2} OR + ${DEVICE_FOUND_ADIS16577_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1657x) + add_compile_definitions(ADIS1657X) + +elseif (${DEVICE_FOUND_ADIS16477_1} OR + ${DEVICE_FOUND_ADIS16477_2} OR + ${DEVICE_FOUND_ADIS16477_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1647x) + add_compile_definitions(ADIS1647X) + +else() + message (FATAL_ERROR " + DEVICE_ID=$ENV{DEVICE_ID} is not a valid value. + Please use one of the following 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, + adis16505-1, adis16505-2, adis16505-3, + adis16507-1, adis16507-2, adis16507-3, + adis16575-2, adis16575-3, + adis16576-2, adis16576-3, + adis16577-2, adis16577-3.") +endif() + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -18,11 +107,12 @@ find_package(rosidl_default_generators REQUIRED) find_library(LIBIIO_LIBRARIES iio) find_path(LIBIIO_INCLUDE_DIRS iio.h) - file(GLOB FILES_H ${CMAKE_SOURCE_DIR}/include/*.h - ${CMAKE_SOURCE_DIR}/include/imu_ros2/*.h) + ${CMAKE_SOURCE_DIR}/include/imu_ros2/*.h + ${CMAKE_SOURCE_DIR}/include/imu_ros2/${DEVICE_PATH}/*.h) -file(GLOB FILES_CPP ${CMAKE_SOURCE_DIR}/src/*.cpp) +file(GLOB FILES_CPP ${CMAKE_SOURCE_DIR}/src/*.cpp + ${CMAKE_SOURCE_DIR}/src/${DEVICE_PATH}/*.cpp) add_executable(imu_ros2_node ${FILES_H} ${FILES_CPP}) @@ -41,14 +131,13 @@ install(TARGETS imu_ros2_node rosidl_generate_interfaces(imu_ros2 "msg/ImuIdentificationData.msg" "msg/VelAngTempData.msg" - "msg/Imu1650xDiagData.msg" "msg/ImuFullMeasuredData.msg" "msg/AccelGyroTempData.msg" - "msg/Imu1657xDiagData.msg" + "msg/${DEVICE_PATH}/ImuDiagData.msg" DEPENDENCIES builtin_interfaces geometry_msgs ) -rosidl_target_interfaces(imu_ros2_node +rosidl_get_typesupport_target(imu_ros2_node ${PROJECT_NAME} "rosidl_typesupport_cpp") ament_export_dependencies(rosidl_default_runtime) @@ -57,24 +146,10 @@ target_link_libraries(imu_ros2_node "${imu_ros2_node}") if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() - - #ament_add_gtest(iio_ros2_test_node test/utest_launch.test test/src/utest.cpp) - add_subdirectory(test) endif() -install(FILES config/imu16505_config.yaml DESTINATION config) -install(FILES config/imu1657x_config.yaml DESTINATION config) - - - +install(FILES config/${DEVICE_PATH}/imu_config.yaml DESTINATION config) ament_package() diff --git a/README.md b/README.md deleted file mode 100644 index 2824f2e..0000000 --- a/README.md +++ /dev/null @@ -1,4 +0,0 @@ -# imu-ros2 -A C++ ROS2 node that reads sensor data from ADI IMU devices and publishes -messages on topics. The node is also able to configure ADI IMU devices. -The ROS2 driver uses LibIIO to retrieve information from IMU Kernel Driver. diff --git a/README.rst b/README.rst new file mode 100644 index 0000000..911d485 --- /dev/null +++ b/README.rst @@ -0,0 +1,977 @@ +ROS2 drivers for ADI's IMUs +=========================== + +.. contents:: + :depth: 2 + +Supported Devices +----------------- + +* `ADIS16465 `_ +* `ADIS16467 `_ +* `ADIS16470 `_ +* `ADIS16475 `_ +* `ADIS16477 `_ +* `ADIS16500 `_ +* `ADIS16505 `_ +* `ADIS16507 `_ +* `ADIS16575 `_ +* `ADIS16576 `_ +* `ADIS16577 `_ + +Overview +-------- + +Analog Devices offers a series of precision, miniature microelectromechanical +system (MEMS) inertial measurement units (IMUs) that include a triaxial +gyroscope and a triaxial accelerometer. Each inertial combines with signal +conditioning that optimizes dynamic performance. + +The factory calibration characterizes each sensor for sensitivity, bias, +alignment, linear acceleration (gyroscope bias), and point of percussion +(accelerometer location). As a result, each sensor has dynamic compensation +formulas that provide accurate sensor measurements over a broad set of +conditions. + +Applications +------------ + +* Navigation, stabilization, and instrumentation +* Unmanned and autonomous vehicles +* Smart agriculture and construction machinery +* Factory/industrial automation, robotics +* Virtual/augmented reality +* Internet of Moving Things + +Using imu-ros2 repository +------------------------- + +This repository implements a ROS2 node that reads data from ADI IMU devices and +publishes the read data on various topics. The node is also able to configure +the IMU devices. + +Prerequisites +^^^^^^^^^^^^^ + +imu-ros2 can be used with ROS2 humble distribution. To install it you may follow +the documentation available here: https://docs.ros.org/en/humble/Installation.html + +imu-ros2 is using LibIIO v0.25. To install LibIIO you may follow the documentation +available here: https://wiki.analog.com/resources/tools-software/linux-software/libiio + +imu-ros2 is compatible with adis16475 Linux Kernel Driver which can be found here: +https://github.com/torvalds/linux/blob/master/drivers/iio/imu/adis16475.c + + +Evaluation setup - Required hardware with remote client +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +* Raspberry Pi 3 or 4 with adis16475 driver and LibIIO installed. + +* `EVAL-ADISIMU1-RPIZ `_ + for connecting ADI IMUs to Raspberry Pi (with ribbon cable). + More information about EVAL-AIDSIMU1-RPIZ connection to RPI can be found here: + https://wiki.analog.com/resources/eval/user-guides/circuits-from-the-lab/eval-adisimu1-rpiz + +* Any of the following supported IMUs connected to the Raspberry Pi using EVAL-ADISIMU1-RPIZ: + `ADIS16465 `_ + `ADIS16467 `_ + `ADIS16470 `_ + `ADIS16475 `_ + `ADIS16477 `_ + `ADIS16500 `_ + `ADIS16505 `_ + `ADIS16507 `_ + `ADIS16575 `_ + `ADIS16576 `_ + `ADIS16577 `_ + +* A client running ROS2 Humble with LibIIO installed on which imu-ros2 node is started. + +* A network connection between Raspberry Pi and the client running the imu-ros2 node. + +The image below shows the information flow from the IMU up to the imu-ros2 node +when using a remote client. + +.. figure:: architecture_remote_client.png + :width: 721 + :align: center + :alt: Architecture Remote Client + +Evaluation setup - Required hardware with local client +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +* Raspberry Pi 3 or 4 with adis16475 driver, LibIIO and ROS2 humble installed. + +* `EVAL-ADISIMU1-RPIZ `_ + for connecting ADI IMUs to Raspberry Pi (with ribbon cable). + More information about EVAL-AIDSIMU1-RPIZ connection to RPI can be found here: + https://wiki.analog.com/resources/eval/user-guides/circuits-from-the-lab/eval-adisimu1-rpiz + +* Any of the following supported IMUs connected to the Raspberry Pi using EVAL-ADISIMU1-RPIZ: + `ADIS16465 `_ + `ADIS16467 `_ + `ADIS16470 `_ + `ADIS16475 `_ + `ADIS16477 `_ + `ADIS16500 `_ + `ADIS16505 `_ + `ADIS16507 `_ + `ADIS16575 `_ + `ADIS16576 `_ + `ADIS16577 `_ + +The image below shows the information flow from the IMU up to the imu-ros2 node +when using a local client. + +.. figure:: architecture_local_client.png + :width: 401 + :align: center + :alt: Architecture Local Client + +Build imu_ros2 from sources +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +If you are not using this node in an existing project, create a new folder ros2_ws, then create the src folder in ros2_ws. +Go to src folder (either in ros2_ws or in your project), and clone the imu_ros2 repository: + +.. code-block:: bash + + git clone https://github.com/analogdevicesinc/imu-ros2.git + +Go back to your main project folder or ros2_ws folder and export the following environment variable, +based on the IMU chip: + +.. code-block:: bash + + export DEVICE_ID={IMU_chip} + + IMU_chip available options are: + 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, + adis16505-1, adis16505-2, adis16505-3, + adis16507-1, adis16507-2, adis16507-3, + adis16575-2, adis16575-3, + adis16576-2, adis16576-3, + adis16577-2, adis16577-3. + +In order select adis16505-2 IMU, run the following command: + +.. code-block:: bash + + export DEVICE_ID=adis16505-2 + +After DEVICE_ID variable is exported, run the following command: + +.. code-block:: bash + + colcon build + +Check wether the build is successful. + +Run imu_ros2 node with local client +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +If the ROS2 environment is running on the same processing unit to which the IMU is +connected to (e.g. on Raspberry Pi), run the following command to start the imu_ros2 node: + +.. code-block:: bash + + source install/setup.sh + ros2 run imu_ros2 imu_ros2_node + +For executing system tests, run the following commands, after the imu_ros2 node +has been started: + + +.. code-block:: bash + + source install/setup.sh + cd install/imu_ros2/lib/imu_ros2_test + + # set measured_data_topic_selection to 0 to test VelAngTempSubscriber (not available for adis1646x) + ros2 param set /imu_ros2_node measured_data_topic_selection 0 + ./imu_ros2_test_node --gtest_filter="VelAngTempSubscriberTest*" + + # set measured_data_topic_selection to 1 to test AccelGyroTempSubscriber + ros2 param set /imu_ros2_node measured_data_topic_selection 1 + ./imu_ros2_test_node --gtest_filter="AccelGyroTempSubscriberTest*" + + # set measured_data_topic_selection to 2 to test ImuSubscriber + ros2 param set /imu_ros2_node measured_data_topic_selection 2 + ./imu_ros2_test_node --gtest_filter="ImuSubscriberTest*" + + # set measured_data_topic_selection to 3 to test ImuFullMeasuredDataSubscriber + ros2 param set /imu_ros2_node measured_data_topic_selection 3 + ./imu_ros2_test_node --gtest_filter="ImuFullMeasuredDataSubscriberTest*" + + # test ImuIdentificationSubscriber + ./imu_ros2_test_node --gtest_filter="ImuIdentificationSubscriberTest*" + + # test ImuDiagSubscriber + ./imu_ros2_test_node --gtest_filter="ImuDiagSubscriberTest*" + +Run imu_ros2 node with remote client +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +If the ROS2 environment is running on a different processing unit (e.g. personal +computer) than the one to which the IMU is connected to (e.g. Raspberry Pi), +make sure the two processing units are connected +to the same network, and find out the IP address of the processing unit to which +the IMU is connected to (e.g. Raspberry Pi) then run the following command to +start the imu_ros2 node: + +.. code-block:: bash + + source install/setup.sh + ros2 run imu_ros2 imu_ros2_node --ros-args -p iio_context_string:="ip:'processing_unit_IP_address'" + +For executing system tests, run the following commands, after the imu_ros2 node +has been started: + +.. code-block:: bash + + source install/setup.sh + cd install/imu_ros2/lib/imu_ros2_test + + # set measured_data_topic_selection to 0 to test VelAngTempSubscriber (not available for adis1646x) + ros2 param set /imu_ros2_node measured_data_topic_selection 0 + ./imu_ros2_test_node --gtest_filter="VelAngTempSubscriberTest*" --ros-args -p iio_context_string:="ip:'processing_unit_IP_address'" + + # set measured_data_topic_selection to 1 to test AccelGyroTempSubscriber + ros2 param set /imu_ros2_node measured_data_topic_selection 1 + ./imu_ros2_test_node --gtest_filter="AccelGyroTempSubscriberTest*" --ros-args -p iio_context_string:="ip:'processing_unit_IP_address'" + + # set measured_data_topic_selection to 2 to test ImuSubscriber + ros2 param set /imu_ros2_node measured_data_topic_selection 2 + ./imu_ros2_test_node --gtest_filter="ImuSubscriberTest*" --ros-args -p iio_context_string:="ip:'processing_unit_IP_address'" + + # set measured_data_topic_selection to 3 to test ImuFullMeasuredDataSubscriber + ros2 param set /imu_ros2_node measured_data_topic_selection 3 + ./imu_ros2_test_node --gtest_filter="ImuFullMeasuredDataSubscriberTest*" --ros-args -p iio_context_string:="ip:'processing_unit_IP_address'" + + # test ImuIdentificationSubscriber + ./imu_ros2_test_node --gtest_filter="ImuIdentificationSubscriberTest*" --ros-args -p iio_context_string:="ip:'processing_unit_IP_address'" + + # test ImuDiagSubscriber + ./imu_ros2_test_node --gtest_filter="ImuDiagSubscriberTest*" --ros-args -p iio_context_string:="ip:'processing_unit_IP_address'" + +imu-ros2 description +-------------------- + +Published topics +^^^^^^^^^^^^^^^^ + +**imufullmeasureddata** topic contains acceleration, gyroscope, delta velocity, delta angle +and temperature data, acquired by polling the IMU device (data ready signal is ignored). +Messages are published on this topic when the **measured_data_topic_selection** parameter is set to 3. +This topic has the following definition: + +.. code-block:: bash + + std_msgs/Header header + geometry_msgs/Vector3 linear_acceleration + geometry_msgs/Vector3 angular_velocity + geometry_msgs/Vector3 delta_velocity + geometry_msgs/Vector3 delta_angle + float64 temperature + +**imu** topic it's the standard imu message type as described here: http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html. +Messages are published on this topic when the **measured_data_topic_selection** parameter is set to 2. +This topic has the following definition: + +.. code-block:: bash + + std_msgs/Header header + geometry_msgs/Quaternion orientation + float64[9] orientation_covariance + geometry_msgs/Vector3 angular_velocity + float64[9] angular_velocity_covariance + geometry_msgs/Vector3 linear_acceleration + float64[9] linear_acceleration_covariance + +**accelgyrotempdata** topic contains acceleration, gyroscope and temperature data, +acquired on each data ready impulse. +Messages are published on this topic when the **measured_data_topic_selection** parameter is set to 1. +This topic has the following definition: + +.. code-block:: bash + + std_msgs/Header header + geometry_msgs/Vector3 linear_acceleration + geometry_msgs/Vector3 angular_velocity + float64 temperature + +**velangtempdata** topic contains delta velocity, delta angle and temperature data, +acquired on each data ready impulse. +Messages are published on this topic when the **measured_data_topic_selection** parameter is set to 0. +Some devices do not support publishing messages with this type. +This topic has the following definition: + +.. code-block:: bash + + std_msgs/Header header + geometry_msgs/Vector3 delta_velocity + geometry_msgs/Vector3 delta_angle + float64 temperature + +**imudiagdata** topic contains various diagnosis flags, +Messages are published on this topic continuously. + +**imuidentificationdata** topic contains device specific identification data. +Messages are published on this topic continuously. +This topic has the following definition: + +.. code-block:: bash + + std_msgs/Header header + string firmware_revision + string firmware_date + uint32 product_id + uint32 serial_number + string gyroscope_measurement_range + +Node parameters +^^^^^^^^^^^^^^^ + +The imu_ros2 driver is using LibIIO and thus a LibIIO context should be given when +starting the node, using **iio_context_string** parameter. +If the parameter is not set, the default value will be used, which is 'local:', +suitable for running the imu_ros2 node on the Raspberry Pi. +If the imu_ros2 node is not running on the Raspberry Pi, the parameter should +be given when starting the imu_ros2 node and it should have the following format: +'ip:rpi_ip_address', where rpi_ip_address is the IP address of the Raspberry Pi. + +The imu_ros2 driver can publish the measured data in various mode, based on +**measured_data_topic_selection** parameter value, as shown below: + +* 0: measured data is published on /velangtempdata topic - not available for adis1646x; sampling is performed on each data ready impulse +* 1: measured data is published on /accelgyrotempdata topic; sampling is performed on each data ready impulse +* 2: measured data is published on /imu topic; sampling performed on each data ready impulse +* 3: measured data is published on /imufullmeasureddata topic (default); sampling is performed by polling the data registers without taking into consideration the data ready impulse + +IMU parameters +^^^^^^^^^^^^^^ + +The imu_ros2 driver allows for IMU configuration. Not all parameters +are available for a device. See https://github.com/analogdevicesinc/imu-ros2/tree/main/config for +chip specific configuration. + ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| Parameter Name | Parameter Description | Parameter type | Parameter Range | ADIS1646X | ADIS1647X | ADIS1650X | ADIS1657X | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| accel_calibbias_x | x-axis acceleration offset correction | integer | -2147483648 up to 2147483647, step 1 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| accel_calibbias_y | y-axis acceleration offset correction | integer | -2147483648 up to 2147483647, step 1 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| accel_calibbias_z | z-axis acceleration offset correction | integer | -2147483648 up to 2147483647, step 1 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| anglvel_calibbias_x | x-axis angular velocity offset correction | integer | -2147483648 up to 2147483647, step 1 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| anglvel_calibbias_y | y-axis angular velocity offset correction | integer | -2147483648 up to 2147483647, step 1 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| filter_low_pass_3db_frequency | Low pass 3db frequency | integer | 10, 20, 70, 80, 164, 360, 720 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| sampling_frequency | Device sampling frequency | double | 1.0 up to max | max = 2000.0 | max = 2000.0 | max = 2000.0 | max = 4000.0 | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| linear_acceleration_compensation | Linear acceleration compensation enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| point_of_percussion_alignment | Point of percussion alignment enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| bias_correction_time_base_control | Time base control | integer | 0 up to 12, step 1 | Supported | Supported | Not Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| x_axis_accelerometer_bias_correction_enable | x-axis accelerometer bias correction enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Not Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| y_axis_accelerometer_bias_correction_enable | y-axis accelerometer bias correction enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Not Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| z_axis_accelerometer_bias_correction_enable | z-axis accelerometer bias correction enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Not Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| x_axis_gyroscope_bias_correction_enable | x-axis gyroscope bias correction enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Not Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| y_axis_gyroscope_bias_correction_enable | y-axis gyroscope bias correction enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Not Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| z_axis_gyroscope_bias_correction_enable | z-axis gyroscope bias correction enable/disable | integer | 0 up to 1, step 1 | Supported | Supported | Not Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| command_to_execute | list of available commands to be executed, device specific | string | software_reset, flash_memory_test, flash_memory_update, sensor_self_test, factory_calibration_restore, bias_correction_update | All commands supported | All commands supported | All commands supported, except bias_correction_update | All commands supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ +| internal_sensor_bandwidth | Internal sensor bandwidth | integer | 0 for wide bandwidth, 1 for 370 Hz | Not Supported | Not Supported | Supported | Supported | ++---------------------------------------------+------------------------------------------------------------+----------------+-------------------------------------------------------------------------------------------------------------------------------+------------------------+------------------------+-------------------------------------------------------+------------------------+ + +Examples +-------- + +adis1646x +^^^^^^^^^ + +**Setup** + +adis1646x ROS2 driver with adis16467-1 connected to Raspberry Pi 4 +Used device-tree for adis16475 Linux driver: https://github.com/analogdevicesinc/linux/blob/rpi-6.1.y/arch/arm/boot/dts/overlays/adis16475-overlay.dts +config.txt entries for device-tree overlay: + +.. code-block:: bash + + dtoverlay=adis16475 + dtparam=device="adi,adis16467-1" + dtparam=drdy_gpio25 + +The image below shows how the adis16467-1 device is connected to Raspberry Pi 4 using +EVAL-ADISIMU1-RPIZ using Mounting Slot I with P7 Connector: + +.. image:: adis16467_1_rpi.jpg + :align: center + :alt: ADIS16467-1 with RPI4 + +**Topic list** + +.. code-block:: bash + + ➜ ros2 topic list + /accelgyrotempdata + /imu + /imudiagdata + /imufullmeasureddata + /imuidentificationdata + +**Parameter list** + +.. code-block:: bash + + ➜ ros2 param list imu_ros2_node + accel_calibbias_x + accel_calibbias_y + accel_calibbias_z + anglvel_calibbias_x + anglvel_calibbias_y + anglvel_calibbias_z + bias_correction_time_base_control + command_to_execute + filter_low_pass_3db_frequency + iio_context_string + linear_acceleration_compensation + measured_data_topic_selection + point_of_percussion_alignment + sampling_frequency + x_axis_accelerometer_bias_correction_enable + x_axis_gyroscope_bias_correction_enable + y_axis_accelerometer_bias_correction_enable + y_axis_gyroscope_bias_correction_enable + z_axis_accelerometer_bias_correction_enable + z_axis_gyroscope_bias_correction_enable + +**Parameter dump** + +.. code-block:: bash + + ➜ ros2 param dump /imu_ros2_node + /imu_ros2_node: + ros__parameters: + accel_calibbias_x: 0 + accel_calibbias_y: 0 + accel_calibbias_z: 0 + anglvel_calibbias_x: 0 + anglvel_calibbias_y: 0 + anglvel_calibbias_z: 0 + bias_correction_time_base_control: 10 + command_to_execute: no_command + filter_low_pass_3db_frequency: 720 + iio_context_string: ip:192.168.0.1 + linear_acceleration_compensation: 1 + measured_data_topic_selection: 3 + point_of_percussion_alignment: 1 + sampling_frequency: 2000.0 + x_axis_accelerometer_bias_correction_enable: 0 + x_axis_gyroscope_bias_correction_enable: 1 + y_axis_accelerometer_bias_correction_enable: 0 + y_axis_gyroscope_bias_correction_enable: 1 + z_axis_accelerometer_bias_correction_enable: 0 + z_axis_gyroscope_bias_correction_enable: 1 + +**Topic echo accelgyrotempdata** + +.. code-block:: bash + + ➜ ros2 param set /imu_ros2_node measured_data_topic_selection 1 + Set parameter successful + ➜ ros2 topic echo accelgyrotempdata + header: + stamp: + sec: 1698751163 + nanosec: 610640655 + frame_id: accelgyrotempdata + linear_acceleration: + x: -0.12255231999999999 + y: 0.49020927999999997 + z: 10.245373952 + angular_velocity: + x: 0.0077987840000000004 + y: -0.008912896 + z: 0.0023592960000000003 + temperature: 37.9 + --- + header: + stamp: + sec: 1698751163 + nanosec: 611141470 + frame_id: accelgyrotempdata + linear_acceleration: + x: 0.036765696 + y: 0.47795404799999996 + z: 10.147332096 + angular_velocity: + x: 0.0057671680000000005 + y: -0.006553600000000001 + z: 0.0 + temperature: 37.9 + + +**Topic echo imu** + +.. code-block:: bash + + ➜ ros2 param set /imu_ros2_node measured_data_topic_selection 2 + Set parameter successful + ➜ ros2 topic echo imu + header: + stamp: + sec: 1698746841 + nanosec: 951239970 + frame_id: imu + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + orientation_covariance: + -1.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + angular_velocity: + x: -0.0021626880000000003 + y: -0.005046272 + z: -0.0015728640000000002 + angular_velocity_covariance: + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + linear_acceleration: + x: -0.073531392 + y: 0.0 + z: 9.98801408 + linear_acceleration_covariance: + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + +**Topic echo imufullmeasureddata** + +.. code-block:: bash + + ➜ ros2 param set /imu_ros2_node measured_data_topic_selection 3 + Set parameter successful + ➜ ros2 topic echo imufullmeasureddata + header: + stamp: + sec: 1698747556 + nanosec: 755176752 + frame_id: imufullmeasureddata + linear_acceleration: + x: 0.06316448599999999 + y: 0.04266031 + z: 9.86933827 + angular_velocity: + x: -0.000793858 + y: -0.001786835 + z: -0.0019665010000000003 + delta_velocity: + x: 2.3436e-05 + y: 8.184e-06 + z: 0.004929186 + delta_angle: + x: 2.7e-07 + y: -1.28e-06 + z: -5.96e-07 + temperature: 41.0 + --- + header: + stamp: + sec: 1698747556 + nanosec: 760426222 + frame_id: imufullmeasureddata + linear_acceleration: + x: 0.003555244 + y: 0.008432765 + z: 9.839817141 + angular_velocity: + x: 4.0902e-05 + y: -0.003819203 + z: -0.0019095020000000002 + delta_velocity: + x: 1.6926e-05 + y: -3.348e-06 + z: 0.004927512 + delta_angle: + x: 1.98e-07 + y: -1.356e-06 + z: -1.092e-06 + temperature: 41.0 + +**Topic echo imuidentificationdata** + +.. code-block:: bash + + ➜ ros2 topic echo /imuidentificationdata + header: + stamp: + sec: 1698747693 + nanosec: 960557599 + frame_id: imuidentificationdata + firmware_revision: '1.6' + firmware_date: 08-29-2017 + product_id: 16467 + serial_number: 107 + gyroscope_measurement_range: +/-125_degrees_per_sec + +**Topic echo imudiagdata** + +.. code-block:: bash + + ➜ ros2 topic echo /imudiagdata + header: + stamp: + sec: 1698747757 + nanosec: 309115737 + frame_id: imudiagdata + diag_data_path_overrun: false + diag_flash_memory_update_error: false + diag_spi_communication_error: false + diag_standby_mode: false + diag_sensor_self_test_error: false + diag_flash_memory_test_error: false + diag_clock_error: false + diag_flash_memory_write_count_exceeded_error: false + flash_counter: 14 + +adis1650x +^^^^^^^^^ + +**Setup** + +adis1650x ROS2 driver with adis16505-2 connected to Raspberry Pi 4 using a ribbon cable +Used device-tree for adis16475 Linux driver: https://github.com/analogdevicesinc/linux/blob/rpi-6.1.y/arch/arm/boot/dts/overlays/adis16475-overlay.dts +config.txt entries for device-tree overlay: + +.. code-block:: bash + + dtoverlay=adis16475 + dtparam=device="adi,adis16505-2" + + +The image below shows how the adis16467-1 device is connected to Raspberry Pi 4 using +EVAL-ADISIMU1-RPIZ using Mounting Slot I with P7 Connector: + +.. image:: adis16505_2_rpi.jpg + :align: center + :alt: ADIS16505-2 with RPI4 + +**Topic list** + +.. code-block:: bash + + ➜ ros2 topic list + /accelgyrotempdata + /imu + /imudiagdata + /imufullmeasureddata + /imuidentificationdata + /velangtempdata + +**Parameter list** + +.. code-block:: bash + + ➜ ros2 param list imu_ros2_node + accel_calibbias_x + accel_calibbias_y + accel_calibbias_z + anglvel_calibbias_x + anglvel_calibbias_y + anglvel_calibbias_z + command_to_execute + filter_low_pass_3db_frequency + iio_context_string + internal_sensor_bandwidth + linear_acceleration_compensation + measured_data_topic_selection + point_of_percussion_alignment + sampling_frequency + +**Parameter dump** + +.. code-block:: bash + + ➜ ros2 param dump /imu_ros2_node + /imu_ros2_node: + ros__parameters: + accel_calibbias_x: 0 + accel_calibbias_y: 0 + accel_calibbias_z: 0 + anglvel_calibbias_x: 0 + anglvel_calibbias_y: 0 + anglvel_calibbias_z: 0 + command_to_execute: no_command + filter_low_pass_3db_frequency: 720 + iio_context_string: ip:192.168.0.1 + internal_sensor_bandwidth: 0 + linear_acceleration_compensation: 1 + measured_data_topic_selection: 3 + point_of_percussion_alignment: 1 + sampling_frequency: 2000.0 + +**Topic echo velangtempdata** + +.. code-block:: bash + + ➜ ros2 param set /imu_ros2_node measured_data_topic_selection 0 + Set parameter successful + ➜ ros2 topic echo velangtempdata + header: + stamp: + sec: 1698753051 + nanosec: 211437438 + frame_id: velangtempdata + delta_velocity: + x: 0.0 + y: 0.0 + z: 0.003014656 + delta_angle: + x: -0.000393216 + y: 0.0 + z: 0.0 + temperature: 32.7 + --- + header: + stamp: + sec: 1698753051 + nanosec: 212438253 + frame_id: velangtempdata + delta_velocity: + x: 0.0 + y: 0.0 + z: 0.003014656 + delta_angle: + x: -0.000393216 + y: 0.0 + z: -0.000393216 + temperature: 32.7 + + +**Topic echo accelgyrotempdata** + +.. code-block:: bash + + ➜ ros2 param set /imu_ros2_node measured_data_topic_selection 1 + Set parameter successful + ➜ ros2 topic echo accelgyrotempdata + header: + stamp: + sec: 1698752813 + nanosec: 865099477 + frame_id: accelgyrotempdata + linear_acceleration: + x: 1.513095168 + y: 6.326386688 + z: 7.5776 + angular_velocity: + x: -0.012189696 + y: 0.009437184 + z: 0.001572864 + temperature: 32.6 + --- + header: + stamp: + sec: 1698752813 + nanosec: 865597207 + frame_id: accelgyrotempdata + linear_acceleration: + x: 1.522794496 + y: 6.1833216 + z: 7.616397312 + angular_velocity: + x: -0.017301504 + y: 0.009437184 + z: 0.001179648 + temperature: 32.6 + + +**Topic echo imu** + +.. code-block:: bash + + ➜ ros2 param set /imu_ros2_node measured_data_topic_selection 2 + Set parameter successful + ➜ ros2 topic echo imu + header: + stamp: + sec: 1698752385 + nanosec: 697059208 + frame_id: imu + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + orientation_covariance: + -1.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + angular_velocity: + x: -0.022413312 + y: 0.005111808 + z: 0.003538944 + angular_velocity_covariance: + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + linear_acceleration: + x: 1.52764416 + y: 4.917559296 + z: 7.866155008 + linear_acceleration_covariance: + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + +**Topic echo imufullmeasureddata** + +.. code-block:: bash + + ➜ ros2 param set /imu_ros2_node measured_data_topic_selection 3 + Set parameter successful + ➜ ros2 topic echo imufullmeasureddata + header: + stamp: + sec: 1698752480 + nanosec: 32117474 + frame_id: imufullmeasureddata + linear_acceleration: + x: 1.498841658 + y: 5.758833587 + z: 7.699544637 + angular_velocity: + x: 0.0180183 + y: 0.006585252 + z: -0.003338532 + delta_velocity: + x: 0.000749064 + y: 0.002869894 + z: 0.003841414 + delta_angle: + x: 1.1514e-05 + y: 4.05e-06 + z: -2.0039999999999998e-06 + temperature: 32.1 + --- + header: + stamp: + sec: 1698752480 + nanosec: 47051862 + frame_id: imufullmeasureddata + linear_acceleration: + x: 1.500955246 + y: 5.752098773 + z: 7.669287517 + angular_velocity: + x: 0.014685954 + y: 0.008353032 + z: 0.000134526 + delta_velocity: + x: 0.000747822 + y: 0.002868008 + z: 0.003832168 + delta_angle: + x: 7.824e-06 + y: 4.956e-06 + z: -1.488e-06 + temperature: 32.2 + + +**Topic echo imuidentificationdata** + +.. code-block:: bash + + ➜ ros2 topic echo /imuidentificationdata + header: + stamp: + sec: 1698752760 + nanosec: 145522368 + frame_id: imuidentificationdata + firmware_revision: '1.6' + firmware_date: 06-27-2019 + product_id: 16505 + serial_number: 1208 + gyroscope_measurement_range: +/-500_degrees_per_sec + +**Topic echo imudiagdata** + +.. code-block:: bash + + ➜ ros2 topic echo /imudiagdata + header: + stamp: + sec: 1698752709 + nanosec: 500543873 + frame_id: imudiagdata + diag_data_path_overrun: false + diag_flash_memory_update_error: false + diag_spi_communication_error: false + diag_standby_mode: false + diag_sensor_self_test_error: false + diag_flash_memory_test_error: false + diag_clock_error: false + diag_acceleration_self_test_error: false + diag_gyroscope1_self_test_error: false + diag_gyroscope2_self_test_error: false + diag_flash_memory_write_count_exceeded_error: false + flash_counter: 22 + diff --git a/adis16467_1_rpi.jpg b/adis16467_1_rpi.jpg new file mode 100644 index 0000000..e89d3bc Binary files /dev/null and b/adis16467_1_rpi.jpg differ diff --git a/adis16505_2_rpi.jpg b/adis16505_2_rpi.jpg new file mode 100644 index 0000000..b973677 Binary files /dev/null and b/adis16505_2_rpi.jpg differ diff --git a/architecture_local_client.png b/architecture_local_client.png new file mode 100644 index 0000000..b1d4877 Binary files /dev/null and b/architecture_local_client.png differ diff --git a/architecture_remote_client.png b/architecture_remote_client.png new file mode 100644 index 0000000..0d4243d Binary files /dev/null and b/architecture_remote_client.png differ diff --git a/config/adis1646x/imu_config.yaml b/config/adis1646x/imu_config.yaml new file mode 100644 index 0000000..1b5b93b --- /dev/null +++ b/config/adis1646x/imu_config.yaml @@ -0,0 +1,21 @@ +/imu_ros2_node: + ros__parameters: + accel_calibbias_x: 0 + accel_calibbias_y: 0 + accel_calibbias_z: 0 + anglvel_calibbias_x: 0 + anglvel_calibbias_y: 0 + anglvel_calibbias_z: 0 + filter_low_pass_3db_frequency: 720 + point_of_percussion_alignment: 1 + linear_acceleration_compensation: 1 + bias_correction_time_base_control: 10 + x_axis_gyroscope_bias_correction_enable: 0 + y_axis_gyroscope_bias_correction_enable: 0 + z_axis_gyroscope_bias_correction_enable: 0 + x_axis_accelerometer_bias_correction_enable: 1 + y_axis_accelerometer_bias_correction_enable: 1 + z_axis_accelerometer_bias_correction_enable: 1 + sampling_frequency: 2000.0 + command_to_execute: no_command + measured_data_topic_selection: 3 diff --git a/config/adis1647x/imu_config.yaml b/config/adis1647x/imu_config.yaml new file mode 100644 index 0000000..1b5b93b --- /dev/null +++ b/config/adis1647x/imu_config.yaml @@ -0,0 +1,21 @@ +/imu_ros2_node: + ros__parameters: + accel_calibbias_x: 0 + accel_calibbias_y: 0 + accel_calibbias_z: 0 + anglvel_calibbias_x: 0 + anglvel_calibbias_y: 0 + anglvel_calibbias_z: 0 + filter_low_pass_3db_frequency: 720 + point_of_percussion_alignment: 1 + linear_acceleration_compensation: 1 + bias_correction_time_base_control: 10 + x_axis_gyroscope_bias_correction_enable: 0 + y_axis_gyroscope_bias_correction_enable: 0 + z_axis_gyroscope_bias_correction_enable: 0 + x_axis_accelerometer_bias_correction_enable: 1 + y_axis_accelerometer_bias_correction_enable: 1 + z_axis_accelerometer_bias_correction_enable: 1 + sampling_frequency: 2000.0 + command_to_execute: no_command + measured_data_topic_selection: 3 diff --git a/config/imu16505_config.yaml b/config/adis1650x/imu_config.yaml similarity index 100% rename from config/imu16505_config.yaml rename to config/adis1650x/imu_config.yaml diff --git a/config/imu1657x_config.yaml b/config/adis1657x/imu_config.yaml similarity index 100% rename from config/imu1657x_config.yaml rename to config/adis1657x/imu_config.yaml diff --git a/include/imu_ros2/adis1646x/adis1646x_data_access.h b/include/imu_ros2/adis1646x/adis1646x_data_access.h new file mode 100644 index 0000000..3363ed7 --- /dev/null +++ b/include/imu_ros2/adis1646x/adis1646x_data_access.h @@ -0,0 +1,65 @@ +#ifndef ADIS1646X_DATA_ACCESS_H +#define ADIS1646X_DATA_ACCESS_H + +#define ADIS_FLS_MEM_ENDURANCE 10000 +#define ADIS_MAX_SAMP_FREQ 2000.0 + +#define ADIS_DIAG_STAT_ADDR 0x02 +#define ADIS_DATA_PATH_OVERRUN_POS 1 +#define ADIS_FLS_MEM_UPDATE_FAIL_POS 2 +#define ADIS_SPI_COMM_ERR_POS 3 +#define ADIS_STDBY_MODE_POS 4 +#define AIDS_SNSR_FAIL_POS 5 +#define ADIS_MEM_FAIL_POS 6 +#define ADIS_CLK_ERR_POS 7 + +#define ADIS_DATA_PATH_OVERRUN (1 << ADIS_DATA_PATH_OVERRUN_POS) +#define ADIS_FLS_MEM_UPDATE_FAIL (1 << ADIS_FLS_MEM_UPDATE_FAIL_POS) +#define ADIS_SPI_COMM_ERR (1 << ADIS_SPI_COMM_ERR_POS) +#define ADIS_STDBY_MODE (1 << ADIS_STDBY_MODE_POS) +#define AIDS_SNSR_FAIL (1 << AIDS_SNSR_FAIL_POS) +#define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS) +#define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS) + +#define ADIS_RANG_MDL_ADDR 0x5E +#define ADIS_GYRO_MEAS_RANG_POS 2 + +#define ADIS_GYRO_MEAS_RANG (3 << ADIS_GYRO_MEAS_RANG_POS) + +#define ADIS_MSC_CTRL_ADDR 0x60 +#define ADIS_DR_POL_POS 0 +#define ADIS_SYNC_POL_POS 1 +#define ADIS_PT_OF_PERG_ALGNMNT_POS 6 +#define ADIS_LN_ACCL_COMP_POS 7 + +#define ADIS_DR_POL (1 << ADIS_DR_POL_POS) +#define ADIS_SYNC_POL (1 << ADIS_SYNC_POL_POS) +#define ADIS_PT_OF_PERG_ALGNMNT (1 << ADIS_PT_OF_PERG_ALGNMNT_POS) +#define ADIS_LN_ACCL_COMP (1 << ADIS_LN_ACCL_COMP_POS) + +#define ADIS_NULL_CNFG_ADDR 0x66 +#define ADIS_TIME_BASE_CONTROL_POS 0 +#define ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS 8 +#define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS 9 +#define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS 10 +#define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS 11 +#define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS 12 +#define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS 13 + +#define ADIS_TIME_BASE_CONTROL 0xF +#define ADIS_X_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS) +#define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS) +#define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS) + +#define ADIS_GLOB_CMD_ADDR 0x68 +#define ADIS_BIAS_CORRECTION_UPDATE (1 << 0) +#define ADIS_FACTORY_CALIBRATION_RESTORE (1 << 1) +#define ADIS_SENSOR_SELF_TEST (1 << 2) +#define ADIS_FLASH_MEMORY_UPDATE (1 << 3) +#define ADIS_FLASH_MEMORY_TEST (1 << 4) +#define ADIS_SOFTWARE_RESET_CMD (1 << 7) + +#endif diff --git a/include/imu_ros2/adis1647x/adis1647x_data_access.h b/include/imu_ros2/adis1647x/adis1647x_data_access.h new file mode 100644 index 0000000..cd3f527 --- /dev/null +++ b/include/imu_ros2/adis1647x/adis1647x_data_access.h @@ -0,0 +1,67 @@ +#ifndef ADIS1647X_DATA_ACCESS_H +#define ADIS1647X_DATA_ACCESS_H + +#define ADIS_HAS_DELTA_BURST + +#define ADIS_FLS_MEM_ENDURANCE 10000 +#define ADIS_MAX_SAMP_FREQ 2000.0 + +#define ADIS_DIAG_STAT_ADDR 0x02 +#define ADIS_DATA_PATH_OVERRUN_POS 1 +#define ADIS_FLS_MEM_UPDATE_FAIL_POS 2 +#define ADIS_SPI_COMM_ERR_POS 3 +#define ADIS_STDBY_MODE_POS 4 +#define AIDS_SNSR_FAIL_POS 5 +#define ADIS_MEM_FAIL_POS 6 +#define ADIS_CLK_ERR_POS 7 + +#define ADIS_DATA_PATH_OVERRUN (1 << ADIS_DATA_PATH_OVERRUN_POS) +#define ADIS_FLS_MEM_UPDATE_FAIL (1 << ADIS_FLS_MEM_UPDATE_FAIL_POS) +#define ADIS_SPI_COMM_ERR (1 << ADIS_SPI_COMM_ERR_POS) +#define ADIS_STDBY_MODE (1 << ADIS_STDBY_MODE_POS) +#define AIDS_SNSR_FAIL (1 << AIDS_SNSR_FAIL_POS) +#define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS) +#define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS) + +#define ADIS_RANG_MDL_ADDR 0x5E +#define ADIS_GYRO_MEAS_RANG_POS 2 + +#define ADIS_GYRO_MEAS_RANG (3 << ADIS_GYRO_MEAS_RANG_POS) + +#define ADIS_MSC_CTRL_ADDR 0x60 +#define ADIS_DR_POL_POS 0 +#define ADIS_SYNC_POL_POS 1 +#define ADIS_PT_OF_PERG_ALGNMNT_POS 6 +#define ADIS_LN_ACCL_COMP_POS 7 + +#define ADIS_DR_POL (1 << ADIS_DR_POL_POS) +#define ADIS_SYNC_POL (1 << ADIS_SYNC_POL_POS) +#define ADIS_PT_OF_PERG_ALGNMNT (1 << ADIS_PT_OF_PERG_ALGNMNT_POS) +#define ADIS_LN_ACCL_COMP (1 << ADIS_LN_ACCL_COMP_POS) + +#define ADIS_NULL_CNFG_ADDR 0x66 +#define ADIS_TIME_BASE_CONTROL_POS 0 +#define ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS 8 +#define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS 9 +#define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS 10 +#define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS 11 +#define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS 12 +#define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS 13 + +#define ADIS_TIME_BASE_CONTROL 0xF +#define ADIS_X_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS) +#define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS) +#define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS) + +#define ADIS_GLOB_CMD_ADDR 0x68 +#define ADIS_BIAS_CORRECTION_UPDATE (1 << 0) +#define ADIS_FACTORY_CALIBRATION_RESTORE (1 << 1) +#define ADIS_SENSOR_SELF_TEST (1 << 2) +#define ADIS_FLASH_MEMORY_UPDATE (1 << 3) +#define ADIS_FLASH_MEMORY_TEST (1 << 4) +#define ADIS_SOFTWARE_RESET_CMD (1 << 7) + +#endif diff --git a/include/imu_ros2/adis1650x/adis1650x_data_access.h b/include/imu_ros2/adis1650x/adis1650x_data_access.h new file mode 100644 index 0000000..4b318e0 --- /dev/null +++ b/include/imu_ros2/adis1650x/adis1650x_data_access.h @@ -0,0 +1,57 @@ +#ifndef ADIS1650X_DATA_ACCESS_H +#define ADIS1650X_DATA_ACCESS_H + +#define ADIS_HAS_DELTA_BURST + +#define ADIS_FLS_MEM_ENDURANCE 10000 +#define ADIS_MAX_SAMP_FREQ 2000.0 + +#define ADIS_DIAG_STAT_ADDR 0x02 +#define ADIS_DATA_PATH_OVERRUN_POS 1 +#define ADIS_FLS_MEM_UPDATE_FAIL_POS 2 +#define ADIS_SPI_COMM_ERR_POS 3 +#define ADIS_STDBY_MODE_POS 4 +#define AIDS_SNSR_FAIL_POS 5 +#define ADIS_MEM_FAIL_POS 6 +#define ADIS_CLK_ERR_POS 7 +#define ADIS_GYRO1_FAIL_POS 8 +#define ADIS_GYRO2_FAIL_POS 9 +#define ADIS_ACCEL_FAIL_POS 10 + +#define ADIS_DATA_PATH_OVERRUN (1 << ADIS_DATA_PATH_OVERRUN_POS) +#define ADIS_FLS_MEM_UPDATE_FAIL (1 << ADIS_FLS_MEM_UPDATE_FAIL_POS) +#define ADIS_SPI_COMM_ERR (1 << ADIS_SPI_COMM_ERR_POS) +#define ADIS_STDBY_MODE (1 << ADIS_STDBY_MODE_POS) +#define AIDS_SNSR_FAIL (1 << AIDS_SNSR_FAIL_POS) +#define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS) +#define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS) +#define ADIS_GYRO1_FAIL (1 << ADIS_GYRO1_FAIL_POS) +#define ADIS_GYRO2_FAIL (1 << ADIS_GYRO2_FAIL_POS) +#define ADIS_ACCEL_FAIL (1 << ADIS_ACCEL_FAIL_POS) + +#define ADIS_RANG_MDL_ADDR 0x5E +#define ADIS_GYRO_MEAS_RANG_POS 2 + +#define ADIS_GYRO_MEAS_RANG (3 << ADIS_GYRO_MEAS_RANG_POS) + +#define ADIS_MSC_CTRL_ADDR 0x60 +#define ADIS_DR_POL_POS 0 +#define ADIS_SYNC_POL_POS 1 +#define ADIS_SENS_BW_POS 4 +#define ADIS_PT_OF_PERG_ALGNMNT_POS 6 +#define ADIS_LN_ACCL_COMP_POS 7 + +#define ADIS_DR_POL (1 << ADIS_DR_POL_POS) +#define ADIS_SYNC_POL (1 << ADIS_SYNC_POL_POS) +#define ADIS_SENS_BW (1 << ADIS_SENS_BW_POS) +#define ADIS_PT_OF_PERG_ALGNMNT (1 << ADIS_PT_OF_PERG_ALGNMNT_POS) +#define ADIS_LN_ACCL_COMP (1 << ADIS_LN_ACCL_COMP_POS) + +#define ADIS_GLOB_CMD_ADDR 0x68 +#define ADIS_FACTORY_CALIBRATION_RESTORE (1 << 1) +#define ADIS_SENSOR_SELF_TEST (1 << 2) +#define ADIS_FLASH_MEMORY_UPDATE (1 << 3) +#define ADIS_FLASH_MEMORY_TEST (1 << 4) +#define ADIS_SOFTWARE_RESET_CMD (1 << 7) + +#endif diff --git a/include/imu_ros2/adis1657x/adis1657x_data_access.h b/include/imu_ros2/adis1657x/adis1657x_data_access.h new file mode 100644 index 0000000..4f2e09d --- /dev/null +++ b/include/imu_ros2/adis1657x/adis1657x_data_access.h @@ -0,0 +1,84 @@ +#ifndef ADIS1657X_DATA_ACCESS_H +#define ADIS1657X_DATA_ACCESS_H + +#define ADIS_HAS_DELTA_BURST + +#define ADIS_FLS_MEM_ENDURANCE 100000 +#define ADIS_MAX_SAMP_FREQ 4000.0 + +#define ADIS_DIAG_STAT_ADDR 0x02 +#define ADIS_SNSR_INIT_FAIL_POS 0 +#define ADIS_DATA_PATH_OVERRUN_POS 1 +#define ADIS_FLS_MEM_UPDATE_FAIL_POS 2 +#define ADIS_SPI_COMM_ERR_POS 3 +#define ADIS_STDBY_MODE_POS 4 +#define AIDS_SNSR_FAIL_POS 5 +#define ADIS_MEM_FAIL_POS 6 +#define ADIS_CLK_ERR_POS 7 +#define ADIS_GYRO_X_FAIL_POS 8 +#define ADIS_GYRO_Y_FAIL_POS 9 +#define ADIS_GYRO_Z_FAIL_POS 10 +#define ADIS_ACCEL_X_FAIL_POS 11 +#define ADIS_ACCEL_Y_FAIL_POS 12 +#define ADIS_ACCEL_Z_FAIL_POS 13 +#define ADIS_ADUC_MCU_FAULT_POS 15 + +#define ADIS_DATA_PATH_OVERRUN (1 << ADIS_DATA_PATH_OVERRUN_POS) +#define ADIS_FLS_MEM_UPDATE_FAIL (1 << ADIS_FLS_MEM_UPDATE_FAIL_POS) +#define ADIS_SPI_COMM_ERR (1 << ADIS_SPI_COMM_ERR_POS) +#define ADIS_STDBY_MODE (1 << ADIS_STDBY_MODE_POS) +#define AIDS_SNSR_FAIL (1 << AIDS_SNSR_FAIL_POS) +#define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS) +#define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS) +#define ADIS_GYRO_X_FAIL (1 << ADIS_GYRO_X_FAIL_POS) +#define ADIS_GYRO_Y_FAIL (1 << ADIS_GYRO_Y_FAIL_POS) +#define ADIS_GYRO_Z_FAIL (1 << ADIS_GYRO_Z_FAIL_POS) +#define ADIS_ACCEL_X_FAIL (1 << ADIS_ACCEL_X_FAIL_POS) +#define ADIS_ACCEL_Y_FAIL (1 << ADIS_ACCEL_Y_FAIL_POS) +#define ADIS_ACCEL_Z_FAIL (1 << ADIS_ACCEL_Z_FAIL_POS) +#define ADIS_ADUC_MCU_FAULT (1 << ADIS_ADUC_MCU_FAULT_POS) + +#define ADIS_RANG_MDL_ADDR 0x5E +#define ADIS_GYRO_MEAS_RANG_POS 2 + +#define ADIS_GYRO_MEAS_RANG (3 << ADIS_GYRO_MEAS_RANG_POS) + +#define ADIS_MSC_CTRL_ADDR 0x60 +#define ADIS_DR_POL_POS 0 +#define ADIS_SYNC_POL_POS 1 +#define ADIS_PT_OF_PERG_ALGNMNT_POS 6 +#define ADIS_LN_ACCL_COMP_POS 7 +#define ADIS_SENS_BW_POS 12 + +#define ADIS_DR_POL (1 << ADIS_DR_POL_POS) +#define ADIS_SYNC_POL (1 << ADIS_SYNC_POL_POS) +#define ADIS_PT_OF_PERG_ALGNMNT (1 << ADIS_PT_OF_PERG_ALGNMNT_POS) +#define ADIS_LN_ACCL_COMP (1 << ADIS_LN_ACCL_COMP_POS) +#define ADIS_SENS_BW (1 << ADIS_SENS_BW_POS) + +#define ADIS_NULL_CNFG_ADDR 0x66 +#define ADIS_TIME_BASE_CONTROL_POS 0 +#define ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS 8 +#define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS 9 +#define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS 10 +#define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS 11 +#define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS 12 +#define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS 13 + +#define ADIS_TIME_BASE_CONTROL 0xF +#define ADIS_X_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS) +#define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS) +#define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS) +#define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS) + +#define ADIS_GLOB_CMD_ADDR 0x68 +#define ADIS_BIAS_CORRECTION_UPDATE (1 << 0) +#define ADIS_FACTORY_CALIBRATION_RESTORE (1 << 1) +#define ADIS_SENSOR_SELF_TEST (1 << 2) +#define ADIS_FLASH_MEMORY_UPDATE (1 << 3) +#define ADIS_FLASH_MEMORY_TEST (1 << 4) +#define ADIS_SOFTWARE_RESET_CMD (1 << 7) + +#endif diff --git a/include/imu_ros2/adis_data_access.h b/include/imu_ros2/adis_data_access.h new file mode 100644 index 0000000..646ecc0 --- /dev/null +++ b/include/imu_ros2/adis_data_access.h @@ -0,0 +1,21 @@ +#ifndef ADIS_DATA_ACCESS_H +#define ADIS_DATA_ACCESS_H + +#define DELTAVEL_DELTAANG_BUFFERED_DATA 0 +#define ACCEL_GYRO_BUFFERED_DATA 1 +#define IMU_STD_MSG_DATA 2 +#define FULL_MEASURED_DATA 3 + +#if defined(ADIS1646X) +#include "adis1646x/adis1646x_data_access.h" +#elif defined(ADIS1647X) +#include "adis1647x/adis1647x_data_access.h" +#elif defined(ADIS1650X) +#include "adis1650x/adis1650x_data_access.h" +#elif defined(ADIS1657X) +#include "adis1657x/adis1657x_data_access.h" +#else +#error "No device defined." +#endif + +#endif diff --git a/include/imu_ros2/iio_wrapper.h b/include/imu_ros2/iio_wrapper.h index 0d431bb..dfcf8eb 100644 --- a/include/imu_ros2/iio_wrapper.h +++ b/include/imu_ros2/iio_wrapper.h @@ -25,7 +25,7 @@ #include -#include "setting_declarations.h" +#include "adis_data_access.h" /** * @brief Wrapper class for libiio library for IMU devices @@ -43,15 +43,23 @@ class IIOWrapper */ ~IIOWrapper(); + /** + * @brief create IIO context based on the given context string and search for + * IIO ADIS device, enable trigger and map device attributes, device channels + * and channels attributes. + * @param context IIO context string + */ + void createContext(const char * context); + /** * @brief Update buffer data. This function should be called before retrieving * data using getBuff** APIs. - * @param burst_data_selection: 0 for acceleration and gyroscope data, 1 for + * @param data_selection 0 for acceleration and gyroscope data, 1 for * delta angle and delta velocity data. * @return Return true if data was read successfully and can be retrieved, * false otherwise. */ - bool updateBuffer(uint32_t burst_data_selection); + bool updateBuffer(uint32_t data_selection); /** * @brief Stops buffer acquisition. @@ -106,6 +114,7 @@ class IIOWrapper */ double getBuffAngularVelocityZ(); +#ifdef ADIS_HAS_DELTA_BURST /** * @brief Get delta velocity on x axis with buffer reads; in this case * the retrieved samples are continuous if the function is called fast enough @@ -134,7 +143,7 @@ class IIOWrapper * @brief Get delta angle on x axis with buffer reads; in this case * the retrieved samples are continuous if the function is called fast enough * and samples are not overwritten. - * @return Return the delta angle on x axis in degrees. + * @return Return the delta angle on x axis in radians. */ double getBuffDeltaAngleX(); @@ -142,7 +151,7 @@ class IIOWrapper * @brief Get delta angle on y axis with buffer reads; in this case * the retrieved samples are continuous if the function is called fast enough * and samples are not overwritten. - * @return Return the delta angle on y axis in degrees. + * @return Return the delta angle on y axis in radians. */ double getBuffDeltaAngleY(); @@ -150,9 +159,10 @@ class IIOWrapper * @brief Get delta angle on z axis with buffer reads; in this case * the retrieved samples are continuous if the function is called fast enough * and samples are not overwritten. - * @return Return the delta angle on z axis in degrees. + * @return Return the delta angle on z axis in radians. */ double getBuffDeltaAngleZ(); +#endif /** * @brief Get temperature with buffer reads; in this case @@ -165,9 +175,10 @@ class IIOWrapper /** * @brief Get buffer timestamp when performing buffer reads; the timestamp * represent the time at which the samples from the devices were read over SPI. - * @return Return the sample timestamp in ticks. + * @param sec The second component of the timestamp + * @param nanosec The nanosecond component of the timestamp */ - int64_t getBuffSampleTimestamp(); + void getBuffSampleTimestamp(int32_t & sec, uint32_t & nanosec); /** * @brief Get linear acceleration on x axis with register reads; in this case @@ -228,7 +239,7 @@ class IIOWrapper * the retrieved samples are not necessary continuous. * @return Return true if the reading was successful and result is valid, * false otherwise. - * @param result The delta angle on x axis in degrees. + * @param result The delta angle on x axis in radians. */ bool getRegDeltaAngleX(double & result); @@ -237,7 +248,7 @@ class IIOWrapper * the retrieved samples are not necessary continuous. * @return Return true if the reading was successful and result is valid, * false otherwise. - * @param result The delta angle on y axis in degrees. + * @param result The delta angle on y axis in radians. */ bool getRegDeltaAngleY(double & result); @@ -246,7 +257,7 @@ class IIOWrapper * the retrieved samples are not necessary continuous. * @return Return true if the reading was successful and result is valid, * false otherwise. - * @param result The delta angle on z axis in degrees. + * @param result The delta angle on z axis in radians. */ bool getRegDeltaAngleZ(double & result); @@ -542,14 +553,6 @@ class IIOWrapper */ bool diag_aduc_mcu_fault(bool & result); - /** - * @brief Get diag checksum error flag data. - * @param result True if failure occurred, false otherwise. - * @return Return true if reading was successful and data is valid, false - * otherwise. - */ - bool diag_checksum_error_flag(bool & result); - /** * @brief Get diag flash memory write count exceeded error data. * @param result True if write count exceeded allowed value, false otherwise. @@ -558,14 +561,6 @@ class IIOWrapper */ bool diag_flash_memory_write_count_exceeded_error(bool & result); - /** - * @brief Get lost samples count data. - * @param result lost samples count - * @return Return true if reading was successful and data is valid, false - * otherwise. - */ - bool lost_samples_count(uint32_t & result); - /** * @brief Get low pass 3db frequency data. * @param result low pass 3db frequency value. @@ -638,22 +633,6 @@ class IIOWrapper */ bool update_linear_acceleration_compensation(uint32_t val); - /** - * @brief Get burst data selection data. - * @param result burst data selection data. - * @return Return true if reading was successful and data is valid, false - * otherwise. - */ - bool burst_data_selection(uint32_t & result); - - /** - * @brief Update burst data selection. - * @param val value to update with. - * @return Return true if writing was with success and - * false if not. - */ - bool update_burst_data_selection(uint32_t val); - /** * @brief Get bias correction time base control data. * @param result bias correction time base control data. @@ -879,8 +858,17 @@ class IIOWrapper double get_scale_temp(); private: - /*! This variable retains local context instance*/ - static struct iio_context * m_local_context; + /** + * @brief Update a field in the register map. + * @param reg The register address where the field is located + * @param val The value to be written. + * @param mask The bitmask to change + * @return Return true if updated was successful, false otherwise. + */ + bool updateField(uint32_t reg, uint32_t val, uint32_t mask); + + /*! This variable retains the IIO context instance */ + static struct iio_context * m_iio_context; /*! This variable retains the device instance */ static struct iio_device * m_dev; @@ -971,13 +959,6 @@ class IIOWrapper /*! This variable retains the scale for the temperature raw value */ static double m_scale_temp; - -public: - /*! This variable retains device name */ - static std::string s_device_name; - - /*! This variable retains device name enum*/ - static IIODeviceName s_device_name_enum; }; #endif // IIO_WRAPPER_H diff --git a/include/imu_ros2/imu_1650x_diag_ros_publisher.h b/include/imu_ros2/imu_1650x_diag_ros_publisher.h deleted file mode 100644 index 3d79b12..0000000 --- a/include/imu_ros2/imu_1650x_diag_ros_publisher.h +++ /dev/null @@ -1,68 +0,0 @@ -/******************************************************************************* - * @file imu_1650x_diag_ros_publisher.h - * @brief Header for adis1650x diagnosis publisher. - * @author Vasile Holonec (Vasile.Holonec@analog.com) - ******************************************************************************* - * Copyright 2023(c) Analog Devices, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - ******************************************************************************/ - -#ifndef IMU_1650X_DIAG_ROS_SUBSCRIBER_H -#define IMU_1650X_DIAG_ROS_SUBSCRIBER_H - -#include - -#include "imu_ros2/imu_1650x_diag_data_provider_interface.h" -#include "imu_ros2/imu_1650x_diag_ros_publisher_interface.h" - -/** - * @brief Class for diagnosis publisher for adis1650x chips. - */ -class Imu1650xDiagRosPublisher : public Imu1650xDiagRosPublisherInterface -{ -public: - /** - * @brief Constructor for Imu1650xDiagRosPublisher. - * @param node The ros2 Node instance. - */ - Imu1650xDiagRosPublisher(std::shared_ptr & node); - - /** - * @brief Destructor for Imu1650xDiagRosPublisher. - */ - ~Imu1650xDiagRosPublisher(); - - /** - * @brief Set the message data provider. - * @param dataProvider Data provider. - */ - void setMessageProvider(Imu1650xDiagDataProviderInterface * dataProvider) override; - - /** - * @brief Run the thread responsible for publishing Imu1650xDiagData message. - */ - void run() override; - -private: - /*! This variable retains the data provider instance. */ - Imu1650xDiagDataProviderInterface * m_data_provider; - - /*! This variable retains the publisher instance. */ - rclcpp::Publisher::SharedPtr m_publisher; - - /*! This variable retains the message that is published. */ - imu_ros2::msg::Imu1650xDiagData m_message; -}; - -#endif // IMU_1650X_DIAG_ROS_SUBSCRIBER_H diff --git a/include/imu_ros2/imu_1657x_diag_data_provider.h b/include/imu_ros2/imu_1657x_diag_data_provider.h deleted file mode 100644 index dbc316d..0000000 --- a/include/imu_ros2/imu_1657x_diag_data_provider.h +++ /dev/null @@ -1,56 +0,0 @@ -/******************************************************************************* - * @file imu_1657x_diag_data_provider.h - * @brief Header for providing diagnosis data for adis1657x. - * @author Vasile Holonec (Vasile.Holonec@analog.com) - ******************************************************************************* - * Copyright 2023(c) Analog Devices, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - ******************************************************************************/ - -#ifndef IMU_1657X_DIAG_DATA_PROVIDER_H -#define IMU_1657X_DIAG_DATA_PROVIDER_H - -#include "imu_ros2/iio_wrapper.h" -#include "imu_ros2/imu_1657x_diag_data_provider_interface.h" - -/** - * @brief Class for diagnosis data provider for adis1657x chips. - */ -class Imu1657xDiagDataProvider : public Imu1657xDiagDataProviderInterface -{ -public: - /** - * @brief Constructor for Imu1657xDiagDataProvider. - */ - Imu1657xDiagDataProvider(); - - /** - * @brief Destructor for Imu1657xDiagDataProvider. - */ - ~Imu1657xDiagDataProvider(); - - /** - * @brief Populate Imu1657xDiagData message with diagnosis data. - * @param message Message containing the diagnosis data. - * @return Return true if the message parameter is successfully populated with - * diagnosis data and false otherwise. - */ - bool getData(imu_ros2::msg::Imu1657xDiagData & message) override; - -private: - /*! This data member is used to access sensor information via libiio. */ - IIOWrapper m_iio_wrapper; -}; - -#endif // IMU_1657X_DIAG_DATA_PROVIDER_H diff --git a/include/imu_ros2/imu_1657x_diag_data_provider_interface.h b/include/imu_ros2/imu_1657x_diag_data_provider_interface.h deleted file mode 100644 index 334561b..0000000 --- a/include/imu_ros2/imu_1657x_diag_data_provider_interface.h +++ /dev/null @@ -1,51 +0,0 @@ -/******************************************************************************* - * @file imu_1657x_diag_data_provider_interface.h - * @brief Interface for providing diagnosis data for adis1657x. - * @author Vasile Holonec (Vasile.Holonec@analog.com) - ******************************************************************************* - * Copyright 2023(c) Analog Devices, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - ******************************************************************************/ - -#ifndef IMU_1657X_DIAG_DATA_PROVIDER_INTERFACE_H -#define IMU_1657X_DIAG_DATA_PROVIDER_INTERFACE_H - -#include "imu_ros2/msg/imu1657x_diag_data.hpp" - -/** - * @brief Interface for diagnosis data provider for adis1657x chips. - */ -class Imu1657xDiagDataProviderInterface -{ -public: - /** - * @brief Constructor for Imu1657xDiagDataProviderInterface. - */ - Imu1657xDiagDataProviderInterface() {} - - /** - * @brief Destructor for Imu1657xDiagDataProviderInterface. - */ - virtual ~Imu1657xDiagDataProviderInterface() {} - - /** - * @brief Populate Imu1657xDiagData message with diagnosis data. - * @param message Message containing the diagnosis data. - * @return Return true if the message parameter is successfully populated with - * diagnosis data and false otherwise. - */ - virtual bool getData(imu_ros2::msg::Imu1657xDiagData & message) = 0; -}; - -#endif // IMU_1657X_DIAG_DATA_PROVIDER_INTERFACE_H diff --git a/include/imu_ros2/imu_1657x_diag_ros_publisher_interface.h b/include/imu_ros2/imu_1657x_diag_ros_publisher_interface.h deleted file mode 100644 index af70ff2..0000000 --- a/include/imu_ros2/imu_1657x_diag_ros_publisher_interface.h +++ /dev/null @@ -1,58 +0,0 @@ -/******************************************************************************* - * @file imu_1657x_diag_ros_publisher_interface.h - * @brief Interface for adis1657x diagnosis publisher. - * @author Vasile Holonec (Vasile.Holonec@analog.com) - ******************************************************************************* - * Copyright 2023(c) Analog Devices, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - ******************************************************************************/ - -#ifndef IMU_1657X_DIAG_ROS_PUBLISHER_INTERFACE_H -#define IMU_1657X_DIAG_ROS_PUBLISHER_INTERFACE_H - -#include -#include - -#include "imu_ros2/ros_task.h" - -class Imu1657xDiagDataProviderInterface; - -/** - * @brief Interface for diagnosis publisher for adis1657x chips. - */ -class Imu1657xDiagRosPublisherInterface : public RosTask -{ -public: - /** - * @brief Constructor for Imu1657xDiagRosPublisherInterface. - */ - Imu1657xDiagRosPublisherInterface() {} - - /** - * @brief Destructor for Imu1657xDiagRosPublisherInterface. - */ - virtual ~Imu1657xDiagRosPublisherInterface() {} - - /** - * @brief Set the message data provider. - * @param dataProvider Data provider. - */ - virtual void setMessageProvider(Imu1657xDiagDataProviderInterface * dataProvider) = 0; - -protected: - /*! The ros2 Node data member. */ - std::shared_ptr m_node; -}; - -#endif // IMU_1657X_DIAG_ROS_PUBLISHER_INTERFACE_H diff --git a/include/imu_ros2/imu_control_parameters.h b/include/imu_ros2/imu_control_parameters.h index 1b798ae..8b6571c 100644 --- a/include/imu_ros2/imu_control_parameters.h +++ b/include/imu_ros2/imu_control_parameters.h @@ -23,7 +23,7 @@ #include -#include "iio_wrapper.h" +#include "imu_ros2/iio_wrapper.h" #include "rcl_interfaces/msg/set_parameters_result.hpp" /** @@ -51,14 +51,9 @@ class ImuControlParameters private: /** - * @brief Add the list of adis1650x parameters to m_attr_current_device. + * @brief Add the list of adis parameters to m_attr_current_device. */ - void declareAdis1650xAttributes(); - - /** - * @brief Add the list of adis1657x parameters to m_attr_current_device. - */ - void declareAdis1657xAttributes(); + void declareAdisAttributes(); /** * @brief Add the list of IIO update functions to diff --git a/include/imu_ros2/imu_1650x_diag_data_provider.h b/include/imu_ros2/imu_diag_data_provider.h similarity index 63% rename from include/imu_ros2/imu_1650x_diag_data_provider.h rename to include/imu_ros2/imu_diag_data_provider.h index 8f31e56..aa4b8f9 100644 --- a/include/imu_ros2/imu_1650x_diag_data_provider.h +++ b/include/imu_ros2/imu_diag_data_provider.h @@ -1,6 +1,6 @@ /******************************************************************************* - * @file imu_1650x_diag_data_provider.h - * @brief Header for providing diagnosis data for adis1650x. + * @file imu_diag_data_provider.h + * @brief Header for providing diagnosis data for adis. * @author Vasile Holonec (Vasile.Holonec@analog.com) ******************************************************************************* * Copyright 2023(c) Analog Devices, Inc. @@ -18,39 +18,39 @@ * limitations under the License. ******************************************************************************/ -#ifndef IMU_1650X_DIAG_DATA_PROVIDER_H -#define IMU_1650X_DIAG_DATA_PROVIDER_H +#ifndef IMU_DIAG_DATA_PROVIDER_H +#define IMU_DIAG_DATA_PROVIDER_H +#include "imu_diag_data_provider_interface.h" #include "imu_ros2/iio_wrapper.h" -#include "imu_ros2/imu_1650x_diag_data_provider_interface.h" /** - * @brief Class for diagnosis data provider for adis1650x chips. + * @brief Class for diagnosis data provider for adis chips. */ -class Imu1650xDiagDataProvider : public Imu1650xDiagDataProviderInterface +class ImuDiagDataProvider : public ImuDiagDataProviderInterface { public: /** - * @brief Constructor for Imu1650xDiagDataProvider. + * @brief Constructor for ImuDiagDataProvider. */ - Imu1650xDiagDataProvider(); + ImuDiagDataProvider(); /** - * @brief Destructor for Imu1650xDiagDataProvider. + * @brief Destructor for ImuDiagDataProvider. */ - ~Imu1650xDiagDataProvider(); + ~ImuDiagDataProvider(); /** - * @brief Populate Imu1650xDiagData message with diagnosis data. + * @brief Populate ImuDiagData message with diagnosis data. * @param message Message containing the diagnosis data. * @return Return true if the message parameter is successfully populated with * diagnosis data and false otherwise. */ - bool getData(imu_ros2::msg::Imu1650xDiagData & message) override; + bool getData(imu_ros2::msg::ImuDiagData & message) override; private: /*! This data member is used to access sensor information via libiio. */ IIOWrapper m_iio_wrapper; }; -#endif // IMU_1650X_DIAG_DATA_PROVIDER_H +#endif // IMU_DIAG_DATA_PROVIDER_H diff --git a/include/imu_ros2/imu_1650x_diag_data_provider_interface.h b/include/imu_ros2/imu_diag_data_provider_interface.h similarity index 59% rename from include/imu_ros2/imu_1650x_diag_data_provider_interface.h rename to include/imu_ros2/imu_diag_data_provider_interface.h index 13d01df..2bd502b 100644 --- a/include/imu_ros2/imu_1650x_diag_data_provider_interface.h +++ b/include/imu_ros2/imu_diag_data_provider_interface.h @@ -1,6 +1,6 @@ /******************************************************************************* - * @file imu_1650x_diag_data_provider_interface.h - * @brief Interface for adis1650x diagnosis publisher. + * @file imu_diag_data_provider_interface.h + * @brief Interface for providing diagnosis data for adis. * @author Vasile Holonec (Vasile.Holonec@analog.com) ******************************************************************************* * Copyright 2023(c) Analog Devices, Inc. @@ -18,34 +18,34 @@ * limitations under the License. ******************************************************************************/ -#ifndef IMU_1650X_DIAG_DATA_PROVIDER_INTERFACE_H -#define IMU_1650X_DIAG_DATA_PROVIDER_INTERFACE_H +#ifndef IMU_DIAG_DATA_PROVIDER_INTERFACE_H +#define IMU_DIAG_DATA_PROVIDER_INTERFACE_H -#include "imu_ros2/msg/imu1650x_diag_data.hpp" +#include "imu_ros2/msg/imu_diag_data.hpp" /** - * @brief Interface for diagnosis data provider for adis1650x chips. + * @brief Interface for diagnosis data provider for adis chips. */ -class Imu1650xDiagDataProviderInterface +class ImuDiagDataProviderInterface { public: /** - * @brief Constructor for Imu1650xDiagDataProviderInterface. + * @brief Constructor for ImuDiagDataProviderInterface. */ - Imu1650xDiagDataProviderInterface() {} + ImuDiagDataProviderInterface() {} /** - * @brief Destructor for Imu1650xDiagDataProviderInterface. + * @brief Destructor for ImuDiagDataProviderInterface. */ - virtual ~Imu1650xDiagDataProviderInterface() {} + virtual ~ImuDiagDataProviderInterface() {} /** - * @brief Populate Imu1650xDiagData message with diagnosis data. + * @brief Populate ImuDiagData message with diagnosis data. * @param message Message containing the diagnosis data. * @return Return true if the message parameter is successfully populated with * diagnosis data and false otherwise. */ - virtual bool getData(imu_ros2::msg::Imu1650xDiagData & message) = 0; + virtual bool getData(imu_ros2::msg::ImuDiagData & message) = 0; }; -#endif // IMU_1650X_DIAG_DATA_PROVIDER_INTERFACE_H +#endif // IMU_DIAG_DATA_PROVIDER_INTERFACE_H diff --git a/include/imu_ros2/imu_1657x_diag_ros_publisher.h b/include/imu_ros2/imu_diag_ros_publisher.h similarity index 61% rename from include/imu_ros2/imu_1657x_diag_ros_publisher.h rename to include/imu_ros2/imu_diag_ros_publisher.h index 9286de4..60a1089 100644 --- a/include/imu_ros2/imu_1657x_diag_ros_publisher.h +++ b/include/imu_ros2/imu_diag_ros_publisher.h @@ -1,5 +1,5 @@ /******************************************************************************* - * @file imu_1657x_diag_ros_publisher.h + * @file imu_diag_ros_publisher.h * @brief Header for adis1657x diagnosis publisher. * @author Vasile Holonec (Vasile.Holonec@analog.com) ******************************************************************************* @@ -18,51 +18,51 @@ * limitations under the License. ******************************************************************************/ -#ifndef IMU_1657X_DIAG_ROS_PUBLISHER_H -#define IMU_1657X_DIAG_ROS_PUBLISHER_H +#ifndef IMU_DIAG_ROS_PUBLISHER_H +#define IMU_DIAG_ROS_PUBLISHER_H #include -#include "imu_ros2/imu_1657x_diag_data_provider_interface.h" -#include "imu_ros2/imu_1657x_diag_ros_publisher_interface.h" +#include "imu_diag_data_provider_interface.h" +#include "imu_diag_ros_publisher_interface.h" /** * @brief Class for diagnosis publisher for adis1657x chips. */ -class Imu1657xDiagRosPublisher : public Imu1657xDiagRosPublisherInterface +class ImuDiagRosPublisher : public ImuDiagRosPublisherInterface { public: /** - * @brief Constructor for Imu1657xDiagRosPublisher. + * @brief Constructor for ImuDiagRosPublisher. * @param node The ros2 Node instance. */ - Imu1657xDiagRosPublisher(std::shared_ptr & node); + ImuDiagRosPublisher(std::shared_ptr & node); /** - * @brief Destructor for Imu1657xDiagRosPublisher. + * @brief Destructor for ImuDiagRosPublisher. */ - ~Imu1657xDiagRosPublisher(); + ~ImuDiagRosPublisher(); /** * @brief Set the message data provider. * @param dataProvider Data provider. */ - void setMessageProvider(Imu1657xDiagDataProviderInterface * dataProvider) override; + void setMessageProvider(ImuDiagDataProviderInterface * dataProvider) override; /** - * @brief Run the thread responsible for publishing Imu1657xDiagData message. + * @brief Run the thread responsible for publishing ImuDiagData message. */ void run() override; private: /*! This variable retains the data provider instance. */ - Imu1657xDiagDataProviderInterface * m_data_provider; + ImuDiagDataProviderInterface * m_data_provider; /*! This variable retains the publisher instance. */ - rclcpp::Publisher::SharedPtr m_publisher; + rclcpp::Publisher::SharedPtr m_publisher; /*! This variable retains the message that is published. */ - imu_ros2::msg::Imu1657xDiagData m_message; + imu_ros2::msg::ImuDiagData m_message; }; -#endif // IMU_1657X_DIAG_ROS_PUBLISHER_H +#endif // IMU_DIAG_ROS_PUBLISHER_H diff --git a/include/imu_ros2/imu_1650x_diag_ros_publisher_interface.h b/include/imu_ros2/imu_diag_ros_publisher_interface.h similarity index 62% rename from include/imu_ros2/imu_1650x_diag_ros_publisher_interface.h rename to include/imu_ros2/imu_diag_ros_publisher_interface.h index 5c55df3..21651b6 100644 --- a/include/imu_ros2/imu_1650x_diag_ros_publisher_interface.h +++ b/include/imu_ros2/imu_diag_ros_publisher_interface.h @@ -1,6 +1,6 @@ /******************************************************************************* - * @file imu_1650x_diag_ros_publisher_interface.h - * @brief Interface for adis1650x diagnosis publisher. + * @file imu_diag_ros_publisher_interface.h + * @brief Interface for adis diagnosis publisher. * @author Vasile Holonec (Vasile.Holonec@analog.com) ******************************************************************************* * Copyright 2023(c) Analog Devices, Inc. @@ -18,41 +18,41 @@ * limitations under the License. ******************************************************************************/ -#ifndef IMU_1650X_DIAG_ROS_PUBLISHER_INTERFACE_H -#define IMU_1650X_DIAG_ROS_PUBLISHER_INTERFACE_H +#ifndef IMU_DIAG_ROS_PUBLISHER_INTERFACE_H +#define IMU_DIAG_ROS_PUBLISHER_INTERFACE_H #include #include #include "imu_ros2/ros_task.h" -class Imu1650xDiagDataProviderInterface; +class ImuDiagDataProviderInterface; /** - * @brief Interface for diagnosis publisher for adis1650x chips. + * @brief Interface for diagnosis publisher for adis chips. */ -class Imu1650xDiagRosPublisherInterface : public RosTask +class ImuDiagRosPublisherInterface : public RosTask { public: /** - * @brief Constructor for Imu1650xDiagRosPublisherInterface. + * @brief Constructor for ImuDiagRosPublisherInterface. */ - Imu1650xDiagRosPublisherInterface() {} + ImuDiagRosPublisherInterface() {} /** - * @brief Destructor for Imu1650xDiagRosPublisherInterface. + * @brief Destructor for ImuDiagRosPublisherInterface. */ - virtual ~Imu1650xDiagRosPublisherInterface() {} + virtual ~ImuDiagRosPublisherInterface() {} /** * @brief Set the message data provider. * @param dataProvider Data provider. */ - virtual void setMessageProvider(Imu1650xDiagDataProviderInterface * dataProvider) = 0; + virtual void setMessageProvider(ImuDiagDataProviderInterface * dataProvider) = 0; protected: /*! The ros2 Node data member. */ std::shared_ptr m_node; }; -#endif // IMU_1650X_DIAG_ROS_PUBLISHER_INTERFACE_H +#endif // IMU_DIAG_ROS_PUBLISHER_INTERFACE_H diff --git a/include/imu_ros2/ros_publisher_group.h b/include/imu_ros2/ros_publisher_group.h index f0293e7..d2c955e 100644 --- a/include/imu_ros2/ros_publisher_group.h +++ b/include/imu_ros2/ros_publisher_group.h @@ -49,11 +49,13 @@ class RosPublisherGroup : public RosPublisherGroupInterface void setAccelGyroTempRosPublisher( AccelGyroTempRosPublisherInterface * accelGyroTempRosPublisher) override; +#ifdef ADIS_HAS_DELTA_BURST /** * @brief Sets the velAngTempRosPublisher publisher in the publisher group. * @param velAngTempRosPublisher The publisher to be set in the group. */ void setVelAngTempRosPublisher(VelAngTempRosPublisherInterface * velAngTempRosPublisher) override; +#endif /** * @brief Sets the imuRosPublisher publisher in the publisher group. @@ -86,8 +88,10 @@ class RosPublisherGroup : public RosPublisherGroupInterface private: /*! Variable to retain AccelGyroTemp message provider. */ AccelGyroTempRosPublisherInterface * m_accelGyroTempRosPublisher; +#ifdef ADIS_HAS_DELTA_BURST /*! Variable to retain VelAngTemp message provider. */ VelAngTempRosPublisherInterface * m_velAngTempRosPublisher; +#endif /*! Variable to retain Imu message provider. */ ImuRosPublisherInterface * m_imuRosPublisher; /*! Variable to retain ImuFullMeasuredData message provider. */ diff --git a/include/imu_ros2/ros_publisher_group_interface.h b/include/imu_ros2/ros_publisher_group_interface.h index cc9fc1e..5f9e358 100644 --- a/include/imu_ros2/ros_publisher_group_interface.h +++ b/include/imu_ros2/ros_publisher_group_interface.h @@ -24,6 +24,7 @@ #include #include +#include "adis_data_access.h" #include "imu_ros2/ros_task.h" class AccelGyroTempRosPublisherInterface; @@ -55,12 +56,14 @@ class RosPublisherGroupInterface : public RosTask virtual void setAccelGyroTempRosPublisher( AccelGyroTempRosPublisherInterface * accelGyroTempRosPublisher) = 0; +#ifdef ADIS_HAS_DELTA_BURST /** * @brief Sets the velAngTempRosPublisher publisher in the publisher group. * @param velAngTempRosPublisher The publisher to be set in the group. */ virtual void setVelAngTempRosPublisher( VelAngTempRosPublisherInterface * velAngTempRosPublisher) = 0; +#endif /** * @brief Sets the imuRosPublisher publisher in the publisher group. diff --git a/include/imu_ros2/setting_declarations.h b/include/imu_ros2/setting_declarations.h deleted file mode 100644 index 673eb22..0000000 --- a/include/imu_ros2/setting_declarations.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************* - * @file setting_declarations.h - * @brief Header for common declarations. - * @author Vasile Holonec (Vasile.Holonec@analog.com) - ******************************************************************************* - * Copyright 2023(c) Analog Devices, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - ******************************************************************************/ - -#ifndef SETTING_DECLARATIONS_H -#define SETTING_DECLARATIONS_H - -enum IIODeviceName -{ - ADIS1650X = 0, - ADIS1657X -}; - -#define ACCEL_GYRO_BUFFERED_DATA 0 -#define DELTAVEL_DELTAANG_BUFFERED_DATA 1 -#define IMU_STD_MSG_DATA 2 -#define FULL_MEASURED_DATA 3 - -#endif // SETTING_DECLARATIONS_H diff --git a/msg/AccelGyroTempData.msg b/msg/AccelGyroTempData.msg index b9aaabe..a67cd25 100644 --- a/msg/AccelGyroTempData.msg +++ b/msg/AccelGyroTempData.msg @@ -18,6 +18,9 @@ # limitations under the License. ################################################################################ +# message header +std_msgs/Header header + # 3-axis acceleration [m/s^2] geometry_msgs/Vector3 linear_acceleration @@ -26,6 +29,3 @@ geometry_msgs/Vector3 angular_velocity # imu internal temperature [°C] float64 temperature - -# sample timestamp -int64 timestamp diff --git a/msg/ImuFullMeasuredData.msg b/msg/ImuFullMeasuredData.msg index aba1093..258fc50 100644 --- a/msg/ImuFullMeasuredData.msg +++ b/msg/ImuFullMeasuredData.msg @@ -18,6 +18,9 @@ # limitations under the License. ################################################################################ +# message header +std_msgs/Header header + # 3-axis acceleration [m/s^2] geometry_msgs/Vector3 linear_acceleration @@ -27,7 +30,7 @@ geometry_msgs/Vector3 angular_velocity # 3-axis delta velocity (delta between two consecutive measurements) [m/s] geometry_msgs/Vector3 delta_velocity -# 3-axis delta angle (delta between two consecutive measurements) [degrees] +# 3-axis delta angle (delta between two consecutive measurements) [radians] geometry_msgs/Vector3 delta_angle # imu internal temperature [°C] diff --git a/msg/ImuIdentificationData.msg b/msg/ImuIdentificationData.msg index abebbdd..1a7694d 100644 --- a/msg/ImuIdentificationData.msg +++ b/msg/ImuIdentificationData.msg @@ -18,6 +18,9 @@ # limitations under the License. ################################################################################ +# message header +std_msgs/Header header + # firmware revision for the imu internal firmware string firmware_revision @@ -32,4 +35,3 @@ uint32 serial_number # imu gyroscope measurement range string gyroscope_measurement_range - diff --git a/msg/VelAngTempData.msg b/msg/VelAngTempData.msg index c9ef733..91657de 100644 --- a/msg/VelAngTempData.msg +++ b/msg/VelAngTempData.msg @@ -18,14 +18,14 @@ # limitations under the License. ################################################################################ +# message header +std_msgs/Header header + # 3-axis delta velocity (delta between two consecutive measurements) [m/s] geometry_msgs/Vector3 delta_velocity -# 3-axis delta angle (delta between two consecutive measurements) [degrees] +# 3-axis delta angle (delta between two consecutive measurements) [radians] geometry_msgs/Vector3 delta_angle # imu internal temperature [°C] float64 temperature - -# sample timestamp -int64 timestamp diff --git a/msg/adis1646x/ImuDiagData.msg b/msg/adis1646x/ImuDiagData.msg new file mode 100644 index 0000000..64e0b06 --- /dev/null +++ b/msg/adis1646x/ImuDiagData.msg @@ -0,0 +1,51 @@ +################################################################################ +# @file ImuDiagData.msg +# @brief Definition of ImuDiagData message +# @author Ramona Gradinariu (ramona.gradinariu@analog.com) +################################################################################ +# Copyright 2023(c) Analog Devices, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +################################################################################ + +# message header +std_msgs/Header header + +# if true, one of the datapaths experienced an overrun condition +bool diag_data_path_overrun + +# if true, the most recent imu memory flash failed +bool diag_flash_memory_update_error + +# if true, the total number of SPI SCLK cycles is not equal to an integer multiple of 16 +bool diag_spi_communication_error + +# if true, the imu voltage across VDD and GND < 2.9V, which causes data processing to stop +bool diag_standby_mode + +# if true, the imu self-test routine failed +bool diag_sensor_self_test_error + +# if true, there is a failure in imu flash memory +bool diag_flash_memory_test_error + +# if true, the internal data sampling clock does not synchronize with the external clock (only in scaled sync mode) +bool diag_clock_error + +# if true, the imu flash memory was written more times than the data-sheet specified endurance +bool diag_flash_memory_write_count_exceeded_error + +# the value of the imu flash writes +uint32 flash_counter + + diff --git a/msg/adis1647x/ImuDiagData.msg b/msg/adis1647x/ImuDiagData.msg new file mode 100644 index 0000000..64e0b06 --- /dev/null +++ b/msg/adis1647x/ImuDiagData.msg @@ -0,0 +1,51 @@ +################################################################################ +# @file ImuDiagData.msg +# @brief Definition of ImuDiagData message +# @author Ramona Gradinariu (ramona.gradinariu@analog.com) +################################################################################ +# Copyright 2023(c) Analog Devices, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +################################################################################ + +# message header +std_msgs/Header header + +# if true, one of the datapaths experienced an overrun condition +bool diag_data_path_overrun + +# if true, the most recent imu memory flash failed +bool diag_flash_memory_update_error + +# if true, the total number of SPI SCLK cycles is not equal to an integer multiple of 16 +bool diag_spi_communication_error + +# if true, the imu voltage across VDD and GND < 2.9V, which causes data processing to stop +bool diag_standby_mode + +# if true, the imu self-test routine failed +bool diag_sensor_self_test_error + +# if true, there is a failure in imu flash memory +bool diag_flash_memory_test_error + +# if true, the internal data sampling clock does not synchronize with the external clock (only in scaled sync mode) +bool diag_clock_error + +# if true, the imu flash memory was written more times than the data-sheet specified endurance +bool diag_flash_memory_write_count_exceeded_error + +# the value of the imu flash writes +uint32 flash_counter + + diff --git a/msg/Imu1650xDiagData.msg b/msg/adis1650x/ImuDiagData.msg similarity index 87% rename from msg/Imu1650xDiagData.msg rename to msg/adis1650x/ImuDiagData.msg index 9427bd6..6520d6b 100644 --- a/msg/Imu1650xDiagData.msg +++ b/msg/adis1650x/ImuDiagData.msg @@ -1,6 +1,6 @@ ################################################################################ -# @file Imu16505DiagData.msg -# @brief Definition of Imu16505DiagData message +# @file ImuDiagData.msg +# @brief Definition of ImuDiagData message # @author RBolboac (ramona.bolboaca@analog.com) ################################################################################ # Copyright 2023(c) Analog Devices, Inc. @@ -18,6 +18,9 @@ # limitations under the License. ################################################################################ +# message header +std_msgs/Header header + # if true, one of the datapaths experienced an overrun condition bool diag_data_path_overrun @@ -48,14 +51,8 @@ bool diag_gyroscope1_self_test_error # if true, a failure occurred on gyroscope 2 bool diag_gyroscope2_self_test_error -# if true, a checksum error occurred in the previous SPI transaction -bool diag_checksum_error_flag - # if true, the imu flash memory was written more times than the data-sheet specified endurance bool diag_flash_memory_write_count_exceeded_error # the value of the imu flash writes uint32 flash_counter - -# the number of lost samples in imu driver, applies only when buffer readings are performed -uint32 lost_samples_count diff --git a/msg/Imu1657xDiagData.msg b/msg/adis1657x/ImuDiagData.msg similarity index 89% rename from msg/Imu1657xDiagData.msg rename to msg/adis1657x/ImuDiagData.msg index 563eb5c..d89b252 100644 --- a/msg/Imu1657xDiagData.msg +++ b/msg/adis1657x/ImuDiagData.msg @@ -1,6 +1,6 @@ ################################################################################ -# @file Imu1657xDiagData.msg -# @brief Definition of Imu1657xDiagData message +# @file ImuDiagData.msg +# @brief Definition of ImuDiagData message # @author RBolboac (ramona.bolboaca@analog.com) ################################################################################ # Copyright 2023(c) Analog Devices, Inc. @@ -18,6 +18,9 @@ # limitations under the License. ################################################################################ +# message header +std_msgs/Header header + # if true, the imu failed to initialize properly bool diag_sensor_initialization_failure @@ -63,14 +66,8 @@ bool diag_z_axis_accelerometer_failure # if true, a fault occurred in the imu microcontroller bool diag_aduc_mcu_fault -# if true, a checksum error occurred in the previous SPI transaction -bool diag_checksum_error_flag - # if true, the imu flash memory was written more times than the data-sheet specified endurance bool diag_flash_memory_write_count_exceeded_error # the value of the imu flash writes uint32 flash_counter - -# the number of lost samples in imu driver, applies only when buffer readings are performed -uint32 lost_samples_count diff --git a/package.xml b/package.xml index fa5e96c..68f5aef 100644 --- a/package.xml +++ b/package.xml @@ -2,10 +2,10 @@ imu_ros2 - 0.0.0 - TODO: Package description - vholonec - TODO: License declaration + 0.0.1 + Publisher for ADI IMUs + rbolboac + Apache License Version 2.0 ament_cmake rclcpp @@ -13,9 +13,6 @@ sensor_msgs geometry_msgs - ament_lint_auto - ament_lint_common - builtin_interfaces rosidl_default_generators builtin_interfaces diff --git a/src/accelgyrotemp_data_provider.cpp b/src/accelgyrotemp_data_provider.cpp index 9d4bc15..69ee3c9 100644 --- a/src/accelgyrotemp_data_provider.cpp +++ b/src/accelgyrotemp_data_provider.cpp @@ -39,7 +39,8 @@ bool AccelGyroTempDataProvider::getData(imu_ros2::msg::AccelGyroTempData & messa message.temperature = m_iio_wrapper.getBuffTemperature(); - message.timestamp = m_iio_wrapper.getBuffSampleTimestamp(); + message.header.frame_id = "accelgyrotempdata"; + m_iio_wrapper.getBuffSampleTimestamp(message.header.stamp.sec, message.header.stamp.nanosec); return true; } diff --git a/src/accelgyrotemp_ros_publisher.cpp b/src/accelgyrotemp_ros_publisher.cpp index 6723585..ca2cf83 100644 --- a/src/accelgyrotemp_ros_publisher.cpp +++ b/src/accelgyrotemp_ros_publisher.cpp @@ -24,8 +24,6 @@ #include #include -#include "imu_ros2/setting_declarations.h" - AccelGyroTempRosPublisher::AccelGyroTempRosPublisher(std::shared_ptr & node) { m_node = node; diff --git a/src/iio_wrapper.cpp b/src/iio_wrapper.cpp index 154a369..f010b7c 100644 --- a/src/iio_wrapper.cpp +++ b/src/iio_wrapper.cpp @@ -20,17 +20,10 @@ #include "imu_ros2/iio_wrapper.h" -#include - -#include -#include -#include #include -#include -#include /*! Maximum allowed number of samples in IIO buffer. */ -#define MAX_NO_OF_SAMPLES 4000 +#define MAX_NO_OF_SAMPLES 1 enum { @@ -41,17 +34,19 @@ enum 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_local_context = nullptr; +struct iio_context * IIOWrapper::m_iio_context = nullptr; struct iio_device * IIOWrapper::m_dev = nullptr; struct iio_device * IIOWrapper::m_dev_trigger = nullptr; @@ -95,48 +90,75 @@ uint32_t buff_data[NO_OF_CHANS + 1][MAX_NO_OF_SAMPLES]; /*! Sampling frequency of the device. */ double samp_freq = 2000.0; /*! Number of samples to be read at once. */ -uint32_t no_of_samp = 2000; -/*! Current set burst data selection. */ -uint32_t current_burst_data_selection = ACCEL_GYRO_BUFFERED_DATA; - -std::string IIOWrapper::s_device_name; -IIODeviceName IIOWrapper::s_device_name_enum = IIODeviceName::ADIS1657X; - -IIOWrapper::IIOWrapper() -{ - // create context - if (m_local_context == nullptr) { - m_local_context = iio_create_local_context(); - if (!m_local_context) throw std::runtime_error("Exception: local context is null"); - - iio_context_set_timeout(m_local_context, 5000); - - std::list supported_devices{ - "adis16500", "adis16505-1", "adis16505-2", "adis16505-3", "adis16507-1", - "adis16507-2", "adis16507-3", "adis16575-2", "adis16575-3", "adis16576-2", - "adis16576-3", "adis16577-2", "adis16577-3"}; - - for (std::string const & devname : supported_devices) { - m_dev = iio_context_find_device(m_local_context, devname.c_str()); - if (m_dev) { - IIOWrapper::s_device_name = devname; - RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "Found device: %s", devname.c_str()); - - if (devname.find("adis1657") != std::string::npos) - IIOWrapper::s_device_name_enum = IIODeviceName::ADIS1657X; - else - IIOWrapper::s_device_name_enum = IIODeviceName::ADIS1650X; - break; +uint32_t no_of_samp = MAX_NO_OF_SAMPLES; +/*! Current set data selection. */ +uint32_t current_data_selection = FULL_MEASURED_DATA; + +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; + } +} + +void IIOWrapper::createContext(const char * context) +{ + if (m_iio_context) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "IIO context already exists."); + return; + } + + 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_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", "adis16505-1", "adis16505-2", "adis16505-3", "adis16507-1", + "adis16507-2", "adis16507-3", "adis16575-2", "adis16575-3", "adis16576-2", "adis16576-3", + "adis16577-2", "adis16577-3"}; + + 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) { + std::string triggerName = devname + "-dev0"; + 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; + } } + break; } - - if (!m_dev) throw std::runtime_error("Exception: no device found, m_dev is null"); } - if (m_dev_trigger == nullptr) { - std::string triggnerName = IIOWrapper::s_device_name + "-dev0"; - m_dev_trigger = iio_context_find_device(m_local_context, triggnerName.c_str()); - if (!m_dev_trigger) throw std::runtime_error("Exception: device trigger data is null"); + 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_device_set_trigger(m_dev, m_dev_trigger); @@ -156,64 +178,77 @@ IIOWrapper::IIOWrapper() 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, "rot_x", false); + m_channel_deltaangl_x = iio_device_find_channel(m_dev, "deltaangl_x", false); if (m_channel_deltaangl_y == nullptr) - m_channel_deltaangl_y = iio_device_find_channel(m_dev, "rot_y", false); + 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, "rot_z", false); + 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, "velocity_x", false); + 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, "velocity_y", false); + 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, "velocity_z", false); + 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, "temp", 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); - 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); - 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); - iio_channel_enable(m_channel_temp); - iio_channel_enable(m_channel_timestamp); - - iio_channel_attr_read_double(m_channel_accel_x, "scale", &m_scale_accel_x); - iio_channel_attr_read_double(m_channel_accel_y, "scale", &m_scale_accel_y); - iio_channel_attr_read_double(m_channel_accel_z, "scale", &m_scale_accel_z); - iio_channel_attr_read_double(m_channel_anglvel_x, "scale", &m_scale_anglvel_x); - iio_channel_attr_read_double(m_channel_anglvel_y, "scale", &m_scale_anglvel_y); - iio_channel_attr_read_double(m_channel_anglvel_z, "scale", &m_scale_anglvel_z); - 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); - iio_channel_attr_read_double(m_channel_temp, "scale", &m_scale_temp); + if (m_channel_temp) iio_channel_enable(m_channel_temp); + + if (m_channel_timestamp) iio_channel_enable(m_channel_timestamp); + + 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 (m_channel_deltaangl_x) + iio_channel_attr_read_double(m_channel_deltaangl_x, "scale", &m_scale_deltaangl_x); + + if (m_channel_deltaangl_y) + iio_channel_attr_read_double(m_channel_deltaangl_y, "scale", &m_scale_deltaangl_y); + + if (m_channel_deltaangl_z) + iio_channel_attr_read_double(m_channel_deltaangl_z, "scale", &m_scale_deltaangl_z); + + if (m_channel_deltavelocity_x) + iio_channel_attr_read_double(m_channel_deltavelocity_x, "scale", &m_scale_deltavelocity_x); + + if (m_channel_deltavelocity_y) + iio_channel_attr_read_double(m_channel_deltavelocity_y, "scale", &m_scale_deltavelocity_y); + + if (m_channel_deltavelocity_z) + iio_channel_attr_read_double(m_channel_deltavelocity_z, "scale", &m_scale_deltavelocity_z); + + if (m_channel_temp) iio_channel_attr_read_double(m_channel_temp, "scale", &m_scale_temp); } -IIOWrapper::~IIOWrapper() +bool IIOWrapper::updateField(uint32_t reg, uint32_t val, uint32_t mask) { - if (m_dev_buffer != nullptr) { - iio_buffer_destroy(m_dev_buffer); - m_dev_buffer = nullptr; - } - if (m_local_context != nullptr) { - iio_context_destroy(m_local_context); - m_local_context = nullptr; - } + 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::stopBufferAcquisition() @@ -227,14 +262,19 @@ void IIOWrapper::stopBufferAcquisition() static ssize_t demux_sample( const struct iio_channel * chn, void * sample, size_t size, __attribute__((unused)) void * d) { - uint64_t val; - iio_channel_convert(chn, &val, sample); - - buff_data[iio_channel_get_index(chn)][buff_write_idx] = val; - - if (iio_channel_get_index(chn) == CHAN_DATA_TIMESTAMP) { - buff_data[CHAN_DATA_TIMESTAMP + 1][buff_write_idx] = val >> 32; - buff_write_idx++; + if (size == 2) { + int16_t val; + iio_channel_convert(chn, &val, sample); + buff_data[iio_channel_get_index(chn)][buff_write_idx] = val; + } else if (size == 4) { + int32_t val; + iio_channel_convert(chn, &val, sample); + buff_data[iio_channel_get_index(chn)][buff_write_idx] = val; + } 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; @@ -242,27 +282,56 @@ static ssize_t demux_sample( return size; } -bool IIOWrapper::updateBuffer(uint32_t burst_data_selection) +bool IIOWrapper::updateBuffer(uint32_t data_selection) { ssize_t ret; - if (current_burst_data_selection != burst_data_selection) { + if (current_data_selection != data_selection) { stopBufferAcquisition(); - if (update_burst_data_selection(burst_data_selection)) - current_burst_data_selection = burst_data_selection; - else { - RCLCPP_INFO( - rclcpp::get_logger("rclcpp_iiowrapper"), "burst data selection could not be configured"); - return false; + if (data_selection == ACCEL_GYRO_BUFFERED_DATA) { +#ifdef ADIS_HAS_DELTA_BURST + 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 + 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); +#endif } - } - if (samp_freq != no_of_samp) stopBufferAcquisition(); + current_data_selection = data_selection; + } if (m_dev_buffer == nullptr) { sampling_frequency(&samp_freq); - no_of_samp = 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; @@ -273,7 +342,8 @@ bool IIOWrapper::updateBuffer(uint32_t burst_data_selection) ret = iio_buffer_refill(m_dev_buffer); if ((ret == 0) || (ret == -110)) { - RCLCPP_INFO(rclcpp::get_logger("rclcpp_iiowrapper"), "no samples available yet, retrying"); + RCLCPP_INFO( + rclcpp::get_logger("rclcpp_iiowrapper"), "no samples available yet, retrying ret = %ld", ret); return false; } if (ret < 0) { @@ -316,6 +386,7 @@ 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() { return (int32_t)buff_data[CHAN_DELTA_ANGL_X][buff_read_idx] * m_scale_deltaangl_x; @@ -345,13 +416,14 @@ double IIOWrapper::getBuffDeltaVelocityZ() { 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; } -int64_t IIOWrapper::getBuffSampleTimestamp() +void IIOWrapper::getBuffSampleTimestamp(int32_t & sec, uint32_t & nanosec) { 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; @@ -360,12 +432,17 @@ int64_t IIOWrapper::getBuffSampleTimestamp() uint64_t timestamp = ((uint64_t)timestamp_48_63 << 48) | ((uint64_t)timestamp_32_47 << 32) | ((uint64_t)timestamp_16_31 << 16) | timestamp_0_15; - return (int64_t)timestamp; + + sec = timestamp / 1000000000; + nanosec = timestamp % 1000000000; } bool IIOWrapper::getRegLinearAccelerationX(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; @@ -375,6 +452,9 @@ bool IIOWrapper::getRegLinearAccelerationX(double & result) bool IIOWrapper::getRegLinearAccelerationY(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; @@ -384,6 +464,9 @@ bool IIOWrapper::getRegLinearAccelerationY(double & result) bool IIOWrapper::getRegLinearAccelerationZ(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; @@ -393,6 +476,9 @@ bool IIOWrapper::getRegLinearAccelerationZ(double & result) bool IIOWrapper::getRegAngularVelocityX(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; @@ -402,6 +488,9 @@ bool IIOWrapper::getRegAngularVelocityX(double & result) bool IIOWrapper::getRegAngularVelocityY(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; @@ -411,6 +500,9 @@ bool IIOWrapper::getRegAngularVelocityY(double & result) bool IIOWrapper::getRegAngularVelocityZ(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; @@ -420,6 +512,9 @@ bool IIOWrapper::getRegAngularVelocityZ(double & result) bool IIOWrapper::getRegDeltaAngleX(double & result) { long long valueRaw; + + if (!m_channel_deltaangl_x) return false; + int ret = iio_channel_attr_read_longlong(m_channel_deltaangl_x, "raw", &valueRaw); result = valueRaw * m_scale_deltaangl_x; @@ -429,6 +524,9 @@ bool IIOWrapper::getRegDeltaAngleX(double & result) bool IIOWrapper::getRegDeltaAngleY(double & result) { long long valueRaw; + + if (!m_channel_deltaangl_y) return false; + int ret = iio_channel_attr_read_longlong(m_channel_deltaangl_y, "raw", &valueRaw); result = valueRaw * m_scale_deltaangl_y; @@ -438,6 +536,9 @@ bool IIOWrapper::getRegDeltaAngleY(double & result) bool IIOWrapper::getRegDeltaAngleZ(double & result) { long long valueRaw; + + if (!m_channel_deltaangl_z) return false; + int ret = iio_channel_attr_read_longlong(m_channel_deltaangl_z, "raw", &valueRaw); result = valueRaw * m_scale_deltaangl_z; @@ -447,6 +548,9 @@ bool IIOWrapper::getRegDeltaAngleZ(double & result) bool IIOWrapper::getRegDeltaVelocityX(double & result) { long long valueRaw; + + if (!m_channel_deltavelocity_x) return false; + int ret = iio_channel_attr_read_longlong(m_channel_deltavelocity_x, "raw", &valueRaw); result = valueRaw * m_scale_deltavelocity_x; @@ -456,6 +560,9 @@ bool IIOWrapper::getRegDeltaVelocityX(double & result) bool IIOWrapper::getRegDeltaVelocityY(double & result) { long long valueRaw; + + if (!m_channel_deltavelocity_y) return false; + int ret = iio_channel_attr_read_longlong(m_channel_deltavelocity_y, "raw", &valueRaw); result = valueRaw * m_scale_deltavelocity_y; @@ -465,6 +572,9 @@ bool IIOWrapper::getRegDeltaVelocityY(double & result) bool IIOWrapper::getRegDeltaVelocityZ(double & result) { long long valueRaw; + + if (!m_channel_deltavelocity_z) return false; + int ret = iio_channel_attr_read_longlong(m_channel_deltavelocity_z, "raw", &valueRaw); result = valueRaw * m_scale_deltavelocity_z; @@ -474,6 +584,9 @@ bool IIOWrapper::getRegDeltaVelocityZ(double & result) bool IIOWrapper::getRegTemperature(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; @@ -483,6 +596,9 @@ bool IIOWrapper::getRegTemperature(double & result) 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; @@ -491,12 +607,17 @@ bool IIOWrapper::anglvel_x_calibbias(int32_t & result) 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; @@ -505,12 +626,17 @@ bool IIOWrapper::anglvel_y_calibbias(int32_t & result) 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; @@ -519,12 +645,17 @@ bool IIOWrapper::anglvel_z_calibbias(int32_t & result) 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; @@ -533,12 +664,17 @@ bool IIOWrapper::accel_x_calibbias(int32_t & result) 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; @@ -547,12 +683,17 @@ bool IIOWrapper::accel_y_calibbias(int32_t & result) 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; @@ -561,6 +702,8 @@ bool IIOWrapper::accel_z_calibbias(int32_t & result) 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); } @@ -568,6 +711,8 @@ 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; @@ -580,6 +725,8 @@ 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; @@ -588,393 +735,635 @@ bool IIOWrapper::update_sampling_frequency(double val) return true; } +#ifdef ADIS_SNSR_INIT_FAIL bool IIOWrapper::diag_sensor_initialization_failure(bool & result) { - return ( - iio_device_debug_attr_read_bool(m_dev, "diag_sensor_initialization_failure", &result) == 0); + 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 bool IIOWrapper::diag_data_path_overrun(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_data_path_overrun", &result) == 0); + 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; } bool IIOWrapper::diag_flash_memory_update_error(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_flash_memory_update_error", &result) == 0); + 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; } bool IIOWrapper::diag_spi_communication_error(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_spi_communication_error", &result) == 0); + 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; } bool IIOWrapper::diag_standby_mode(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_standby_mode", &result) == 0); + 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; } bool IIOWrapper::diag_sensor_self_test_error(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_sensor_self_test_error", &result) == 0); + 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 & AIDS_SNSR_FAIL) >> AIDS_SNSR_FAIL_POS; + result = reg_val; + + return true; } bool IIOWrapper::diag_flash_memory_test_error(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_flash_memory_test_error", &result) == 0); + 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; } bool IIOWrapper::diag_clock_error(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_clk_error", &result) == 0); + 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; } +#ifdef ADIS_GYRO1_FAIL bool IIOWrapper::diag_gyroscope1_self_test_error(bool & result) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_gyroscope1_self_test_error", &result) == 0); + 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) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_gyroscope2_self_test_error", &result) == 0); + 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) { - return ( - iio_device_debug_attr_read_bool(m_dev, "diag_acceleration_self_test_error", &result) == 0); + 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) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_x_axis_gyroscope_failure", &result) == 0); + 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_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) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_y_axis_gyroscope_failure", &result) == 0); + 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_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) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_z_axis_gyroscope_failure", &result) == 0); + 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_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) { - return ( - iio_device_debug_attr_read_bool(m_dev, "diag_x_axis_accelerometer_failure", &result) == 0); + 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_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) { - return ( - iio_device_debug_attr_read_bool(m_dev, "diag_y_axis_accelerometer_failure", &result) == 0); + 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_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) { - return ( - iio_device_debug_attr_read_bool(m_dev, "diag_z_axis_accelerometer_failure", &result) == 0); + 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_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) { - return (iio_device_debug_attr_read_bool(m_dev, "diag_aduc_mcu_fault", &result) == 0); -} + uint32_t reg_val; -bool IIOWrapper::diag_checksum_error_flag(bool & result) -{ - return (iio_device_debug_attr_read_bool(m_dev, "diag_checksum_error_flag", &result) == 0); -} + if (!m_dev) return false; -bool IIOWrapper::diag_flash_memory_write_count_exceeded_error(bool & result) -{ - return ( - iio_device_debug_attr_read_bool( - m_dev, "diag_flash_memory_write_count_exceeded_error", &result) == 0); + 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::lost_samples_count(uint32_t & result) +bool IIOWrapper::diag_flash_memory_write_count_exceeded_error(bool & result) { - long long valuel; - int ret = iio_device_debug_attr_read_longlong(m_dev, "lost_samples_count", &valuel); + uint32_t reg_val; + bool ret = flash_counter(reg_val); + if (!ret) return false; - result = valuel; - return (ret == 0); + result = reg_val > ADIS_FLS_MEM_ENDURANCE; + return true; } 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 (ret == 0); + 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); } bool IIOWrapper::gyroscope_measurement_range(std::string & result) { - char valuec[32]; - int ret = iio_device_debug_attr_read(m_dev, "gyroscope_measurement_range", valuec, 32); + uint32_t reg_val; - result = valuec; - return (ret > 0); + if (!m_dev) return false; + + int ret = iio_device_reg_read(m_dev, ADIS_RANG_MDL_ADDR, ®_val); + if (ret) return false; + + 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: + result = "+/-500_degrees_per_sec"; + return true; + case 3: + result = "+/-2000_degrees_per_sec"; + return true; + default: + return false; + } } +#ifdef ADIS_SENS_BW bool IIOWrapper::internal_sensor_bandwidth(uint32_t & result) { - long long valuel; - int ret = iio_device_debug_attr_read_longlong(m_dev, "internal_sensor_bandwidth", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 (iio_device_debug_attr_write_longlong(m_dev, "internal_sensor_bandwidth", val) == 0); + return updateField(ADIS_MSC_CTRL_ADDR, val << ADIS_SENS_BW_POS, ADIS_SENS_BW); } +#endif bool IIOWrapper::point_of_percussion_alignment(uint32_t & result) { - long long valuel; - int ret = iio_device_debug_attr_read_longlong(m_dev, "point_of_percussion_alignment", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); -} + int ret = iio_device_reg_read(m_dev, ADIS_MSC_CTRL_ADDR, &result); + if (ret) return false; -bool IIOWrapper::update_point_of_percussion_alignment(uint32_t val) -{ - return (iio_device_debug_attr_write_longlong(m_dev, "point_of_percussion_alignment", val) == 0); + result = (result & ADIS_PT_OF_PERG_ALGNMNT) >> ADIS_PT_OF_PERG_ALGNMNT_POS; + return true; } -bool IIOWrapper::update_linear_acceleration_compensation(uint32_t val) +bool IIOWrapper::update_point_of_percussion_alignment(uint32_t val) { - return ( - iio_device_debug_attr_write_longlong(m_dev, "linear_acceleration_compensation", val) == 0); + return updateField( + ADIS_MSC_CTRL_ADDR, val << ADIS_PT_OF_PERG_ALGNMNT_POS, ADIS_PT_OF_PERG_ALGNMNT); } bool IIOWrapper::linear_acceleration_compensation(uint32_t & result) { - long long valuel; - int ret = iio_device_debug_attr_read_longlong(m_dev, "linear_acceleration_compensation", &valuel); - - result = valuel; - return (ret == 0); -} + if (!m_dev) return false; -bool IIOWrapper::burst_data_selection(uint32_t & result) -{ - long long valuel; - int ret = iio_device_debug_attr_read_longlong(m_dev, "burst_data_selection", &valuel); + int ret = iio_device_reg_read(m_dev, ADIS_MSC_CTRL_ADDR, &result); + if (ret) return false; - result = valuel; - return (ret == 0); + result = (result & ADIS_LN_ACCL_COMP) >> ADIS_LN_ACCL_COMP_POS; + return true; } -bool IIOWrapper::update_burst_data_selection(uint32_t val) +bool IIOWrapper::update_linear_acceleration_compensation(uint32_t val) { - return (iio_device_debug_attr_write_longlong(m_dev, "burst_data_selection", val) == 0); + return updateField(ADIS_MSC_CTRL_ADDR, val << ADIS_LN_ACCL_COMP_POS, ADIS_LN_ACCL_COMP); } +#ifdef ADIS_NULL_CNFG_ADDR bool IIOWrapper::bias_correction_time_base_control(uint32_t & result) { - long long valuel; - int ret = - iio_device_debug_attr_read_longlong(m_dev, "bias_correction_time_base_control", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 ( - iio_device_debug_attr_write_longlong(m_dev, "bias_correction_time_base_control", val) == 0); + 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) { - long long valuel; - int ret = - iio_device_debug_attr_read_longlong(m_dev, "x_axis_gyroscope_bias_correction_enable", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 ( - iio_device_debug_attr_write_longlong(m_dev, "x_axis_gyroscope_bias_correction_enable", val) == - 0); + 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) { - long long valuel; - int ret = - iio_device_debug_attr_read_longlong(m_dev, "y_axis_gyroscope_bias_correction_enable", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 ( - iio_device_debug_attr_write_longlong(m_dev, "y_axis_gyroscope_bias_correction_enable", val) == - 0); + 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) { - long long valuel; - int ret = - iio_device_debug_attr_read_longlong(m_dev, "z_axis_gyroscope_bias_correction_enable", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 ( - iio_device_debug_attr_write_longlong(m_dev, "z_axis_gyroscope_bias_correction_enable", val) == - 0); + 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) { - long long valuel; - int ret = iio_device_debug_attr_read_longlong( - m_dev, "x_axis_accelerometer_bias_correction_enable", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 ( - iio_device_debug_attr_write_longlong( - m_dev, "x_axis_accelerometer_bias_correction_enable", val) == 0); + 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) { - long long valuel; - int ret = iio_device_debug_attr_read_longlong( - m_dev, "y_axis_accelerometer_bias_correction_enable", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 ( - iio_device_debug_attr_write_longlong( - m_dev, "y_axis_accelerometer_bias_correction_enable", val) == 0); + 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) { - long long valuel; - int ret = iio_device_debug_attr_read_longlong( - m_dev, "z_axis_accelerometer_bias_correction_enable", &valuel); + if (!m_dev) return false; - result = valuel; - return (ret == 0); + 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 ( - iio_device_debug_attr_write_longlong( - m_dev, "z_axis_accelerometer_bias_correction_enable", val) == 0); + 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() { - return (iio_device_debug_attr_write_longlong(m_dev, "bias_correction_update", 1) == 0); + 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() { - return (iio_device_debug_attr_write_longlong(m_dev, "factory_calibration_restore", 1) == 0); + 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() { - return (iio_device_debug_attr_write_longlong(m_dev, "sensor_self_test", 1) == 0); + 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() { - return (iio_device_debug_attr_write_longlong(m_dev, "flash_memory_update", 1) == 0); + 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); } bool IIOWrapper::flash_memory_test() { - return (iio_device_debug_attr_write_longlong(m_dev, "flash_memory_test", 1) == 0); + 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); } bool IIOWrapper::software_reset() { - return (iio_device_debug_attr_write_longlong(m_dev, "software_reset", 1) == 0); + 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 (ret > 0); + 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 (ret > 0); + 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 (ret == 0); + 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 (ret == 0); + return true; } -bool IIOWrapper::flash_counter(uint32_t & value) +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_counter", &valuel); + int ret = iio_device_debug_attr_read_longlong(m_dev, "flash_count", &valuel); + if (ret) return false; - value = valuel; - return (ret == 0); + result = valuel; + return true; } double IIOWrapper::get_scale_accel() { return m_scale_accel_x; } diff --git a/src/imu_1650x_diag_data_provider.cpp b/src/imu_1650x_diag_data_provider.cpp deleted file mode 100644 index 780cdf9..0000000 --- a/src/imu_1650x_diag_data_provider.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/******************************************************************************* - * @file imu_1650x_diag_data_provider.cpp - * @brief Implementation for providing diagnosis data for adis1650x. - * @author Vasile Holonec (Vasile.Holonec@analog.com) - ******************************************************************************* - * Copyright 2023(c) Analog Devices, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License")) - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - ******************************************************************************/ - -#include "imu_ros2/imu_1650x_diag_data_provider.h" - -#include - -Imu1650xDiagDataProvider::Imu1650xDiagDataProvider() {} - -Imu1650xDiagDataProvider::~Imu1650xDiagDataProvider() {} - -bool Imu1650xDiagDataProvider::getData(imu_ros2::msg::Imu1650xDiagData & message) -{ - if (!m_iio_wrapper.diag_data_path_overrun(message.diag_data_path_overrun)) return false; - - if (!m_iio_wrapper.diag_flash_memory_update_error(message.diag_flash_memory_update_error)) - return false; - - if (!m_iio_wrapper.diag_spi_communication_error(message.diag_spi_communication_error)) - return false; - - if (!m_iio_wrapper.diag_standby_mode(message.diag_standby_mode)) return false; - - if (!m_iio_wrapper.diag_sensor_self_test_error(message.diag_sensor_self_test_error)) return false; - - if (!m_iio_wrapper.diag_flash_memory_test_error(message.diag_flash_memory_test_error)) - return false; - - if (!m_iio_wrapper.diag_clock_error(message.diag_clock_error)) return false; - - if (!m_iio_wrapper.diag_acceleration_self_test_error(message.diag_acceleration_self_test_error)) - return false; - - if (!m_iio_wrapper.diag_gyroscope1_self_test_error(message.diag_gyroscope1_self_test_error)) - return false; - - if (!m_iio_wrapper.diag_gyroscope2_self_test_error(message.diag_gyroscope2_self_test_error)) - return false; - - if (!m_iio_wrapper.diag_checksum_error_flag(message.diag_checksum_error_flag)) return false; - - if (!m_iio_wrapper.diag_flash_memory_write_count_exceeded_error( - message.diag_flash_memory_write_count_exceeded_error)) - return false; - - if (!m_iio_wrapper.flash_counter(message.flash_counter)) return false; - - return m_iio_wrapper.lost_samples_count(message.lost_samples_count); -} diff --git a/src/imu_1657x_diag_ros_publisher.cpp b/src/imu_1657x_diag_ros_publisher.cpp deleted file mode 100644 index dcbc0f0..0000000 --- a/src/imu_1657x_diag_ros_publisher.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/******************************************************************************* - * @file imu_1657x_diag_ros_publisher.cpp - * @brief Implementation for adis1657x diagnosis publisher. - * @author Vasile Holonec (Vasile.Holonec@analog.com) - ******************************************************************************* - * Copyright 2023(c) Analog Devices, Inc. - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - ******************************************************************************/ - -#include "imu_ros2/imu_1657x_diag_ros_publisher.h" - -#include - -Imu1657xDiagRosPublisher::Imu1657xDiagRosPublisher(std::shared_ptr & node) -{ - m_node = node; - m_publisher = node->create_publisher("imu1657xdiagdata", 10); -} - -Imu1657xDiagRosPublisher::~Imu1657xDiagRosPublisher() { delete m_data_provider; } - -void Imu1657xDiagRosPublisher::setMessageProvider(Imu1657xDiagDataProviderInterface * dataProvider) -{ - m_data_provider = dataProvider; -} - -void Imu1657xDiagRosPublisher::run() -{ - std::thread::id this_id = std::this_thread::get_id(); - std::cout << "thread " << this_id << " started...\n"; - RCLCPP_INFO( - rclcpp::get_logger("imu_1657x_diag_ros_publisher"), "startThread: Imu1657xDiagRosPublisher"); - - while (rclcpp::ok()) { - if (m_data_provider->getData(m_message)) - m_publisher->publish(m_message); - else - RCLCPP_INFO( - rclcpp::get_logger("imu_1657x_diag_ros_publisher"), "error reading diagnosis data"); - } - - this_id = std::this_thread::get_id(); - std::cout << "thread " << this_id << " ended...\n"; - RCLCPP_INFO( - rclcpp::get_logger("imu_1657x_diag_ros_publisher"), "endThread: Imu1657xDiagRosPublisher"); -} diff --git a/src/imu_control_parameters.cpp b/src/imu_control_parameters.cpp index 30a0701..f5b141a 100644 --- a/src/imu_control_parameters.cpp +++ b/src/imu_control_parameters.cpp @@ -29,17 +29,7 @@ ImuControlParameters::ImuControlParameters(std::shared_ptr & node) { m_node = node; - switch (IIOWrapper::s_device_name_enum) { - case IIODeviceName::ADIS1650X: - declareAdis1650xAttributes(); - break; - case IIODeviceName::ADIS1657X: - declareAdis1657xAttributes(); - break; - default: - break; - } - + declareAdisAttributes(); mapIIOUpdateFunctionsInt32(); mapIIOGetFunctionsInt32(); mapIIOUpdateFunctionsUint32(); @@ -53,23 +43,7 @@ ImuControlParameters::ImuControlParameters(std::shared_ptr & node) ImuControlParameters::~ImuControlParameters() {} -void ImuControlParameters::declareAdis1650xAttributes() -{ - m_attr_current_device.push_back("anglvel_calibbias_x"); - m_attr_current_device.push_back("anglvel_calibbias_y"); - m_attr_current_device.push_back("anglvel_calibbias_z"); - m_attr_current_device.push_back("accel_calibbias_x"); - m_attr_current_device.push_back("accel_calibbias_y"); - m_attr_current_device.push_back("accel_calibbias_z"); - - m_attr_current_device.push_back("filter_low_pass_3db_frequency"); - m_attr_current_device.push_back("internal_sensor_bandwidth"); - m_attr_current_device.push_back("point_of_percussion_alignment"); - m_attr_current_device.push_back("linear_acceleration_compensation"); - m_attr_current_device.push_back("sampling_frequency"); -} - -void ImuControlParameters::declareAdis1657xAttributes() +void ImuControlParameters::declareAdisAttributes() { m_attr_current_device.push_back("anglvel_calibbias_x"); m_attr_current_device.push_back("anglvel_calibbias_y"); @@ -79,10 +53,13 @@ void ImuControlParameters::declareAdis1657xAttributes() m_attr_current_device.push_back("accel_calibbias_z"); m_attr_current_device.push_back("filter_low_pass_3db_frequency"); +#ifdef ADIS_SENS_BW m_attr_current_device.push_back("internal_sensor_bandwidth"); +#endif m_attr_current_device.push_back("point_of_percussion_alignment"); m_attr_current_device.push_back("linear_acceleration_compensation"); +#ifdef ADIS_NULL_CNFG_ADDR m_attr_current_device.push_back("bias_correction_time_base_control"); m_attr_current_device.push_back("x_axis_gyroscope_bias_correction_enable"); m_attr_current_device.push_back("y_axis_accelerometer_bias_correction_enable"); @@ -90,6 +67,8 @@ void ImuControlParameters::declareAdis1657xAttributes() m_attr_current_device.push_back("x_axis_accelerometer_bias_correction_enable"); m_attr_current_device.push_back("y_axis_gyroscope_bias_correction_enable"); m_attr_current_device.push_back("z_axis_gyroscope_bias_correction_enable"); +#endif + m_attr_current_device.push_back("sampling_frequency"); } @@ -117,12 +96,15 @@ void ImuControlParameters::mapIIOUpdateFunctionsUint32() { m_func_map_update_uint32_params["filter_low_pass_3db_frequency"] = &IIOWrapper::update_filter_low_pass_3db_frequency; +#ifdef ADIS_SENS_BW m_func_map_update_uint32_params["internal_sensor_bandwidth"] = &IIOWrapper::update_internal_sensor_bandwidth; +#endif m_func_map_update_uint32_params["point_of_percussion_alignment"] = &IIOWrapper::update_point_of_percussion_alignment; m_func_map_update_uint32_params["linear_acceleration_compensation"] = &IIOWrapper::update_linear_acceleration_compensation; +#ifdef ADIS_NULL_CNFG_ADDR m_func_map_update_uint32_params["bias_correction_time_base_control"] = &IIOWrapper::update_bias_correction_time_base_control; m_func_map_update_uint32_params["x_axis_gyroscope_bias_correction_enable"] = @@ -137,18 +119,22 @@ void ImuControlParameters::mapIIOUpdateFunctionsUint32() &IIOWrapper::update_y_axis_accelerometer_bias_correction_enable; m_func_map_update_uint32_params["z_axis_accelerometer_bias_correction_enable"] = &IIOWrapper::update_z_axis_accelerometer_bias_correction_enable; +#endif } void ImuControlParameters::mapIIOGetFunctionsUint32() { m_func_map_get_uint32_params["filter_low_pass_3db_frequency"] = &IIOWrapper::filter_low_pass_3db_frequency; +#ifdef ADIS_SENS_BW m_func_map_get_uint32_params["internal_sensor_bandwidth"] = &IIOWrapper::internal_sensor_bandwidth; +#endif m_func_map_get_uint32_params["point_of_percussion_alignment"] = &IIOWrapper::point_of_percussion_alignment; m_func_map_get_uint32_params["linear_acceleration_compensation"] = &IIOWrapper::linear_acceleration_compensation; +#ifdef ADIS_NULL_CNFG_ADDR m_func_map_get_uint32_params["bias_correction_time_base_control"] = &IIOWrapper::bias_correction_time_base_control; m_func_map_get_uint32_params["x_axis_gyroscope_bias_correction_enable"] = @@ -163,6 +149,7 @@ void ImuControlParameters::mapIIOGetFunctionsUint32() &IIOWrapper::y_axis_accelerometer_bias_correction_enable; m_func_map_get_uint32_params["z_axis_accelerometer_bias_correction_enable"] = &IIOWrapper::z_axis_accelerometer_bias_correction_enable; +#endif } void ImuControlParameters::mapIIOUpdateFunctionsDouble() @@ -183,13 +170,9 @@ void ImuControlParameters::mapIIOCommandFunctions() m_func_map_execute_commands["sensor_self_test"] = &IIOWrapper::sensor_self_test; m_func_map_execute_commands["factory_calibration_restore"] = &IIOWrapper::factory_calibration_restore; - - switch (IIOWrapper::s_device_name_enum) { - case IIODeviceName::ADIS1657X: - m_func_map_execute_commands["bias_correction_update"] = &IIOWrapper::bias_correction_update; - default: - break; - } +#ifdef ADIS_BIAS_CORRECTION_UPDATE + m_func_map_execute_commands["bias_correction_update"] = &IIOWrapper::bias_correction_update; +#endif } void ImuControlParameters::declareParameterDescription() @@ -202,30 +185,28 @@ void ImuControlParameters::declareParameterDescription() auto param_range_0_720 = rcl_interfaces::msg::IntegerRange{}; param_range_0_720.from_value = 0; param_range_0_720.to_value = 720; - param_range_0_720.step = 1; auto param_range_01 = rcl_interfaces::msg::IntegerRange{}; param_range_01.from_value = 0; param_range_01.to_value = 1; param_range_01.step = 1; - auto param_range_0_12 = rcl_interfaces::msg::IntegerRange{}; - param_range_0_12.from_value = 0; - param_range_0_12.to_value = 12; - param_range_0_12.step = 1; - auto param_range_03 = rcl_interfaces::msg::IntegerRange{}; +#ifdef ADIS_HAS_DELTA_BURST param_range_03.from_value = 0; +#else + param_range_03.from_value = 1; +#endif param_range_03.to_value = 3; param_range_03.step = 1; - m_param_description["anglvel_calibbias_x"] = "x-axis acceleration offset correction"; + m_param_description["anglvel_calibbias_x"] = "x-axis angular velocity offset correction"; m_param_constraints_integer["anglvel_calibbias_x"] = param_range_calibbias; - m_param_description["anglvel_calibbias_y"] = "y-axis acceleration offset correction"; + m_param_description["anglvel_calibbias_y"] = "y-axis angular velocity offset correction"; m_param_constraints_integer["anglvel_calibbias_y"] = param_range_calibbias; - m_param_description["anglvel_calibbias_z"] = "z-axis acceleration offset correction"; + m_param_description["anglvel_calibbias_z"] = "z-axis angular velocity offset correction"; m_param_constraints_integer["anglvel_calibbias_z"] = param_range_calibbias; m_param_description["accel_calibbias_x"] = "x-axis acceleration offset correction"; @@ -240,77 +221,80 @@ void ImuControlParameters::declareParameterDescription() m_param_description["filter_low_pass_3db_frequency"] = "Low pass 3db frequency"; m_param_constraints_integer["filter_low_pass_3db_frequency"] = param_range_0_720; +#ifdef ADIS_SENS_BW m_param_description["internal_sensor_bandwidth"] = - "0 for wide bandwidth \n \ - 1 for 370 Hz"; + "\n0: wide bandwidth" + "\n1: 370 Hz"; m_param_constraints_integer["internal_sensor_bandwidth"] = param_range_01; +#endif m_param_description["point_of_percussion_alignment"] = - "0 for disabling point of percussion alignment \n \ - 1 for enabling point of percussion alignment"; + "\n0: point of percussion alignment disable" + "\n1: point of percussion alignment enable"; m_param_constraints_integer["point_of_percussion_alignment"] = param_range_01; m_param_description["linear_acceleration_compensation"] = - "0 for disabling linear acceleration compensation \n \ - 1 for enabling linear acceleration compensation"; + "\n0: linear acceleration compensation disable" + "\n1: linear acceleration compensation enable"; m_param_constraints_integer["linear_acceleration_compensation"] = param_range_01; +#ifdef ADIS_NULL_CNFG_ADDR + auto param_range_0_12 = rcl_interfaces::msg::IntegerRange{}; + param_range_0_12.from_value = 0; + param_range_0_12.to_value = 12; + param_range_0_12.step = 1; + m_param_description["bias_correction_time_base_control"] = "Time base control"; m_param_constraints_integer["bias_correction_time_base_control"] = param_range_0_12; m_param_description["x_axis_gyroscope_bias_correction_enable"] = - "0 x-axis gyroscope bias correction disabled \n \ - 1 x-axis gyroscope bias correction enabled"; + "\n0: x-axis gyroscope bias correction disabled" + "\n1: x-axis gyroscope bias correction enabled"; m_param_constraints_integer["x_axis_gyroscope_bias_correction_enable"] = param_range_01; m_param_description["y_axis_gyroscope_bias_correction_enable"] = - "0 y-axis gyroscope bias correction disabled \n \ - 1 y-axis gyroscope bias correction enabled"; + "\n0: y-axis gyroscope bias correction disabled" + "\n1: y-axis gyroscope bias correction enabled"; m_param_constraints_integer["y_axis_gyroscope_bias_correction_enable"] = param_range_01; m_param_description["z_axis_gyroscope_bias_correction_enable"] = - "0 z-axis gyroscope bias correction disabled \n \ - 1 z-axis gyroscope bias correction enabled"; + "\n0: z-axis gyroscope bias correction disabled" + "\n1: z-axis gyroscope bias correction enabled"; m_param_constraints_integer["z_axis_gyroscope_bias_correction_enable"] = param_range_01; m_param_description["x_axis_accelerometer_bias_correction_enable"] = - "0 x-axis accelerometer bias correction disabled \n \ - 1 x-axis accelerometer bias correction enabled"; + "\n0: x-axis accelerometer bias correction disabled" + "\n1: x-axis accelerometer bias correction enabled"; m_param_constraints_integer["x_axis_accelerometer_bias_correction_enable"] = param_range_01; m_param_description["y_axis_accelerometer_bias_correction_enable"] = - "0 y-axis accelerometer bias correction disabled \n \ - 1 y-axis accelerometer bias correction enabled"; + "\n0: y-axis accelerometer bias correction disabled" + "\n1: z-axis accelerometer bias correction enabled"; m_param_constraints_integer["y_axis_accelerometer_bias_correction_enable"] = param_range_01; m_param_description["z_axis_accelerometer_bias_correction_enable"] = - "0 z-axis accelerometer bias correction disabled \n \ - 1 z-axis accelerometer bias correction enabled"; + "\n0: z-axis accelerometer bias correction disabled" + "\n1: z-axis accelerometer bias correction enabled"; m_param_constraints_integer["z_axis_accelerometer_bias_correction_enable"] = param_range_01; +#endif + + m_param_description["measured_data_topic_selection"] = "\nmeasured_data_topic_selection values:"; +#ifdef ADIS_HAS_DELTA_BURST + m_param_description["measured_data_topic_selection"].append( + "\n0: measured data is published on /velangtempdata topic"); +#endif + m_param_description["measured_data_topic_selection"].append( + "\n1: measured data is published on /accelgyrotempdata topic"); + + m_param_description["measured_data_topic_selection"].append( + "\n2: measured data is published on /imu topic" + "\n3: measured data is published on /imufullmeasureddata topic (default)"); - m_param_description["measured_data_topic_selection"] = - "measured_data_topic_selection values:\n \ - 0: measured data is published on /accelgyrotempdata topic \n \ - 1: measured data is published on /velangtempdata topic \n \ - 2: measured data is published on /imu topic \n \ - 3: measured data is published on /imufullmeasureddata topic (default)"; m_param_constraints_integer["measured_data_topic_selection"] = param_range_03; auto param_range_float = rcl_interfaces::msg::FloatingPointRange{}; param_range_float.from_value = 1.0; - param_range_float.step = 0.1; - - switch (IIOWrapper::s_device_name_enum) { - case IIODeviceName::ADIS1650X: - param_range_float.to_value = 2000.0; - break; - case IIODeviceName::ADIS1657X: - param_range_float.to_value = 4000.0; - break; - default: - break; - } - + param_range_float.to_value = ADIS_MAX_SAMP_FREQ; m_param_description["sampling_frequency"] = "Device sampling frequency"; m_param_constraints_floating["sampling_frequency"] = param_range_float; } @@ -364,22 +348,16 @@ void ImuControlParameters::declareParameters() auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; param_desc.description = - "command_to_execute values:\n \ - software_reset: performs a software reset on the device \n \ - flash_memory_test: performs a flash memory test on the device \n \ - flash_memory_update: performs a flash memory update on the device \n \ - sensor_self_test: performs a sensor self test on the device \n \ - factory_calibration_restore: performs a factory calibration restore on the device"; - - switch (IIOWrapper::s_device_name_enum) { - case IIODeviceName::ADIS1657X: - param_desc.description.append( - "\n \ - bias_correction_update: triggers a bias correction, using the bias correction factors"); - break; - default: - break; - } + "\ncommand_to_execute values:" + "\nsoftware_reset: performs a software reset on the device" + "\nflash_memory_test: performs a flash memory test on the device" + "\nflash_memory_update: performs a flash memory update on the device" + "\nsensor_self_test: performs a sensor self test on the device" + "\nfactory_calibration_restore: performs a factory calibration restore on the device"; +#ifdef ADIS_BIAS_CORRECTION_UPDATE + param_desc.description.append( + "\nbias_correction_update: triggers a bias correction, using the bias correction factors"); +#endif m_node->declare_parameter("command_to_execute", "no_command", param_desc); } diff --git a/src/imu_data_provider.cpp b/src/imu_data_provider.cpp index 37b9a77..6df5e6d 100644 --- a/src/imu_data_provider.cpp +++ b/src/imu_data_provider.cpp @@ -38,6 +38,9 @@ bool ImuDataProvider::getData(sensor_msgs::msg::Imu & message) message.angular_velocity.y = m_iio_wrapper.getBuffAngularVelocityY(); message.angular_velocity.z = m_iio_wrapper.getBuffAngularVelocityZ(); + message.header.frame_id = "imu"; + m_iio_wrapper.getBuffSampleTimestamp(message.header.stamp.sec, message.header.stamp.nanosec); + message.orientation_covariance[0] = -1; return true; diff --git a/src/imu_1657x_diag_data_provider.cpp b/src/imu_diag_data_provider.cpp similarity index 71% rename from src/imu_1657x_diag_data_provider.cpp rename to src/imu_diag_data_provider.cpp index 95faa80..793f5f0 100644 --- a/src/imu_1657x_diag_data_provider.cpp +++ b/src/imu_diag_data_provider.cpp @@ -1,6 +1,6 @@ /******************************************************************************* - * @file imu_1657x_diag_data_provider.cpp - * @brief Implementation for providing diagnosis data for adis1657x. + * @file imu_diag_data_provider.cpp + * @brief Implementation for providing diagnosis data for adis. * @author Vasile Holonec (Vasile.Holonec@analog.com) ******************************************************************************* * Copyright 2023(c) Analog Devices, Inc. @@ -18,16 +18,22 @@ * limitations under the License. ******************************************************************************/ -#include "imu_ros2/imu_1657x_diag_data_provider.h" +#include "imu_ros2/imu_diag_data_provider.h" -Imu1657xDiagDataProvider::Imu1657xDiagDataProvider() {} +#include -Imu1657xDiagDataProvider::~Imu1657xDiagDataProvider() {} +ImuDiagDataProvider::ImuDiagDataProvider() {} -bool Imu1657xDiagDataProvider::getData(imu_ros2::msg::Imu1657xDiagData & message) +ImuDiagDataProvider::~ImuDiagDataProvider() {} + +bool ImuDiagDataProvider::getData(imu_ros2::msg::ImuDiagData & message) { + message.header.frame_id = "imudiagdata"; + +#ifdef ADIS_SNSR_INIT_FAIL if (!m_iio_wrapper.diag_sensor_initialization_failure(message.diag_sensor_initialization_failure)) return false; +#endif if (!m_iio_wrapper.diag_data_path_overrun(message.diag_data_path_overrun)) return false; @@ -46,33 +52,58 @@ bool Imu1657xDiagDataProvider::getData(imu_ros2::msg::Imu1657xDiagData & message if (!m_iio_wrapper.diag_clock_error(message.diag_clock_error)) return false; +#ifdef ADIS_GYRO1_FAIL + if (!m_iio_wrapper.diag_gyroscope1_self_test_error(message.diag_gyroscope1_self_test_error)) + return false; +#endif + +#ifdef ADIS_GYRO2_FAIL + if (!m_iio_wrapper.diag_gyroscope2_self_test_error(message.diag_gyroscope2_self_test_error)) + return false; +#endif + +#ifdef ADIS_ACCEL_FAIL + if (!m_iio_wrapper.diag_acceleration_self_test_error(message.diag_acceleration_self_test_error)) + return false; +#endif + +#ifdef ADIS_GYRO_X_FAIL if (!m_iio_wrapper.diag_x_axis_gyroscope_failure(message.diag_x_axis_gyroscope_failure)) return false; +#endif +#ifdef ADIS_GYRO_Y_FAIL if (!m_iio_wrapper.diag_y_axis_gyroscope_failure(message.diag_y_axis_gyroscope_failure)) return false; +#endif +#ifdef ADIS_GYRO_Z_FAIL if (!m_iio_wrapper.diag_z_axis_gyroscope_failure(message.diag_z_axis_gyroscope_failure)) return false; +#endif +#ifdef ADIS_ACCEL_X_FAIL if (!m_iio_wrapper.diag_x_axis_accelerometer_failure(message.diag_x_axis_accelerometer_failure)) return false; +#endif +#ifdef ADIS_ACCEL_Y_FAIL if (!m_iio_wrapper.diag_y_axis_accelerometer_failure(message.diag_y_axis_accelerometer_failure)) return false; +#endif +#ifdef ADIS_ACCEL_Z_FAIL if (!m_iio_wrapper.diag_z_axis_accelerometer_failure(message.diag_z_axis_accelerometer_failure)) return false; +#endif +#ifdef ADIS_ADUC_MCU_FAULT if (!m_iio_wrapper.diag_aduc_mcu_fault(message.diag_aduc_mcu_fault)) return false; - - if (!m_iio_wrapper.diag_checksum_error_flag(message.diag_checksum_error_flag)) return false; +#endif if (!m_iio_wrapper.diag_flash_memory_write_count_exceeded_error( message.diag_flash_memory_write_count_exceeded_error)) return false; - if (!m_iio_wrapper.flash_counter(message.flash_counter)) return false; - - return m_iio_wrapper.lost_samples_count(message.lost_samples_count); + return m_iio_wrapper.flash_counter(message.flash_counter); } diff --git a/src/imu_1650x_diag_ros_publisher.cpp b/src/imu_diag_ros_publisher.cpp similarity index 56% rename from src/imu_1650x_diag_ros_publisher.cpp rename to src/imu_diag_ros_publisher.cpp index 6496b7a..717dedf 100644 --- a/src/imu_1650x_diag_ros_publisher.cpp +++ b/src/imu_diag_ros_publisher.cpp @@ -1,6 +1,6 @@ /******************************************************************************* - * @file imu_1650x_diag_ros_publisher.cpp - * @brief Implementation for adis1650x diagnosis publisher. + * @file imu_diag_ros_publisher.cpp + * @brief Implementation for adis diagnosis publisher. * @author Vasile Holonec (Vasile.Holonec@analog.com) ******************************************************************************* * Copyright 2023(c) Analog Devices, Inc. @@ -18,40 +18,39 @@ * limitations under the License. ******************************************************************************/ -#include "imu_ros2/imu_1650x_diag_ros_publisher.h" +#include "imu_ros2/imu_diag_ros_publisher.h" #include -Imu1650xDiagRosPublisher::Imu1650xDiagRosPublisher(std::shared_ptr & node) +ImuDiagRosPublisher::ImuDiagRosPublisher(std::shared_ptr & node) { m_node = node; - m_publisher = node->create_publisher("imu1650xdiagdata", 10); + m_publisher = node->create_publisher("imudiagdata", 10); } -Imu1650xDiagRosPublisher::~Imu1650xDiagRosPublisher() { delete m_data_provider; } +ImuDiagRosPublisher::~ImuDiagRosPublisher() { delete m_data_provider; } -void Imu1650xDiagRosPublisher::setMessageProvider(Imu1650xDiagDataProviderInterface * dataProvider) +void ImuDiagRosPublisher::setMessageProvider(ImuDiagDataProviderInterface * dataProvider) { m_data_provider = dataProvider; } -void Imu1650xDiagRosPublisher::run() +void ImuDiagRosPublisher::run() { std::thread::id this_id = std::this_thread::get_id(); std::cout << "thread " << this_id << " started...\n"; - RCLCPP_INFO( - rclcpp::get_logger("imu_1650x_diag_ros_publisher"), "startThread: Imu1650xDiagRosPublisher"); + RCLCPP_INFO(rclcpp::get_logger("imu_diag_ros_publisher"), "startThread: ImuDiagRosPublisher"); while (rclcpp::ok()) { - if (m_data_provider->getData(m_message)) + if (m_data_provider->getData(m_message)) { + rclcpp::Time now = m_node->get_clock()->now(); + m_message.header.stamp = now; m_publisher->publish(m_message); - else - RCLCPP_INFO( - rclcpp::get_logger("imu_1650x_diag_ros_publisher"), "error reading diagnosis data"); + } else + RCLCPP_INFO(rclcpp::get_logger("imu_diag_ros_publisher"), "error reading diagnosis data"); } this_id = std::this_thread::get_id(); std::cout << "thread " << this_id << " ended...\n"; - RCLCPP_INFO( - rclcpp::get_logger("imu_1650x_diag_ros_publisher"), "endThread: Imu1650xDiagRosPublisher"); + RCLCPP_INFO(rclcpp::get_logger("imu_diag_ros_publisher"), "endThread: ImuDiagRosPublisher"); } diff --git a/src/imu_full_measured_data_provider.cpp b/src/imu_full_measured_data_provider.cpp index 05d7234..caee106 100644 --- a/src/imu_full_measured_data_provider.cpp +++ b/src/imu_full_measured_data_provider.cpp @@ -29,6 +29,8 @@ bool ImuFullMeasuredDataProvider::getData(imu_ros2::msg::ImuFullMeasuredData & d { m_iio_wrapper.stopBufferAcquisition(); + data.header.frame_id = "imufullmeasureddata"; + if (!m_iio_wrapper.getRegLinearAccelerationX(data.linear_acceleration.x)) return false; if (!m_iio_wrapper.getRegLinearAccelerationY(data.linear_acceleration.y)) return false; diff --git a/src/imu_full_measured_data_ros_publisher.cpp b/src/imu_full_measured_data_ros_publisher.cpp index 15dde22..8b2a641 100644 --- a/src/imu_full_measured_data_ros_publisher.cpp +++ b/src/imu_full_measured_data_ros_publisher.cpp @@ -24,8 +24,6 @@ #include #include -#include "imu_ros2/setting_declarations.h" - ImuFullMeasuredDataRosPublisher::ImuFullMeasuredDataRosPublisher( std::shared_ptr & node) { @@ -44,9 +42,11 @@ void ImuFullMeasuredDataRosPublisher::setMessageProvider( void ImuFullMeasuredDataRosPublisher::publish() { - if (m_data_provider->getData(m_message)) + if (m_data_provider->getData(m_message)) { + rclcpp::Time now = m_node->get_clock()->now(); + m_message.header.stamp = now; m_publisher->publish(m_message); - else + } else RCLCPP_INFO( rclcpp::get_logger("imu_full_measured_data_ros_publisher"), "error reading full measured data"); diff --git a/src/imu_identification_data_provider.cpp b/src/imu_identification_data_provider.cpp index c07b2e8..0a1eb2e 100644 --- a/src/imu_identification_data_provider.cpp +++ b/src/imu_identification_data_provider.cpp @@ -26,6 +26,8 @@ ImuIdentificationDataProvider::~ImuIdentificationDataProvider() {} bool ImuIdentificationDataProvider::getData(imu_ros2::msg::ImuIdentificationData & message) { + message.header.frame_id = "imuidentificationdata"; + if (!m_iio_wrapper.firmware_revision(message.firmware_revision)) return false; if (!m_iio_wrapper.firmware_date(message.firmware_date)) return false; diff --git a/src/imu_identification_ros_publisher.cpp b/src/imu_identification_ros_publisher.cpp index d91ae9d..d9961b9 100644 --- a/src/imu_identification_ros_publisher.cpp +++ b/src/imu_identification_ros_publisher.cpp @@ -22,8 +22,6 @@ #include -#include "imu_ros2/setting_declarations.h" - ImuIdentificationRosPublisher::ImuIdentificationRosPublisher(std::shared_ptr & node) { m_node = node; @@ -51,6 +49,8 @@ void ImuIdentificationRosPublisher::run() m_data_provider->getData(m_message); while (rclcpp::ok()) { + rclcpp::Time now = m_node->get_clock()->now(); + m_message.header.stamp = now; m_publisher->publish(m_message); } diff --git a/src/imu_ros2_node.cpp b/src/imu_ros2_node.cpp index bb9ed6a..b91eda9 100644 --- a/src/imu_ros2_node.cpp +++ b/src/imu_ros2_node.cpp @@ -18,19 +18,12 @@ * limitations under the License. *******************************************************************************/ -#include -#include -#include -#include - #include "imu_ros2/accelgyrotemp_data_provider.h" #include "imu_ros2/accelgyrotemp_ros_publisher.h" -#include "imu_ros2/imu_1650x_diag_data_provider.h" -#include "imu_ros2/imu_1650x_diag_ros_publisher.h" -#include "imu_ros2/imu_1657x_diag_data_provider.h" -#include "imu_ros2/imu_1657x_diag_ros_publisher.h" #include "imu_ros2/imu_control_parameters.h" #include "imu_ros2/imu_data_provider.h" +#include "imu_ros2/imu_diag_data_provider.h" +#include "imu_ros2/imu_diag_ros_publisher.h" #include "imu_ros2/imu_full_measured_data_provider.h" #include "imu_ros2/imu_full_measured_data_ros_publisher.h" #include "imu_ros2/imu_identification_data_provider.h" @@ -38,14 +31,13 @@ #include "imu_ros2/imu_ros_publisher.h" #include "imu_ros2/ros_publisher_group.h" #include "imu_ros2/ros_publisher_group_interface.h" -#include "imu_ros2/setting_declarations.h" +#ifdef ADIS_HAS_DELTA_BURST #include "imu_ros2/velangtemp_data_provider.h" #include "imu_ros2/velangtemp_ros_publisher.h" +#endif + #include "imu_ros2/worker_thread.h" #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; /** * @brief main function to run imu-ros2 @@ -64,6 +56,19 @@ int main(int argc, char * argv[]) std::cout << "mainthread " << this_id << " running...\n"; RCLCPP_INFO(rclcpp::get_logger("rclcpp_main"), "running: main thread"); + auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; + param_desc.description = + "\nDefault value is \'local:\', to be used when the imu_ros2 node is running locally." + "\nIf the imu_ros2 node is running remotely, please use \'ip:rpi_ip_address\',"; + + imu_node->declare_parameter("iio_context_string", "local:", param_desc); + + /* First make sure IIO context is available */ + std::string context = + imu_node->get_parameter("iio_context_string").get_parameter_value().get(); + IIOWrapper m_iio_wrapper; + m_iio_wrapper.createContext(context.c_str()); + ImuControlParameters * ctrl_params = new ImuControlParameters(imu_node); AccelGyroTempDataProviderInterface * accel_gyro_data_provider = new AccelGyroTempDataProvider(); @@ -75,9 +80,11 @@ int main(int argc, char * argv[]) ImuRosPublisherInterface * imu_std_publisher = new ImuRosPublisher(imu_node); imu_std_publisher->setMessageProvider(imu_std_data_provider); +#ifdef ADIS_HAS_DELTA_BURST VelAngTempDataProviderInterface * vel_ang_data_provider = new VelAngTempDataProvider(); VelAngTempRosPublisherInterface * vel_ang_publisher = new VelAngTempRosPublisher(imu_node); vel_ang_publisher->setMessageProvider(vel_ang_data_provider); +#endif ImuFullMeasuredDataProviderInterface * full_data_provider = new ImuFullMeasuredDataProvider(); ImuFullMeasuredDataRosPublisherInterface * full_data_publisher = @@ -86,7 +93,9 @@ int main(int argc, char * argv[]) RosPublisherGroupInterface * publisher_group = new RosPublisherGroup(imu_node); publisher_group->setAccelGyroTempRosPublisher(accel_gyro_publisher); +#ifdef ADIS_HAS_DELTA_BURST publisher_group->setVelAngTempRosPublisher(vel_ang_publisher); +#endif publisher_group->setImuRosPublisher(imu_std_publisher); publisher_group->setImuFullMeasuredDataRosPublisher(full_data_publisher); publisher_group->setImuControlParameters(ctrl_params); @@ -100,58 +109,34 @@ int main(int argc, char * argv[]) ident_publisher->setMessageProvider(ident_data_provider); RosTask * ident_task = dynamic_cast(ident_publisher); - Imu1650xDiagDataProviderInterface * diag1650x_data_provider = nullptr; - Imu1650xDiagRosPublisherInterface * diag1650x_publisher = nullptr; - RosTask * diag1650x_task = nullptr; - - Imu1657xDiagDataProviderInterface * diag1657x_data_provider = nullptr; - Imu1657xDiagRosPublisherInterface * diag1657x_publisher = nullptr; - RosTask * diag1657x_task = nullptr; - switch (IIOWrapper::s_device_name_enum) { - case IIODeviceName::ADIS1650X: - diag1650x_data_provider = new Imu1650xDiagDataProvider(); - diag1650x_publisher = new Imu1650xDiagRosPublisher(imu_node); - diag1650x_publisher->setMessageProvider(diag1650x_data_provider); - - diag1650x_task = dynamic_cast(diag1650x_publisher); - break; - case IIODeviceName::ADIS1657X: - diag1657x_data_provider = new Imu1657xDiagDataProvider(); - diag1657x_publisher = new Imu1657xDiagRosPublisher(imu_node); - diag1657x_publisher->setMessageProvider(diag1657x_data_provider); - - diag1657x_task = dynamic_cast(diag1657x_publisher); - break; - default: { - break; - } - } + ImuDiagDataProviderInterface * diag_data_provider = nullptr; + ImuDiagRosPublisherInterface * diag_publisher = nullptr; + RosTask * diag_task = nullptr; + + diag_data_provider = new ImuDiagDataProvider(); + diag_publisher = new ImuDiagRosPublisher(imu_node); + diag_publisher->setMessageProvider(diag_data_provider); + + diag_task = dynamic_cast(diag_publisher); WorkerThread publisher_group_thread(publisher_group_task); WorkerThread ident_thread(ident_task); - if (diag1650x_task) { - WorkerThread diag1650x_thread(diag1650x_task); - diag1650x_thread.join(); - } - - if (diag1657x_task) { - WorkerThread diag1657x_thread(diag1657x_task); - diag1657x_thread.join(); - } + WorkerThread diag_thread(diag_task); + diag_thread.join(); publisher_group_thread.join(); ident_thread.join(); delete ctrl_params; delete accel_gyro_publisher; +#ifdef ADIS_HAS_DELTA_BURST delete vel_ang_publisher; +#endif delete imu_std_publisher; delete full_data_publisher; delete ident_publisher; - - if (diag1650x_publisher) delete diag1650x_publisher; - if (diag1657x_publisher) delete diag1657x_publisher; + delete diag_publisher; rclcpp::shutdown(); diff --git a/src/imu_ros_publisher.cpp b/src/imu_ros_publisher.cpp index 1d88a24..c5a0001 100644 --- a/src/imu_ros_publisher.cpp +++ b/src/imu_ros_publisher.cpp @@ -23,8 +23,6 @@ #include #include -#include "imu_ros2/setting_declarations.h" - ImuRosPublisher::ImuRosPublisher(std::shared_ptr & node) { m_node = node; diff --git a/src/ros_publisher_group.cpp b/src/ros_publisher_group.cpp index 612d25d..bcc2201 100644 --- a/src/ros_publisher_group.cpp +++ b/src/ros_publisher_group.cpp @@ -27,8 +27,9 @@ #include "imu_ros2/imu_control_parameters.h" #include "imu_ros2/imu_full_measured_data_ros_publisher_interface.h" #include "imu_ros2/imu_ros_publisher_interface.h" -#include "imu_ros2/setting_declarations.h" +#ifdef ADIS_HAS_DELTA_BURST #include "imu_ros2/velangtemp_ros_publisher_interface.h" +#endif RosPublisherGroup::RosPublisherGroup(std::shared_ptr & node) { m_node = node; } @@ -40,11 +41,13 @@ void RosPublisherGroup::setAccelGyroTempRosPublisher( m_accelGyroTempRosPublisher = accelGyroTempRosPublisher; } +#ifdef ADIS_HAS_DELTA_BURST void RosPublisherGroup::setVelAngTempRosPublisher( VelAngTempRosPublisherInterface * velAngTempRosPublisher) { m_velAngTempRosPublisher = velAngTempRosPublisher; } +#endif void RosPublisherGroup::setImuRosPublisher(ImuRosPublisherInterface * imuRosPublisher) { @@ -78,9 +81,11 @@ void RosPublisherGroup::run() case ACCEL_GYRO_BUFFERED_DATA: m_accelGyroTempRosPublisher->publish(); break; +#ifdef ADIS_HAS_DELTA_BURST case DELTAVEL_DELTAANG_BUFFERED_DATA: m_velAngTempRosPublisher->publish(); break; +#endif case IMU_STD_MSG_DATA: m_imuRosPublisher->publish(); break; @@ -88,6 +93,9 @@ void RosPublisherGroup::run() m_imuFullMeasuredDataRosPublisher->publish(); break; default: { + RCLCPP_INFO( + rclcpp::get_logger("ros_publisher_group"), + "Invalid value for measured_data_topic_selection parameter."); break; } } diff --git a/src/velangtemp_data_provider.cpp b/src/velangtemp_data_provider.cpp index a130b79..3966c4e 100644 --- a/src/velangtemp_data_provider.cpp +++ b/src/velangtemp_data_provider.cpp @@ -27,6 +27,7 @@ VelAngTempDataProvider::~VelAngTempDataProvider() {} bool VelAngTempDataProvider::getData(imu_ros2::msg::VelAngTempData & message) { +#ifdef ADIS_HAS_DELTA_BURST if (!m_iio_wrapper.updateBuffer(DELTAVEL_DELTAANG_BUFFERED_DATA)) return false; message.delta_angle.x = m_iio_wrapper.getBuffDeltaAngleX(); @@ -39,7 +40,11 @@ bool VelAngTempDataProvider::getData(imu_ros2::msg::VelAngTempData & message) message.temperature = m_iio_wrapper.getBuffTemperature(); - message.timestamp = m_iio_wrapper.getBuffSampleTimestamp(); + message.header.frame_id = "velangtempdata"; + m_iio_wrapper.getBuffSampleTimestamp(message.header.stamp.sec, message.header.stamp.nanosec); return true; +#else + return false; +#endif } diff --git a/src/velangtemp_ros_publisher.cpp b/src/velangtemp_ros_publisher.cpp index 8939c32..040565f 100644 --- a/src/velangtemp_ros_publisher.cpp +++ b/src/velangtemp_ros_publisher.cpp @@ -24,8 +24,6 @@ #include #include -#include "imu_ros2/setting_declarations.h" - VelAngTempRosPublisher::VelAngTempRosPublisher(std::shared_ptr & node) { m_node = node; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index fb3b59f..13b1724 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -5,6 +5,95 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +string (COMPARE EQUAL "adis16465-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16465_1) +string (COMPARE EQUAL "adis16465-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16465_2) +string (COMPARE EQUAL "adis16465-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16465_3) +string (COMPARE EQUAL "adis16467-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16467_1) +string (COMPARE EQUAL "adis16467-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16467_2) +string (COMPARE EQUAL "adis16467-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16467_3) +string (COMPARE EQUAL "adis16470" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16470) +string (COMPARE EQUAL "adis16475-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16475_1) +string (COMPARE EQUAL "adis16475-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16475_2) +string (COMPARE EQUAL "adis16475-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16475_3) + +string (COMPARE EQUAL "adis16477-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16477_1) +string (COMPARE EQUAL "adis16477-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16477_2) +string (COMPARE EQUAL "adis16477-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16477_3) + +string (COMPARE EQUAL "adis16500" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16500) +string (COMPARE EQUAL "adis16505-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16505_1) +string (COMPARE EQUAL "adis16505-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16505_2) +string (COMPARE EQUAL "adis16505-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16505_3) +string (COMPARE EQUAL "adis16507-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16507_1) +string (COMPARE EQUAL "adis16507-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16507_2) +string (COMPARE EQUAL "adis16507-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16507_3) + +string (COMPARE EQUAL "adis16575-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16575_2) +string (COMPARE EQUAL "adis16575-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16575_3) +string (COMPARE EQUAL "adis16576-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16576_2) +string (COMPARE EQUAL "adis16576-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16576_3) +string (COMPARE EQUAL "adis16577-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16577_2) +string (COMPARE EQUAL "adis16577-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16577_3) + +if (${DEVICE_FOUND_ADIS16500} OR + ${DEVICE_FOUND_ADIS16505_1} OR + ${DEVICE_FOUND_ADIS16505_2} OR + ${DEVICE_FOUND_ADIS16505_3} OR + ${DEVICE_FOUND_ADIS16507_1} OR + ${DEVICE_FOUND_ADIS16507_2} OR + ${DEVICE_FOUND_ADIS16507_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1650x) + add_compile_definitions(ADIS1650X) + +elseif (${DEVICE_FOUND_ADIS16465_1} OR + ${DEVICE_FOUND_ADIS16465_2} OR + ${DEVICE_FOUND_ADIS16465_3} OR + ${DEVICE_FOUND_ADIS16467_1} OR + ${DEVICE_FOUND_ADIS16467_2} OR + ${DEVICE_FOUND_ADIS16467_3} OR + ${DEVICE_FOUND_ADIS16470} OR + ${DEVICE_FOUND_ADIS16475_1} OR + ${DEVICE_FOUND_ADIS16475_2} OR + ${DEVICE_FOUND_ADIS16475_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1646x) + add_compile_definitions(ADIS1646X) + +elseif (${DEVICE_FOUND_ADIS16565_2} OR + ${DEVICE_FOUND_ADIS16565_3} OR + ${DEVICE_FOUND_ADIS16576_2} OR + ${DEVICE_FOUND_ADIS16576_3} OR + ${DEVICE_FOUND_ADIS16577_2} OR + ${DEVICE_FOUND_ADIS16577_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1657x) + add_compile_definitions(ADIS1657X) + +elseif (${DEVICE_FOUND_ADIS16477_1} OR + ${DEVICE_FOUND_ADIS16477_2} OR + ${DEVICE_FOUND_ADIS16477_3}) + message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") + set(DEVICE_PATH adis1647x) + add_compile_definitions(ADIS1647X) + +else() + message (FATAL_ERROR " + DEVICE_ID=$ENV{DEVICE_ID} is not a valid value. + Please use one of the following 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, + adis16505-1, adis16505-2, adis16505-3, + adis16507-1, adis16507-2, adis16507-3, + adis16575-2, adis16575-3, + adis16576-2, adis16576-3, + adis16577-2, adis16577-3.") +endif() + # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) @@ -16,7 +105,8 @@ find_library(LIBIIO_LIBRARIES iio) find_path(LIBIIO_INCLUDE_DIRS iio.h) file(GLOB FILES_H ${CMAKE_SOURCE_DIR}/include/*.h - ${CMAKE_SOURCE_DIR}/include/iio_ros2_package/*.h) + ${CMAKE_SOURCE_DIR}/include/iio_ros2_package/*.h + ${CMAKE_SOURCE_DIR}/include/imu_ros2/${DEVICE_PATH}/*.h) file(GLOB FILES_CPP ${CMAKE_SOURCE_DIR}/src/*.cpp) @@ -28,8 +118,7 @@ ament_add_gtest(imu_ros2_test_node utest_launch.test src/utest.cpp src/imu_subscriber_test.cpp src/imu_full_measured_data_subscriber_test.cpp src/imu_identification_subscriber_test.cpp - src/imu_1650x_diag_subscriber_test.cpp - src/imu_1657x_diag_subscriber_test.cpp + src/imu_diag_subscriber_test.cpp src/velangtemp_subscriber_test.cpp src/accelgyrotemp_subscriber_test.cpp ${CMAKE_SOURCE_DIR}/include/imu_ros2/iio_wrapper.h @@ -45,7 +134,7 @@ ament_target_dependencies(imu_ros2_test_node rclcpp geometry_msgs std_msgs senso install(TARGETS imu_ros2_test_node DESTINATION lib/${PROJECT_NAME}) - -rosidl_target_interfaces(imu_ros2_test_node + rosidl_get_typesupport_target(imu_ros2_test_node imu_ros2 "rosidl_typesupport_cpp") + target_link_libraries(imu_ros2_test_node "${imu_ros2_node}" iio) diff --git a/test/package.xml b/test/package.xml index df527d5..3009a6b 100644 --- a/test/package.xml +++ b/test/package.xml @@ -2,21 +2,19 @@ imu_ros2_test - 0.0.0 - TODO: Package description - vholonec - TODO: License declaration + 0.0.1 + Tests for ADI IMUs Publisher + rbolboac + Apache License Version 2.0 ament_cmake rclcpp std_msgs sensor_msgs - ament_lint_auto - ament_lint_common ament_cmake_gtest - - builtin_interfaces + + builtin_interfaces rosidl_default_generators builtin_interfaces rosidl_default_runtime diff --git a/test/src/accelgyrotemp_subscriber_test.cpp b/test/src/accelgyrotemp_subscriber_test.cpp index f6a1de1..dd2ec19 100644 --- a/test/src/accelgyrotemp_subscriber_test.cpp +++ b/test/src/accelgyrotemp_subscriber_test.cpp @@ -58,7 +58,14 @@ TEST(AccelGyroTempSubscriberTest, test_accelgyrotemp_publisher) { IIOWrapper iio_wrapper; - auto node = rclcpp::Node::make_shared("accelgyrotempdata"); + auto node = rclcpp::Node::make_shared("test_accelgyrotempdata_publisher"); + + node->declare_parameter("iio_context_string", "local:"); + + std::string context = + node->get_parameter("iio_context_string").get_parameter_value().get(); + IIOWrapper m_iio_wrapper; + m_iio_wrapper.createContext(context.c_str()); std::string topic = "accelgyrotempdata"; @@ -71,9 +78,10 @@ TEST(AccelGyroTempSubscriberTest, test_accelgyrotemp_publisher) &callbackExecuted](imu_ros2::msg::AccelGyroTempData msg) -> void { RCLCPP_INFO( rclcpp::get_logger("accelgyrotemp_subscriber_test"), - "linear acceleration x axis: %f \nlinear acceleration y axis: %f\nlinear acceleration z " + "\nlinear acceleration x axis: %f \nlinear acceleration y axis: %f\nlinear acceleration z " "axis: %f\nangular velocity x axis: %f\nangular velocity y axis: %f\nangular velocity z " - "axis: %f\n temperature: %f\n ", + "axis: %f\n" + "temperature: %f\n ", msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z, msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, msg.temperature); diff --git a/test/src/imu_1657x_diag_subscriber_test.cpp b/test/src/imu_1657x_diag_subscriber_test.cpp deleted file mode 100644 index 6cf820b..0000000 --- a/test/src/imu_1657x_diag_subscriber_test.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/******************************************************************************* -* @file imu_1657x_diag_subscriber_test.cpp -* @brief Test imu 1657x diag data -* @author Vasile Holonec (Vasile.Holonec@analog.com) -******************************************************************************** -* Copyright 2023(c) Analog Devices, Inc. - -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -#include - -#include -#include - -#include "imu_ros2/msg/imu1657x_diag_data.hpp" - -/** - * @brief Class for testing Imu1650xDiagData. - * - * This class instantiates a subscriber node and listens to data on - * Imu1657xDiagData topic and compares it against a range of expected - * values. - */ -class Imu1657xDiagSubscriberTest : public ::testing::Test -{ -public: - /** - * @brief Set up the test case. - */ - static void SetUpTestCase() {} - - /** - * @brief Tear down the test case. - */ - static void TearDownTestCase() { rclcpp::shutdown(); } -}; - -/** - * @brief Imu1657xDiagSubscriberTest - * - * This test instantiates a subscriber node and listens to data on - * Imu1657xDiagData topic and compares it against a range of expected - * values. - */ -TEST(Imu1657xDiagSubscriberTest, test_imu_1657x_diag_data_publisher) -{ - auto node = rclcpp::Node::make_shared("imu1657xdiagdata"); - - std::string topic = "imu1657xdiagdata"; - bool callbackExecuted = false; - - auto callback = [&callbackExecuted](imu_ros2::msg::Imu1657xDiagData msg) -> void { - RCLCPP_INFO( - rclcpp::get_logger("imu_1657x_diag_subscriber_test"), - "diag_sensor_initialization_failure= %d \n" - "diag data: diag_data_path_overrun = %d \n" - "diag_flash_memory_update_error = %d \n" - "diag_spi_communication_error = %d \n" - "diag_standby_mode = %d \n" - "diag_sensor_self_test_error = %d \n" - "diag_flash_memory_test_error = %d \n" - "diag_clock_error = %d \n" - "diag_x_axis_gyroscope_failure = %d \n" - "diag_y_axis_gyroscope_failure = %d \n" - "diag_z_axis_gyroscope_failure = %d \n" - "diag_x_axis_accelerometer_failure = %d \n" - "diag_y_axis_accelerometer_failure = %d \n" - "diag_z_axis_accelerometer_failure = %d \n" - "diag_aduc_mcu_fault = %d \n" - "diag_checksum_error_flag = %d \n" - "diag_flash_memory_write_count_exceeded_error = %d \n" - "flash_counter = %d \n" - "lost_samples_count = %d \n", - msg.diag_sensor_initialization_failure, msg.diag_data_path_overrun, - msg.diag_flash_memory_update_error, msg.diag_spi_communication_error, msg.diag_standby_mode, - msg.diag_sensor_self_test_error, msg.diag_flash_memory_test_error, msg.diag_clock_error, - msg.diag_x_axis_gyroscope_failure, msg.diag_y_axis_gyroscope_failure, - msg.diag_z_axis_gyroscope_failure, msg.diag_x_axis_accelerometer_failure, - msg.diag_y_axis_accelerometer_failure, msg.diag_z_axis_accelerometer_failure, - msg.diag_aduc_mcu_fault, msg.diag_checksum_error_flag, - msg.diag_flash_memory_write_count_exceeded_error, msg.flash_counter, msg.lost_samples_count); - - ASSERT_TRUE(msg.diag_sensor_initialization_failure == false); - ASSERT_TRUE(msg.diag_data_path_overrun == false); - ASSERT_TRUE(msg.diag_flash_memory_update_error == false); - ASSERT_TRUE(msg.diag_spi_communication_error == false); - ASSERT_TRUE(msg.diag_standby_mode == false); - ASSERT_TRUE(msg.diag_sensor_self_test_error == false); - ASSERT_TRUE(msg.diag_flash_memory_test_error == false); - ASSERT_TRUE(msg.diag_clock_error == false); - ASSERT_TRUE(msg.diag_x_axis_gyroscope_failure == false); - ASSERT_TRUE(msg.diag_y_axis_gyroscope_failure == false); - ASSERT_TRUE(msg.diag_z_axis_gyroscope_failure == false); - ASSERT_TRUE(msg.diag_x_axis_accelerometer_failure == false); - ASSERT_TRUE(msg.diag_y_axis_accelerometer_failure == false); - ASSERT_TRUE(msg.diag_z_axis_accelerometer_failure == false); - ASSERT_TRUE(msg.diag_aduc_mcu_fault == false); - ASSERT_TRUE(msg.diag_checksum_error_flag == false); - ASSERT_TRUE(msg.diag_flash_memory_write_count_exceeded_error == false); - ASSERT_TRUE(msg.flash_counter < 100000); - ASSERT_TRUE(msg.lost_samples_count == 0); - - callbackExecuted = true; - }; - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - - auto subscriber = node->create_subscription(topic, 10, callback); - - std::chrono::seconds sec(1); - - while (!callbackExecuted) executor.spin_once(sec); -} diff --git a/test/src/imu_1650x_diag_subscriber_test.cpp b/test/src/imu_diag_subscriber_test.cpp similarity index 53% rename from test/src/imu_1650x_diag_subscriber_test.cpp rename to test/src/imu_diag_subscriber_test.cpp index cbedf7c..b1d7202 100644 --- a/test/src/imu_1650x_diag_subscriber_test.cpp +++ b/test/src/imu_diag_subscriber_test.cpp @@ -1,6 +1,6 @@ /******************************************************************************* -* @file imu_1650x_diag_subscriber_test.cpp -* @brief Test imu 1650x diag data +* @file imu__diag_subscriber_test.cpp +* @brief Test imu diag data * @author Vasile Holonec (Vasile.Holonec@analog.com) ******************************************************************************** * Copyright 2023(c) Analog Devices, Inc. @@ -23,16 +23,17 @@ #include #include -#include "imu_ros2/msg/imu1650x_diag_data.hpp" +#include "imu_ros2/adis_data_access.h" +#include "imu_ros2/msg/imu_diag_data.hpp" /** - * @brief Class for testing Imu1650xDiagData. + * @brief Class for testing ImuDiagData. * * This class instantiates a subscriber node and listens to data on - * Imu1650xDiagData topic and compares it against a range of expected + * ImuDiagData topic and compares it against a range of expected * values. */ -class Imu1650xDiagSubscriberTest : public ::testing::Test +class ImuDiagSubscriberTest : public ::testing::Test { public: /** @@ -47,43 +48,24 @@ class Imu1650xDiagSubscriberTest : public ::testing::Test }; /** - * @brief Imu1650xDiagSubscriberTest + * @brief ImuDiagSubscriberTest * * This test instantiates a subscriber node and listens to data on - * Imu1650xDiagData topic and compares it against a range of expected + * ImuDiagData topic and compares it against a range of expected * values. */ -TEST(Imu1650xDiagSubscriberTest, test_imu_1650x_diag_data_publisher) +TEST(ImuDiagSubscriberTest, test_imu__diag_data_publisher) { - auto node = rclcpp::Node::make_shared("imu1650xdiagdata"); + auto node = rclcpp::Node::make_shared("test_imudiagdata_publisher"); - std::string topic = "imu1650xdiagdata"; + std::string topic = "imudiagdata"; bool callbackExecuted = false; - auto callback = [&callbackExecuted](imu_ros2::msg::Imu1650xDiagData msg) -> void { - RCLCPP_INFO( - rclcpp::get_logger("imu_1650x_diag_subscriber_test"), - "diag data: diag_data_path_overrun = %d \n" - "diag_flash_memory_update_error = %d \n" - "diag_spi_communication_error = %d \n" - "diag_standby_mode = %d \n" - "diag_sensor_self_test_error = %d \n" - "diag_flash_memory_test_error = %d \n" - "diag_clock_error = %d \n" - "diag_acceleration_self_test_error = %d \n" - "diag_gyroscope1_self_test_error = %d \n" - "diag_gyroscope2_self_test_error = %d \n" - "diag_checksum_error_flag = %d \n" - "diag_flash_memory_write_count_exceeded_error = %d \n" - "lost_samples_count = %d \n" - "flash_counter = %d \n", - msg.diag_data_path_overrun, msg.diag_flash_memory_update_error, - msg.diag_spi_communication_error, msg.diag_standby_mode, msg.diag_sensor_self_test_error, - msg.diag_flash_memory_test_error, msg.diag_clock_error, msg.diag_acceleration_self_test_error, - msg.diag_gyroscope1_self_test_error, msg.diag_gyroscope2_self_test_error, - msg.diag_checksum_error_flag, msg.diag_flash_memory_write_count_exceeded_error, - msg.lost_samples_count, msg.flash_counter); + auto callback = [&callbackExecuted](imu_ros2::msg::ImuDiagData msg) -> void { +#ifdef ADIS_SNSR_INIT_FAIL + ASSERT_TRUE(msg.diag_sensor_initialization_failure == false); +#endif ASSERT_TRUE(msg.diag_data_path_overrun == false); ASSERT_TRUE(msg.diag_flash_memory_update_error == false); ASSERT_TRUE(msg.diag_spi_communication_error == false); @@ -91,21 +73,46 @@ TEST(Imu1650xDiagSubscriberTest, test_imu_1650x_diag_data_publisher) ASSERT_TRUE(msg.diag_sensor_self_test_error == false); ASSERT_TRUE(msg.diag_flash_memory_test_error == false); ASSERT_TRUE(msg.diag_clock_error == false); +#ifdef ADIS_ACCEL_FAIL ASSERT_TRUE(msg.diag_acceleration_self_test_error == false); +#endif +#ifdef ADIS_GYRO1_FAIL ASSERT_TRUE(msg.diag_gyroscope1_self_test_error == false); +#endif +#ifdef ADIS_GYRO2_FAIL ASSERT_TRUE(msg.diag_gyroscope2_self_test_error == false); - ASSERT_TRUE(msg.diag_checksum_error_flag == false); +#endif +#ifdef ADIS_GYRO_X_FAIL + ASSERT_TRUE(msg.diag_x_axis_gyroscope_failure == false); +#endif +#ifdef ADIS_GYRO_Y_FAIL + ASSERT_TRUE(msg.diag_y_axis_gyroscope_failure == false); +#endif +#ifdef ADIS_GYRO_Z_FAIL + ASSERT_TRUE(msg.diag_z_axis_gyroscope_failure == false); +#endif +#ifdef ADIS_ACCEL_X_FAIL + ASSERT_TRUE(msg.diag_x_axis_accelerometer_failure == false); +#endif +#ifdef ADIS_ACCEL_Y_FAIL + ASSERT_TRUE(msg.diag_y_axis_accelerometer_failure == false); +#endif +#ifdef ADIS_ACCEL_Z_FAIL + ASSERT_TRUE(msg.diag_z_axis_accelerometer_failure == false); +#endif +#ifdef ADIS_ADUC_MCU_FAULT + ASSERT_TRUE(msg.diag_aduc_mcu_fault == false); +#endif ASSERT_TRUE(msg.diag_flash_memory_write_count_exceeded_error == false); - ASSERT_TRUE(msg.lost_samples_count == 0); - ASSERT_TRUE(msg.flash_counter < 10000); + ASSERT_TRUE(msg.flash_counter < ADIS_FLS_MEM_ENDURANCE); callbackExecuted = true; }; rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); - auto subscriber = node->create_subscription(topic, 10, callback); + auto subscriber = node->create_subscription(topic, 10, callback); std::chrono::seconds sec(1); diff --git a/test/src/imu_full_measured_data_subscriber_test.cpp b/test/src/imu_full_measured_data_subscriber_test.cpp index fc3b33c..f0d416c 100644 --- a/test/src/imu_full_measured_data_subscriber_test.cpp +++ b/test/src/imu_full_measured_data_subscriber_test.cpp @@ -58,7 +58,14 @@ TEST(ImuFullMeasuredDataSubscriberTest, test_imu_full_measured_data_publisher) { IIOWrapper iio_wrapper; - auto node = rclcpp::Node::make_shared("imufullmeasureddata"); + auto node = rclcpp::Node::make_shared("test_imufullmeasureddata_publisher"); + + node->declare_parameter("iio_context_string", "local:"); + + std::string context = + node->get_parameter("iio_context_string").get_parameter_value().get(); + IIOWrapper m_iio_wrapper; + m_iio_wrapper.createContext(context.c_str()); std::string topic = "imufullmeasureddata"; @@ -73,7 +80,7 @@ TEST(ImuFullMeasuredDataSubscriberTest, test_imu_full_measured_data_publisher) &callbackExecuted](imu_ros2::msg::ImuFullMeasuredData msg) -> void { RCLCPP_INFO( rclcpp::get_logger("imu_full_measured_data_subscriber_test"), - "linear acceleration x axis: %f \nlinear acceleration y axis: %f\nlinear acceleration z " + "\nlinear acceleration x axis: %f \nlinear acceleration y axis: %f\nlinear acceleration z " "axis: %f\nangular velocity x axis: %f\nangular velocity y axis: %f\n" "angular velocity z axis: %f\ndelta velocity x axis: %f \ndelta velocity y axis: %f\n" "delta velocity z axis: %f\ndelta angle x axis: %f\ndelta angle y axis: %f\n" diff --git a/test/src/imu_identification_subscriber_test.cpp b/test/src/imu_identification_subscriber_test.cpp index 163cd32..3d44e48 100644 --- a/test/src/imu_identification_subscriber_test.cpp +++ b/test/src/imu_identification_subscriber_test.cpp @@ -58,7 +58,15 @@ TEST(ImuIdentificationSubscriberTest, test_imu_identification_publisher) { IIOWrapper iio_wrapper; - auto node = rclcpp::Node::make_shared("imuidentificationdata"); + auto node = rclcpp::Node::make_shared("test_imuidentificationdata_publisher"); + + node->declare_parameter("iio_context_string", "local:"); + + std::string context = + node->get_parameter("iio_context_string").get_parameter_value().get(); + IIOWrapper m_iio_wrapper; + m_iio_wrapper.createContext(context.c_str()); + std::string topic = "imuidentificationdata"; bool callbackExecuted = false; @@ -73,8 +81,14 @@ TEST(ImuIdentificationSubscriberTest, test_imu_identification_publisher) auto callback = [&imu_message, &callbackExecuted](imu_ros2::msg::ImuIdentificationData msg) -> void { RCLCPP_INFO( - rclcpp::get_logger("imu_identification_subscriber_test"), " device info: %s %s %d \n", - msg.firmware_revision.c_str(), msg.firmware_date.c_str(), msg.product_id); + rclcpp::get_logger("imu_identification_subscriber_test"), + "\nproduct id: %d" + "\nserial number: %d" + "\nfirmware revision: %s" + "\nfirmare date: %s" + "\ngyroscope measurement range: %s", + msg.product_id, msg.serial_number, msg.firmware_revision.c_str(), msg.firmware_date.c_str(), + msg.gyroscope_measurement_range.c_str()); ASSERT_TRUE(msg.firmware_revision == imu_message.firmware_revision); ASSERT_TRUE(msg.firmware_date == imu_message.firmware_date); ASSERT_TRUE(msg.product_id == imu_message.product_id); diff --git a/test/src/imu_subscriber_test.cpp b/test/src/imu_subscriber_test.cpp index 0e0e99c..99b8c9f 100644 --- a/test/src/imu_subscriber_test.cpp +++ b/test/src/imu_subscriber_test.cpp @@ -58,19 +58,27 @@ TEST(ImuSubscriberTest, test_imu_publisher) { IIOWrapper iio_wrapper; - auto node = rclcpp::Node::make_shared("imu"); + auto node = rclcpp::Node::make_shared("test_imu_publisher"); + + node->declare_parameter("iio_context_string", "local:"); + + std::string context = + node->get_parameter("iio_context_string").get_parameter_value().get(); + IIOWrapper m_iio_wrapper; + m_iio_wrapper.createContext(context.c_str()); std::string topic = "imu"; double scale_accel = iio_wrapper.get_scale_accel(); double scale_angvel = iio_wrapper.get_scale_anglvel(); + bool callbackExecuted = false; auto callback = [&scale_accel, &scale_angvel, &callbackExecuted](sensor_msgs::msg::Imu msg) -> void { RCLCPP_INFO( rclcpp::get_logger("imu_subscriber_test"), - "linear acceleration x axis: %f \nlinear acceleration y axis: %f\nlinear acceleration z " + "\nlinear acceleration x axis: %f \nlinear acceleration y axis: %f\nlinear acceleration z " "axis: %f\n" "angular velocity x axis: %f\nangular velocity y axis: %f\nangular velocity z axis: %f\n", msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z, diff --git a/test/src/velangtemp_subscriber_test.cpp b/test/src/velangtemp_subscriber_test.cpp index 7acbdac..e0eeb9f 100644 --- a/test/src/velangtemp_subscriber_test.cpp +++ b/test/src/velangtemp_subscriber_test.cpp @@ -54,11 +54,19 @@ class VelAngTempSubscriberTest : public ::testing::Test * VelAngTempData topic and compares it against a range of expected * values. */ +#ifdef ADIS_HAS_DELTA_BURST TEST(VelAngTempSubscriberTest, test_velangtemp_publisher) { IIOWrapper iio_wrapper; - auto node = rclcpp::Node::make_shared("velangtempdata"); + auto node = rclcpp::Node::make_shared("test_velangtempdata_publisher"); + + node->declare_parameter("iio_context_string", "local:"); + + std::string context = + node->get_parameter("iio_context_string").get_parameter_value().get(); + IIOWrapper m_iio_wrapper; + m_iio_wrapper.createContext(context.c_str()); std::string topic = "velangtempdata"; @@ -114,3 +122,4 @@ TEST(VelAngTempSubscriberTest, test_velangtemp_publisher) while (!callbackExecuted) executor.spin_once(sec); } +#endif