Add support for adis1655X

Signed-off-by: rbudai <[email protected]>

.gitignore

@@ -1,3 +1,4 @@
 # Ignore all txtuser files
 *.txt.user
 *.cpp.orig
+.vscode

CMakeLists.txt

@@ -36,6 +36,10 @@ string (COMPARE EQUAL "adis16547-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16547_1)
 string (COMPARE EQUAL "adis16547-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16547_2)
 string (COMPARE EQUAL "adis16547-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16547_3)
 
+string(COMPARE EQUAL "adis16550" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16550)
+string(COMPARE EQUAL "adis16550w" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16550W)
+string(COMPARE EQUAL "adis16550W" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16550W)
+
 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)
@@ -79,6 +83,12 @@ message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.")
 set(DEVICE_PATH adis1654x)
 add_compile_definitions(ADIS1654X)
 
+elseif (${DEVICE_FOUND_ADIS16550} OR 
+        ${DEVICE_FOUND_ADIS16550W})
+message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.")
+set(DEVICE_PATH adis1655x)
+add_compile_definitions(ADIS1655X)
+
 elseif (${DEVICE_FOUND_ADIS16575_2} OR
         ${DEVICE_FOUND_ADIS16575_3} OR
         ${DEVICE_FOUND_ADIS16576_2} OR
@@ -111,6 +121,7 @@ else()
   adis16507-1, adis16507-2, adis16507-3,
   adis16545-1, adis16545-2, adis16545-3,
   adis16547-1, adis16547-2, adis16547-3,
+  adis16550, adis16550W,
   adis16575-2, adis16575-3,
   adis16576-2, adis16576-3,
   adis16577-2, adis16577-3.")

config/adis1655x/imu_config.yaml

@@ -0,0 +1,27 @@
+/imu_ros2_node:
+  ros__parameters:
+    accel_calibbias_x: 0
+    accel_calibscale_x: 0
+    accel_calibbias_y: 0
+    accel_calibscale_y: 0
+    accel_calibbias_z: 0
+    accel_calibscale_z: 0
+    anglvel_calibbias_x: 0
+    anglvel_calibscale_x: 0
+    anglvel_calibbias_y: 0
+    anglvel_calibscale_y: 0
+    anglvel_calibbias_z: 0
+    anglvel_calibscale_z: 0
+    filter_low_pass_3db_frequency: 100
+    point_of_percussion_alignment: 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: 4000.0
+    command_to_execute: no_command
+    measured_data_topic_selection: 3
+    iio_context_string: ip:192.168.0.1

include/imu_ros2/adis1655x/adis1655x_data_access.h

@@ -0,0 +1,113 @@
+#ifndef ADIS1655X_DATA_ACCESS_H
+#define ADIS1655X_DATA_ACCESS_H
+
+// has delta channels
+#define ADIS_HAS_DELTA_BURST
+
+// has calibration scale channel
+#define ADIS_HAS_CALIB_SCALE
+
+#define ADIS_FLS_MEM_ENDURANCE 100000
+#define ADIS_MAX_SAMP_FREQ 4250.0
+
+// value to add to reg addr per page
+#define ADIS_PAGE_ID_VAL 0x80
+
+// global commands
+#define ADIS_GLOB_CMD_PAGE_ID 0x00
+#define ADIS_GLOB_CMD_ADDR_WITHOUT_PAGE 0x50
+#define ADIS_GLOB_CMD_ADDR \
+  (ADIS_PAGE_ID_VAL * ADIS_PT_OF_PERC_PAGE_ID + ADIS_GLOB_CMD_ADDR_WITHOUT_PAGE)
+
+#define ADIS_SENSOR_SELF_TEST_POS 1
+#define ADIS_FLASH_MEMORY_UPDATE_POS 3
+#define ADIS_FACTORY_CALIBRATION_RESTORE_POS 2
+#define ADIS_SOFTWARE_RESET_CMD_POS 5
+
+#define ADIS_SENSOR_SELF_TEST (1 << ADIS_SENSOR_SELF_TEST_POS)
+#define ADIS_FLASH_MEMORY_UPDATE (1 << ADIS_FLASH_MEMORY_UPDATE_POS)
+#define ADIS_FACTORY_CALIBRATION_RESTORE (1 << ADIS_FACTORY_CALIBRATION_RESTORE_POS)
+#define ADIS_SOFTWARE_RESET_CMD (1 << ADIS_SOFTWARE_RESET_CMD_POS)
+
+// status and error flag indication
+#define ADIS_DIAG_STAT_PAGE_ID 0x00
+#define ADIS_DIAG_STAT_ADDR_WITHOUT_PAGE 0x0E
+#define ADIS_DIAG_STAT_ADDR \
+  (ADIS_PAGE_ID_VAL * ADIS_DIAG_STAT_PAGE_ID + ADIS_DIAG_STAT_ADDR_WITHOUT_PAGE)
+
+#define ADIS_MEM_FAIL_POS 0
+#define ADIS_CRC_ERROR_POS 1
+#define ADIS_FLS_MEM_UPDATE_FAIL_POS 2
+#define ADIS_SNSR_FAIL_POS 4
+#define ADIS_SPI_COMM_ERR_POS 6
+#define ADIS_DATA_PATH_OVERRUN_POS 7
+#define ADIS_CLK_ERR_POS 10
+#define ADIS_WDG_TIMER_FLAG_POS 15
+
+#define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS)
+#define ADIS_CRC_ERROR (1 << ADIS_CRC_ERROR_POS)
+#define ADIS_FLS_MEM_UPDATE_FAIL (1 << ADIS_FLS_MEM_UPDATE_FAIL_POS)
+#define ADIS_SNSR_FAIL (1 << ADIS_SNSR_FAIL_POS)
+#define ADIS_SPI_COMM_ERR (1 << ADIS_SPI_COMM_ERR_POS)
+#define ADIS_DATA_PATH_OVERRUN (1 << ADIS_DATA_PATH_OVERRUN_POS)
+#define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS)
+#define ADIS_WDG_TIMER_FLAG (1 << ADIS_WDG_TIMER_FLAG_POS)
+
+// self test error flags
+#define ADIS_DIAG_STS_PAGE_ID 0x00
+#define ADIS_DIAG_STS_REG_WITHOUT_PAGE 0x0F
+#define ADIS_DIAG_STS_REG \
+  (ADIS_PAGE_ID_VAL * ADIS_DIAG_STS_PAGE_ID + ADIS_DIAG_STS_REG_WITHOUT_PAGE)
+
+#define ADIS_GYRO_ACCEL_FAIL_REG ADIS_DIAG_STS_REG
+#define ADIS_GYRO_X_FAIL_POS 0
+#define ADIS_GYRO_Y_FAIL_POS 2
+#define ADIS_GYRO_Z_FAIL_POS 4
+#define ADIS_ACCEL_X_FAIL_POS 6
+#define ADIS_ACCEL_Y_FAIL_POS 8
+#define ADIS_ACCEL_Z_FAIL_POS 10
+
+#define ADIS_GYRO_X_FAIL (3 << ADIS_GYRO_X_FAIL_POS)
+#define ADIS_GYRO_Y_FAIL (3 << ADIS_GYRO_Y_FAIL_POS)
+#define ADIS_GYRO_Z_FAIL (3 << ADIS_GYRO_Z_FAIL_POS)
+#define ADIS_ACCEL_X_FAIL (3 << ADIS_ACCEL_X_FAIL_POS)
+#define ADIS_ACCEL_Y_FAIL (3 << ADIS_ACCEL_Y_FAIL_POS)
+#define ADIS_ACCEL_Z_FAIL (3 << ADIS_ACCEL_Z_FAIL_POS)
+
+// measurement range identifier
+#define ADIS_RANG_MDL_PAGE_ID 0x00
+#define ADIS_RANG_MDL_ADDR_WITHOUT_PAGE 0x10
+#define ADIS_RANG_MDL_ADDR \
+  (ADIS_PAGE_ID_VAL * ADIS_RANG_MDL_PAGE_ID + ADIS_RANG_MDL_ADDR_WITHOUT_PAGE)
+
+// point of percussion
+#define ADIS_PT_OF_PERC_PAGE_ID 0x00
+#define ADIS_PT_OF_PERC_REG_ADDR_WITHOUT_PAGE 0x52
+#define ADIS_PT_OF_PERC_REG_ADDR \
+  (ADIS_PAGE_ID_VAL * ADIS_PT_OF_PERC_PAGE_ID + ADIS_PT_OF_PERC_REG_ADDR_WITHOUT_PAGE)
+#define ADIS_PT_OF_PERC_ALGNMNT_POS 4
+#define ADIS_PT_OF_PERC_ALGNMNT (1 << ADIS_PT_OF_PERC_ALGNMNT_POS)
+
+// continuous bias estimation
+// #define ADIS_NULL_CNFG_PAGE_ID 0x03
+// #define ADIS_NULL_CNFG_ADDR_WITHOUT_PAGE 0x0E
+// #define ADIS_NULL_CNFG_ADDR \
+//   (ADIS_PAGE_ID_VAL * ADIS_NULL_CNFG_PAGE_ID + ADIS_NULL_CNFG_ADDR_WITHOUT_PAGE)
+
+// #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)
+
+#endif

include/imu_ros2/adis1657x/adis1657x_data_access.h

@@ -41,7 +41,6 @@
 
 #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

include/imu_ros2/adis_data_access.h

@@ -48,6 +48,8 @@ enum adis_device_id
   ADIS16547_1,
   ADIS16547_2,
   ADIS16547_3,
+  ADIS16550,
+  ADIS16550W,
   ADIS16575_2,
   ADIS16575_3,
   ADIS16576_2,
@@ -64,6 +66,8 @@ enum adis_device_id
 #include "adis1650x/adis1650x_data_access.h"
 #elif defined(ADIS1654X)
 #include "adis1654x/adis1654x_data_access.h"
+#elif defined(ADIS1655X)
+#include "adis1655x/adis1655x_data_access.h"
 #elif defined(ADIS1657X)
 #include "adis1657x/adis1657x_data_access.h"
 #else

include/imu_ros2/iio_wrapper.h

@@ -394,7 +394,7 @@ class IIOWrapper
    */
   bool update_accel_calibbias_z(int32_t val);
 
-#ifdef ADIS1654X
+#if defined(ADIS1654X) || defined(ADIS1655X)
   /**
    * @brief Get low pass 3db frequency data for x angular velocity.
    * @param result low pass 3db frequency value.

launch/imu_with_madgwick_filter_rviz.launch.py

@@ -19,7 +19,7 @@ def generate_launch_description():
         executable='imu_ros2_node',
         parameters=[{'measured_data_topic_selection': 2},
                     # the IP address of the processing unit to which the IMU is connected to
-                    {'iio_context_string': "ip:0.0.0.0"},],
+                    {'iio_context_string': "ip:10.42.1.83"},],
         remappings=[('/imu','/imu/data_raw')],
         output='screen'
         )

msg/adis1655x/ImuDiagData.msg

@@ -0,0 +1,70 @@
+################################################################################
+#   @file   ImuDiagData.msg
+#   @brief  Definition of ImuDiagData message
+#   @author Robert Budai ([email protected])
+################################################################################
+# 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, sensor automatically reset themselves to clear an issue
+bool diag_automatic_reset
+
+# if true, while operating in scaled sync mode, indicates the sampling time is not scaling correctly
+bool diag_clock_error
+
+# if true, the most recent imu memory flash failed
+bool diag_flash_memory_test_error
+
+# if true, indicates the failure of the inertial sensor
+bool diag_sensor_self_test_error
+
+#if true, indicates communication error on SPI interface
+bool diag_spi_communication_error
+
+# if true, indicates failure on CRC calculation
+bool diag_crc_error
+
+# if true, a failure occurred on x axis gyroscope
+bool diag_x_axis_gyroscope_failure
+
+# if true, a failure occurred on y axis gyroscope
+bool diag_y_axis_gyroscope_failure
+
+# if true, a failure occurred on z axis gyroscope
+bool diag_z_axis_gyroscope_failure
+
+# if true, a failure occurred on x axis accelerometer
+bool diag_x_axis_accelerometer_failure
+
+# if true, a failure occurred on y axis accelerometer
+bool diag_y_axis_accelerometer_failure
+
+# if true, a failure occurred on z axis accelerometer
+bool diag_z_axis_accelerometer_failure
+
+# 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

src/iio_wrapper.cpp

@@ -135,8 +135,8 @@ void IIOWrapper::createContext(const char * context)
     "adis16470",   "adis16475-1", "adis16475-2", "adis16475-3", "adis16477-1", "adis16477-2",
     "adis16477-3", "adis16500",   "adis16501",   "adis16505-1", "adis16505-2", "adis16505-3",
     "adis16507-1", "adis16507-2", "adis16507-3", "adis16545-1", "adis16545-2", "adis16545-3",
-    "adis16547-1", "adis16547-2", "adis16547-3", "adis16575-2", "adis16575-3", "adis16576-2",
-    "adis16576-3", "adis16577-2", "adis16577-3"};
+    "adis16547-1", "adis16547-2", "adis16547-3", "adis16550",   "adis16550w",  "adis16575-2",
+    "adis16575-3", "adis16576-2", "adis16576-3", "adis16577-2", "adis16577-3"};
 
   uint8_t dev_id = 0;
 
@@ -1109,7 +1109,7 @@ bool IIOWrapper::update_sampling_frequency(double val)
   return true;
 }
 
-#ifdef ADIS1654X
+#if defined(ADIS1654X) || defined(ADIS1655X)
 bool IIOWrapper::angvel_x_filter_low_pass_3db(uint32_t & result)
 {
   long long valuel;
@@ -1734,8 +1734,11 @@ bool IIOWrapper::gyroscope_measurement_range(std::string & result)
   int ret = iio_device_reg_read(m_dev, ADIS_RANG_MDL_ADDR, &reg_val);
   if (ret) return false;
 
+#ifdef ADIS1655X
+  result = "+/-300_degrees_per_sec";
+  return true;
+#else
   reg_val = (reg_val & ADIS_GYRO_MEAS_RANG) >> ADIS_GYRO_MEAS_RANG_POS;
-
   switch (reg_val) {
     case 0:
       result = "+/-125_degrees_per_sec";
@@ -1752,7 +1755,8 @@ bool IIOWrapper::gyroscope_measurement_range(std::string & result)
       return true;
     default:
       return false;
-  }
+
+#endif
 }
 
 #ifdef ADIS_SENS_BW