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,9 @@ 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 "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 +82,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_definition(ADIS1655X)
+
 elseif (${DEVICE_FOUND_ADIS16575_2} OR
         ${DEVICE_FOUND_ADIS16575_3} OR
         ${DEVICE_FOUND_ADIS16576_2} OR
@@ -111,6 +120,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.")

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

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

@@ -1734,8 +1734,14 @@ 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;
 
-  reg_val = (reg_val & ADIS_GYRO_MEAS_RANG) >> ADIS_GYRO_MEAS_RANG_POS;
+#ifdef ADIS1655x
+
+  result = "+/-" + itoa((float)(*reg_val) / 80.0 * 300.0) + "_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";
@@ -1753,6 +1759,8 @@ bool IIOWrapper::gyroscope_measurement_range(std::string & result)
     default:
       return false;
   }
+
+#endif
 }
 
 #ifdef ADIS_SENS_BW

test/CMakeLists.txt

@@ -36,6 +36,9 @@ 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 "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)
@@ -69,7 +72,7 @@ elseif (${DEVICE_FOUND_ADIS16465_1} OR
   set(DEVICE_PATH adis1646x)
   add_compile_definitions(ADIS1646X)
 
-  elseif (${DEVICE_FOUND_ADIS16545_1} OR
+elseif (${DEVICE_FOUND_ADIS16545_1} OR
         ${DEVICE_FOUND_ADIS16545_2} OR
         ${DEVICE_FOUND_ADIS16545_3} OR
         ${DEVICE_FOUND_ADIS16547_1} OR
@@ -79,6 +82,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_definition(ADIS1655X)
+
 elseif (${DEVICE_FOUND_ADIS16575_2} OR
         ${DEVICE_FOUND_ADIS16575_3} OR
         ${DEVICE_FOUND_ADIS16576_2} OR
@@ -101,7 +110,6 @@ message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.")
 set(DEVICE_PATH adis1654x)
 add_compile_definitions(ADIS1654X)
 
-
 else()
   message (FATAL_ERROR "
   DEVICE_ID=$ENV{DEVICE_ID} is not a valid value.
@@ -117,6 +125,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.")

test/src/imu_diag_subscriber_test.cpp

@@ -75,7 +75,7 @@ TEST(ImuDiagSubscriberTest, test_imu__diag_data_publisher)
 #ifdef ADIS_FLS_MEM_UPDATE_FAIL
     ASSERT_TRUE(msg.diag_flash_memory_update_error == false);
 #endif
-#ifdef ADIS_SPI_COMM_ERROR
+#ifdef ADIS_SPI_COMM_ERR
     ASSERT_TRUE(msg.diag_spi_communication_error == false);
 #endif
 #ifdef ADIS_CRC_ERROR