From c8fca128e56607b55b7763649068f9c5a2c5747f Mon Sep 17 00:00:00 2001 From: rbudai Date: Wed, 12 Jun 2024 13:47:10 +0300 Subject: [PATCH] Add adis16545-3 support Adding support for adis16545-3 sensor, initial commit Signed-off-by: rbudai --- CMakeLists.txt | 19 + README.rst | 8 + config/adis1654x/imu_config.yaml | 27 ++ .../adis1646x/adis1646x_data_access.h | 11 +- .../adis1647x/adis1647x_data_access.h | 10 +- .../adis1650x/adis1650x_data_access.h | 10 +- .../adis1654x/adis1654x_data_access.h | 94 +++++ .../adis1657x/adis1657x_data_access.h | 26 +- include/imu_ros2/adis_data_access.h | 8 + include/imu_ros2/iio_wrapper.h | 246 ++++++++++- msg/adis1654x/ImuDiagData.msg | 76 ++++ src/iio_wrapper.cpp | 398 ++++++++++++++++-- src/imu_control_parameters.cpp | 184 +++++++- src/imu_diag_data_provider.cpp | 22 + test/CMakeLists.txt | 25 ++ test/src/imu_diag_subscriber_test.cpp | 20 + 16 files changed, 1081 insertions(+), 103 deletions(-) create mode 100644 config/adis1654x/imu_config.yaml create mode 100644 include/imu_ros2/adis1654x/adis1654x_data_access.h create mode 100644 msg/adis1654x/ImuDiagData.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 5dbdaaf..09c297d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,13 @@ 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 "adis16545-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16545_1) +string (COMPARE EQUAL "adis16545-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16545_2) +string (COMPARE EQUAL "adis16545-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16545_3) +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 "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) @@ -62,6 +69,16 @@ elseif (${DEVICE_FOUND_ADIS16465_1} OR set(DEVICE_PATH adis1646x) add_compile_definitions(ADIS1646X) +elseif (${DEVICE_FOUND_ADIS16545_1} OR + ${DEVICE_FOUND_ADIS16545_2} OR + ${DEVICE_FOUND_ADIS16545_3} OR + ${DEVICE_FOUND_ADIS16547_1} OR + ${DEVICE_FOUND_ADIS16547_2} OR + ${DEVICE_FOUND_ADIS16547_3}) +message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") +set(DEVICE_PATH adis1654x) +add_compile_definitions(ADIS1654X) + elseif (${DEVICE_FOUND_ADIS16575_2} OR ${DEVICE_FOUND_ADIS16575_3} OR ${DEVICE_FOUND_ADIS16576_2} OR @@ -92,6 +109,8 @@ else() 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.") diff --git a/README.rst b/README.rst index 998ec9b..a9aa9d4 100644 --- a/README.rst +++ b/README.rst @@ -16,6 +16,8 @@ Supported Devices * `ADIS16501 `_ * `ADIS16505 `_ * `ADIS16507 `_ +* `ADIS16545 `_ +* `ADIS16547 `_ * `ADIS16575 `_ * `ADIS16576 `_ * `ADIS16577 `_ @@ -84,6 +86,8 @@ Evaluation setup - Required hardware with remote client `ADIS16501 `_ `ADIS16505 `_ `ADIS16507 `_ + `ADIS16545 `_ + `ADIS16547 `_ `ADIS16575 `_ `ADIS16576 `_ `ADIS16577 `_ @@ -119,6 +123,8 @@ Evaluation setup - Required hardware with local client `ADIS16500 `_ `ADIS16501 `_ `ADIS16505 `_ + `ADIS16545 `_ + `ADIS16547 `_ `ADIS16507 `_ `ADIS16575 `_ `ADIS16576 `_ @@ -159,6 +165,8 @@ based on the IMU chip: 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. diff --git a/config/adis1654x/imu_config.yaml b/config/adis1654x/imu_config.yaml new file mode 100644 index 0000000..b412556 --- /dev/null +++ b/config/adis1654x/imu_config.yaml @@ -0,0 +1,27 @@ +/imu_ros2_node: + ros__parameters: + command_to_execute: no_command + measured_data_topic_selection: 3 + iio_context_string: ip:192.168.0.1 + 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 + sampling_frequency: 4250.0 + 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 + diff --git a/include/imu_ros2/adis1646x/adis1646x_data_access.h b/include/imu_ros2/adis1646x/adis1646x_data_access.h index 56dd398..73d52bb 100644 --- a/include/imu_ros2/adis1646x/adis1646x_data_access.h +++ b/include/imu_ros2/adis1646x/adis1646x_data_access.h @@ -9,7 +9,7 @@ #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_SNSR_FAIL_POS 5 #define ADIS_MEM_FAIL_POS 6 #define ADIS_CLK_ERR_POS 7 @@ -17,7 +17,7 @@ #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_SNSR_FAIL (1 << ADIS_SNSR_FAIL_POS) #define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS) #define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS) @@ -29,12 +29,11 @@ #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 @@ -62,4 +61,8 @@ #define ADIS_FLASH_MEMORY_TEST (1 << 4) #define ADIS_SOFTWARE_RESET_CMD (1 << 7) +#define ADIS_PT_OF_PERC_REG_ADDR ADIS_MSC_CTRL_ADDR +#define ADIS_PT_OF_PERC_ALGNMNT_POS 6 +#define ADIS_PT_OF_PERC_ALGNMNT (1 << ADIS_PT_OF_PERC_ALGNMNT_POS) + #endif diff --git a/include/imu_ros2/adis1647x/adis1647x_data_access.h b/include/imu_ros2/adis1647x/adis1647x_data_access.h index c022281..97e84ff 100644 --- a/include/imu_ros2/adis1647x/adis1647x_data_access.h +++ b/include/imu_ros2/adis1647x/adis1647x_data_access.h @@ -11,7 +11,7 @@ #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_SNSR_FAIL_POS 5 #define ADIS_MEM_FAIL_POS 6 #define ADIS_CLK_ERR_POS 7 @@ -19,7 +19,7 @@ #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_SNSR_FAIL (1 << ADIS_SNSR_FAIL_POS) #define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS) #define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS) @@ -31,12 +31,10 @@ #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 @@ -64,4 +62,8 @@ #define ADIS_FLASH_MEMORY_TEST (1 << 4) #define ADIS_SOFTWARE_RESET_CMD (1 << 7) +#define ADIS_PT_OF_PERC_REG_ADDR ADIS_MSC_CTRL_ADDR +#define ADIS_PT_OF_PERC_ALGNMNT_POS 6 +#define ADIS_PT_OF_PERC_ALGNMNT (1 << ADIS_PT_OF_PERC_ALGNMNT_POS) + #endif diff --git a/include/imu_ros2/adis1650x/adis1650x_data_access.h b/include/imu_ros2/adis1650x/adis1650x_data_access.h index 06041d3..f9ec2bf 100644 --- a/include/imu_ros2/adis1650x/adis1650x_data_access.h +++ b/include/imu_ros2/adis1650x/adis1650x_data_access.h @@ -11,7 +11,7 @@ #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_SNSR_FAIL_POS 5 #define ADIS_MEM_FAIL_POS 6 #define ADIS_CLK_ERR_POS 7 #define ADIS_GYRO1_FAIL_POS 8 @@ -22,7 +22,7 @@ #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_SNSR_FAIL (1 << ADIS_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) @@ -38,13 +38,11 @@ #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 @@ -54,4 +52,8 @@ #define ADIS_FLASH_MEMORY_TEST (1 << 4) #define ADIS_SOFTWARE_RESET_CMD (1 << 7) +#define ADIS_PT_OF_PERC_REG_ADDR ADIS_MSC_CTRL_ADDR +#define ADIS_PT_OF_PERC_ALGNMNT_POS 6 +#define ADIS_PT_OF_PERC_ALGNMNT (1 << ADIS_PT_OF_PERC_ALGNMNT_POS) + #endif diff --git a/include/imu_ros2/adis1654x/adis1654x_data_access.h b/include/imu_ros2/adis1654x/adis1654x_data_access.h new file mode 100644 index 0000000..4b24a30 --- /dev/null +++ b/include/imu_ros2/adis1654x/adis1654x_data_access.h @@ -0,0 +1,94 @@ +#ifndef ADIS1654X_DATA_ACCESS_H +#define ADIS1654X_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 0x03 +#define ADIS_GLOB_CMD_ADDR_WITHOUT_PAGE 0x02 +#define ADIS_GLOB_CMD_ADDR \ + (ADIS_PAGE_ID_VAL * ADIS_PT_OF_PERC_PAGE_ID + ADIS_GLOB_CMD_ADDR_WITHOUT_PAGE) + +#define ADIS_BIAS_CORRECTION_UPDATE_POS 0 +#define ADIS_SENSOR_SELF_TEST_POS 1 +#define ADIS_FLASH_MEMORY_UPDATE_POS 3 +#define ADIS_FACTORY_CALIBRATION_RESTORE_POS 6 +#define ADIS_SOFTWARE_RESET_CMD_POS 7 + +#define ADIS_BIAS_CORRECTION_UPDATE (1 << ADIS_BIAS_CORRECTION_UPDATE_POS) +#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 0x08 +#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 1 +#define ADIS_CRC_ERROR_POS 2 +#define ADIS_SPI_COMM_ERR_POS 3 +#define ADIS_SNSR_FAIL_POS 5 +#define ADIS_FLS_MEM_UPDATE_FAIL_POS 6 +#define ADIS_DATA_PATH_OVERRUN_POS 7 +#define ADIS_CLK_ERR_POS 8 +#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_SPI_COMM_ERR (1 << ADIS_SPI_COMM_ERR_POS) +#define ADIS_SNSR_FAIL (1 << ADIS_SNSR_FAIL_POS) +#define ADIS_FLS_MEM_UPDATE_FAIL (1 << ADIS_FLS_MEM_UPDATE_FAIL_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 0x0A +#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 1 +#define ADIS_GYRO_Z_FAIL_POS 2 +#define ADIS_ACCEL_X_FAIL_POS 3 +#define ADIS_ACCEL_Y_FAIL_POS 4 +#define ADIS_ACCEL_Z_FAIL_POS 5 + +#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) + +// measurement range identifier +#define ADIS_RANG_MDL_PAGE_ID 0x03 +#define ADIS_RANG_MDL_ADDR_WITHOUT_PAGE 0x12 +#define ADIS_RANG_MDL_ADDR \ + (ADIS_PAGE_ID_VAL * ADIS_RANG_MDL_PAGE_ID + ADIS_RANG_MDL_ADDR_WITHOUT_PAGE) +#define ADIS_GYRO_MEAS_RANG_POS 2 +#define ADIS_GYRO_MEAS_RANG (3 << ADIS_GYRO_MEAS_RANG_POS) + +// point of percussion +#define ADIS_PT_OF_PERC_PAGE_ID 0x03 +#define ADIS_PT_OF_PERC_REG_ADDR_WITHOUT_PAGE 0x0A +#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 6 +#define ADIS_PT_OF_PERC_ALGNMNT (1 << ADIS_PT_OF_PERC_ALGNMNT_POS) + +#endif diff --git a/include/imu_ros2/adis1657x/adis1657x_data_access.h b/include/imu_ros2/adis1657x/adis1657x_data_access.h index 2716dde..be6e7b9 100644 --- a/include/imu_ros2/adis1657x/adis1657x_data_access.h +++ b/include/imu_ros2/adis1657x/adis1657x_data_access.h @@ -12,9 +12,18 @@ #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_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 ADIS_SNSR_FAIL (1 << ADIS_SNSR_FAIL_POS) +#define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS) +#define ADIS_ADUC_MCU_FAULT (1 << ADIS_ADUC_MCU_FAULT_POS) + +#define ADIS_GYRO_ACCEL_FAIL_REG ADIS_DIAG_STAT_ADDR #define ADIS_GYRO_X_FAIL_POS 8 #define ADIS_GYRO_Y_FAIL_POS 9 #define ADIS_GYRO_Z_FAIL_POS 10 @@ -22,13 +31,6 @@ #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) @@ -36,7 +38,6 @@ #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 @@ -46,13 +47,11 @@ #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) @@ -81,4 +80,9 @@ #define ADIS_FLASH_MEMORY_TEST (1 << 4) #define ADIS_SOFTWARE_RESET_CMD (1 << 7) +// Point of percussion +#define ADIS_PT_OF_PERC_REG_ADDR ADIS_MSC_CTRL_ADDR +#define ADIS_PT_OF_PERC_ALGNMNT_POS 6 +#define ADIS_PT_OF_PERC_ALGNMNT (1 << ADIS_PT_OF_PERC_ALGNMNT_POS) + #endif diff --git a/include/imu_ros2/adis_data_access.h b/include/imu_ros2/adis_data_access.h index e9fab4d..abc7fb0 100644 --- a/include/imu_ros2/adis_data_access.h +++ b/include/imu_ros2/adis_data_access.h @@ -42,6 +42,12 @@ enum adis_device_id ADIS16507_1, ADIS16507_2, ADIS16507_3, + ADIS16545_1, + ADIS16545_2, + ADIS16545_3, + ADIS16547_1, + ADIS16547_2, + ADIS16547_3, ADIS16575_2, ADIS16575_3, ADIS16576_2, @@ -56,6 +62,8 @@ enum adis_device_id #include "adis1647x/adis1647x_data_access.h" #elif defined(ADIS1650X) #include "adis1650x/adis1650x_data_access.h" +#elif defined(ADIS1654X) +#include "adis1654x/adis1654x_data_access.h" #elif defined(ADIS1657X) #include "adis1657x/adis1657x_data_access.h" #else diff --git a/include/imu_ros2/iio_wrapper.h b/include/imu_ros2/iio_wrapper.h index cf4f70a..dcf7e8c 100644 --- a/include/imu_ros2/iio_wrapper.h +++ b/include/imu_ros2/iio_wrapper.h @@ -52,8 +52,8 @@ class IIOWrapper void createContext(const char * context); /** - * @brief Update buffer data. This function should be called before retrieving - * data using getBuff** APIs. + * @brief Update buffer data. This function should be called before + * retrieving data using getBuff** APIs. * @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, @@ -174,7 +174,8 @@ 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. + * represent the time at which the samples from the devices were read over + * SPI. * @param sec The second component of the timestamp * @param nanosec The nanosecond component of the timestamp */ @@ -393,6 +394,204 @@ class IIOWrapper */ bool update_accel_calibbias_z(int32_t val); +#ifdef ADIS1654X + /** + * @brief Get low pass 3db frequency data for x angular velocity. + * @param result low pass 3db frequency value. + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool angvel_x_filter_low_pass_3db(uint32_t & result); + /** + * @brief Update low pass 3db frequency for x angular velocity. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_angvel_x_filter_low_pass_3db(uint32_t val); + + /** + * @brief Get low pass 3db frequency data for y angular velocity. + * @param result low pass 3db frequency value. + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool angvel_y_filter_low_pass_3db(uint32_t & result); + /** + * @brief Update low pass 3db frequency for y angular velocity. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_angvel_y_filter_low_pass_3db(uint32_t val); + + /** + * @brief Get low pass 3db frequency data for z angular velocity. + * @param result low pass 3db frequency value. + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool angvel_z_filter_low_pass_3db(uint32_t & result); + /** + * @brief Update low pass 3db frequency for z angular velocity. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_angvel_z_filter_low_pass_3db(uint32_t val); + + /** + * @brief Get low pass 3db frequency data for x acceleration. + * @param result low pass 3db frequency value. + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool accel_x_filter_low_pass_3db(uint32_t & result); + /** + * @brief Update low pass 3db frequency for x acceleration. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_accel_x_filter_low_pass_3db(uint32_t val); + + /** + * @brief Get low pass 3db frequency data for y acceleration. + * @param result low pass 3db frequency value. + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool accel_y_filter_low_pass_3db(uint32_t & result); + /** + * @brief Update low pass 3db frequency for y acceleration. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_accel_y_filter_low_pass_3db(uint32_t val); + + /** + * @brief Get low pass 3db frequency data for z acceleration. + * @param result low pass 3db frequency value. + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool accel_z_filter_low_pass_3db(uint32_t & result); + /** + * @brief Update low pass 3db frequency for z acceleration. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_accel_z_filter_low_pass_3db(uint32_t val); + +#else + /** + * @brief Get low pass 3db frequency data. + * @param result low pass 3db frequency value. + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool filter_low_pass_3db_frequency(uint32_t & result); + + /** + * @brief Update low pass 3db frequency. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_filter_low_pass_3db_frequency(uint32_t val); +#endif + +#ifdef ADIS_HAS_CALIB_SCALE + /** + * @brief Get linear acceleration calibration scale on x axis. + * @param result linear acceleration calibration scale on x axis. + * @return Return true if reading was with success and + * false if not. + */ + bool accel_x_calibscale(int32_t & result); + /** + * @brief Get linear acceleration calibration scale on y axis. + * @param result linear acceleration calibration scale on y axis. + * @return Return true if reading was with success and + * false if not. + */ + bool accel_y_calibscale(int32_t & result); + /** + * @brief Get linear acceleration calibration scale on z axis. + * @param result linear acceleration calibration scale on z axis. + * @return Return true if reading was with success and + * false if not. + */ + bool accel_z_calibscale(int32_t & result); + /** + * @brief Get linear angular velocity calibration scale on x axis. + * @param result linear angular velocity calibration scale on x axis. + * @return Return true if reading was with success and + * false if not. + */ + bool anglvel_x_calibscale(int32_t & result); + /** + * @brief Get linear angular velocity calibration scale on y axis. + * @param result linear angular velocity calibration scale on y axis. + * @return Return true if reading was with success and + * false if not. + */ + bool anglvel_y_calibscale(int32_t & result); + /** + * @brief Get linear angular velocity calibration scale on z axis. + * @param result linear angular velocity calibration scale on z axis. + * @return Return true if reading was with success and + * false if not. + */ + bool anglvel_z_calibscale(int32_t & result); + + /** + * @brief Update linear acceleration calibration scale on x axis. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_accel_calibscale_x(int32_t val); + /** + * @brief Update linear acceleration calibration scale on y axis. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_accel_calibscale_y(int32_t val); + /** + * @brief Update linear acceleration calibration scale on y axis. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_accel_calibscale_z(int32_t val); + /** + * @brief Update angular velocity calibration scale on x axis. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_anglvel_calibscale_x(int32_t val); + /** + * @brief Update angular velocity calibration scale on y axis. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_anglvel_calibscale_y(int32_t val); + /** + * @brief Update angular velocity calibration scale on z axis. + * @param val value to update with. + * @return Return true if writing was with success and + * false if not. + */ + bool update_anglvel_calibscale_z(int32_t val); + +#endif + /** * @brief Get sampling frequency. * @param result current set sampling frequency @@ -425,6 +624,14 @@ class IIOWrapper */ bool diag_data_path_overrun(bool & result); + /** + * @brief Get automatic reset data + * @param result True if device reset has occured + * @return true Return true if reading was successful and data is valid, + * false otherwise. + */ + bool diag_automatic_reset(bool & result); + /** * @brief Get diag flash memory update_error data. * @param result True if failure occurred, false otherwise. @@ -441,6 +648,14 @@ class IIOWrapper */ bool diag_spi_communication_error(bool & result); + /** + * @brief Get diag crc error. + * @param result True if calculation failure occurred in a CRC + * @return Return true if reading was successful and data is valid, false + * otherwise. + */ + bool diag_crc_error(bool & result); + /** * @brief Get diag standby mode data. * @param result True if device is in standby mode, false otherwise. @@ -561,22 +776,6 @@ class IIOWrapper */ bool diag_flash_memory_write_count_exceeded_error(bool & result); - /** - * @brief Get low pass 3db frequency data. - * @param result low pass 3db frequency value. - * @return Return true if reading was successful and data is valid, false - * otherwise. - */ - bool filter_low_pass_3db_frequency(uint32_t & result); - - /** - * @brief Update low pass 3db frequency. - * @param val value to update with. - * @return Return true if writing was with success and - * false if not. - */ - bool update_filter_low_pass_3db_frequency(uint32_t val); - /** * @brief Get gyroscope measurement range data. * @param result gyroscope measurement range data. @@ -1039,13 +1238,16 @@ class IIOWrapper /*! This variable retains the timestamp channel */ static struct iio_channel * m_channel_timestamp; - /*! This variable retains the scale for the linear acceleration x raw value */ + /*! This variable retains the scale for the linear acceleration x raw value + */ static double m_scale_accel_x; - /*! This variable retains the scale for the linear acceleration y raw value */ + /*! This variable retains the scale for the linear acceleration y raw value + */ static double m_scale_accel_y; - /*! This variable retains the scale for the linear acceleration z raw value */ + /*! This variable retains the scale for the linear acceleration z raw value + */ static double m_scale_accel_z; /*! This variable retains the scale for the angular velocity x raw value */ diff --git a/msg/adis1654x/ImuDiagData.msg b/msg/adis1654x/ImuDiagData.msg new file mode 100644 index 0000000..6671954 --- /dev/null +++ b/msg/adis1654x/ImuDiagData.msg @@ -0,0 +1,76 @@ +################################################################################ +# @file ImuDiagData.msg +# @brief Definition of ImuDiagData message +# @author Robert Budai (robert.budai@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, 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, indicates the occurrence of a processing overrun +bool diag_processing_overrun + +# 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, indicates the device booted up using the backup memory bank +bool diag_boot_mem_fail + +# 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 diff --git a/src/iio_wrapper.cpp b/src/iio_wrapper.cpp index 78c38bf..16e6e45 100644 --- a/src/iio_wrapper.cpp +++ b/src/iio_wrapper.cpp @@ -134,8 +134,8 @@ void IIOWrapper::createContext(const char * context) "adis16465-1", "adis16465-2", "adis16465-3", "adis16467-1", "adis16467-2", "adis16467-3", "adis16470", "adis16475-1", "adis16475-2", "adis16475-3", "adis16477-1", "adis16477-2", "adis16477-3", "adis16500", "adis16501", "adis16505-1", "adis16505-2", "adis16505-3", - "adis16507-1", "adis16507-2", "adis16507-3", "adis16575-2", "adis16575-3", "adis16576-2", - "adis16576-3", "adis16577-2", "adis16577-3"}; + "adis16507-1", "adis16507-2", "adis16507-3", "adis16545-3", "adis16575-2", "adis16575-3", + "adis16576-2", "adis16576-3", "adis16577-2", "adis16577-3"}; uint8_t dev_id = 0; @@ -146,7 +146,8 @@ void IIOWrapper::createContext(const char * context) if (m_dev_trigger == nullptr) { const std::string devid = iio_device_get_id(m_dev); - /* Get device number in the form of 0, 1, etc. to append to the trigger name. */ + /* Get device number in the form of 0, 1, etc. to append to the + * trigger name. */ std::string devnb; for (long unsigned int i = 0; i < devid.std::string::length(); i++) if (isdigit(devid.c_str()[i])) devnb.std::string::append(&devid.c_str()[i]); @@ -244,7 +245,8 @@ void IIOWrapper::createContext(const char * context) iio_channel_attr_read_double(m_channel_deltavelocity_y, "scale", &m_scale_deltavelocity_y); iio_channel_attr_read_double(m_channel_deltavelocity_z, "scale", &m_scale_deltavelocity_z); } else { - /* Set scale manually in case delta channels are not available in the linux driver. */ + /* Set scale manually in case delta channels are not available in the + * linux driver. */ setDeltaAngleScales((enum adis_device_id)dev_id); setDeltaVelocityScales((enum adis_device_id)dev_id); } @@ -299,6 +301,12 @@ void IIOWrapper::setDeltaAngleScales(enum adis_device_id id) case ADIS16500: case ADIS16505_3: case ADIS16507_3: + case ADIS16545_1: + case ADIS16545_2: + case ADIS16545_3: + case ADIS16547_1: + case ADIS16547_2: + case ADIS16547_3: m_scale_deltaangl_x = 0.000000017; m_scale_deltaangl_y = 0.000000017; m_scale_deltaangl_z = 0.000000017; @@ -349,6 +357,12 @@ void IIOWrapper::setDeltaVelocityScales(enum adis_device_id id) case ADIS16505_1: case ADIS16505_2: case ADIS16505_3: + case ADIS16545_1: + case ADIS16545_2: + case ADIS16545_3: + case ADIS16547_1: + case ADIS16547_2: + case ADIS16547_3: case ADIS16575_2: case ADIS16575_3: case ADIS16576_2: @@ -385,7 +399,8 @@ static ssize_t demux_sample( iio_channel_convert(chn, &val, sample); buff_data[iio_channel_get_index(chn)][buff_write_idx] = val; if (!has_timestamp_channel) { - /* timestamp channel is not available, have to update buff_write_idx */ + /* timestamp channel is not available, have to update buff_write_idx + */ buff_write_idx++; } } else if (size == 4) { @@ -393,7 +408,8 @@ static ssize_t demux_sample( iio_channel_convert(chn, &val, sample); buff_data[iio_channel_get_index(chn)][buff_write_idx] = val; if (!has_timestamp_channel) { -/* timestamp channel is not available, have to update buff_write_idx for last read channel */ +/* timestamp channel is not available, have to update buff_write_idx for last + * read channel */ #ifdef ADIS_HAS_DELTA_BURST if (iio_channel_get_index(chn) == CHAN_DELTA_VEL_Z) buff_write_idx++; #endif @@ -461,7 +477,8 @@ bool IIOWrapper::updateBuffer(uint32_t data_selection) sampling_frequency(&samp_freq); no_of_samp = MAX_NO_OF_SAMPLES; if (no_of_samp > samp_freq) - /* Overwrite number of samples based on sampling frequency, to avoid timeout errors from LibIIO */ + /* 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); @@ -1091,6 +1108,271 @@ bool IIOWrapper::update_sampling_frequency(double val) return true; } +#ifdef ADIS1654X +bool IIOWrapper::angvel_x_filter_low_pass_3db(uint32_t & result) +{ + long long valuel; + + if (!m_channel_anglvel_x) return false; + + int ret = + iio_channel_attr_read_longlong(m_channel_anglvel_x, "filter_low_pass_3db_frequency", &valuel); + + result = valuel; + return (ret == 0); +} +bool IIOWrapper::update_angvel_x_filter_low_pass_3db(uint32_t val) +{ + if (!m_channel_anglvel_x) return false; + + return ( + iio_channel_attr_write_longlong(m_channel_anglvel_x, "filter_low_pass_3db_frequency", val) == + 0); +} + +bool IIOWrapper::angvel_y_filter_low_pass_3db(uint32_t & result) +{ + long long valuel; + + if (!m_channel_anglvel_y) return false; + + int ret = + iio_channel_attr_read_longlong(m_channel_anglvel_y, "filter_low_pass_3db_frequency", &valuel); + + result = valuel; + return (ret == 0); +} +bool IIOWrapper::update_angvel_y_filter_low_pass_3db(uint32_t val) +{ + if (!m_channel_anglvel_y) return false; + + return ( + iio_channel_attr_write_longlong(m_channel_anglvel_y, "filter_low_pass_3db_frequency", val) == + 0); +} + +bool IIOWrapper::angvel_z_filter_low_pass_3db(uint32_t & result) +{ + long long valuel; + + if (!m_channel_anglvel_z) return false; + + int ret = + iio_channel_attr_read_longlong(m_channel_anglvel_z, "filter_low_pass_3db_frequency", &valuel); + + result = valuel; + return (ret == 0); +} +bool IIOWrapper::update_angvel_z_filter_low_pass_3db(uint32_t val) +{ + if (!m_channel_anglvel_z) return false; + + return ( + iio_channel_attr_write_longlong(m_channel_anglvel_z, "filter_low_pass_3db_frequency", val) == + 0); +} + +bool IIOWrapper::accel_x_filter_low_pass_3db(uint32_t & result) +{ + long long valuel; + + if (!m_channel_accel_x) return false; + + int ret = + iio_channel_attr_read_longlong(m_channel_accel_x, "filter_low_pass_3db_frequency", &valuel); + + result = valuel; + return (ret == 0); +} +bool IIOWrapper::update_accel_x_filter_low_pass_3db(uint32_t val) +{ + if (!m_channel_accel_x) return false; + + return ( + iio_channel_attr_write_longlong(m_channel_accel_x, "filter_low_pass_3db_frequency", val) == 0); +} + +bool IIOWrapper::accel_y_filter_low_pass_3db(uint32_t & result) +{ + long long valuel; + + if (!m_channel_accel_y) return false; + + int ret = + iio_channel_attr_read_longlong(m_channel_accel_y, "filter_low_pass_3db_frequency", &valuel); + + result = valuel; + return (ret == 0); +} +bool IIOWrapper::update_accel_y_filter_low_pass_3db(uint32_t val) +{ + if (!m_channel_accel_y) return false; + + return ( + iio_channel_attr_write_longlong(m_channel_accel_y, "filter_low_pass_3db_frequency", val) == 0); +} + +bool IIOWrapper::accel_z_filter_low_pass_3db(uint32_t & result) +{ + long long valuel; + + if (!m_channel_accel_z) return false; + + int ret = + iio_channel_attr_read_longlong(m_channel_accel_z, "filter_low_pass_3db_frequency", &valuel); + + result = valuel; + return (ret == 0); +} +bool IIOWrapper::update_accel_z_filter_low_pass_3db(uint32_t val) +{ + if (!m_channel_accel_z) return false; + + return ( + iio_channel_attr_write_longlong(m_channel_accel_z, "filter_low_pass_3db_frequency", val) == 0); +} +#else + +bool IIOWrapper::filter_low_pass_3db_frequency(uint32_t & result) +{ + long long valuel; + + if (!m_dev) return false; + + int ret = iio_device_attr_read_longlong(m_dev, "filter_low_pass_3db_frequency", &valuel); + if (ret) return false; + + result = valuel; + return true; +} + +bool IIOWrapper::update_filter_low_pass_3db_frequency(uint32_t val) +{ + if (!m_dev) return false; + + return (iio_device_attr_write_longlong(m_dev, "filter_low_pass_3db_frequency", val) == 0); +} + +#endif + +#ifdef ADIS_HAS_CALIB_SCALE + +bool IIOWrapper::accel_x_calibscale(int32_t & result) +{ + long long valuel; + + if (!m_channel_accel_x) return false; + + int ret = iio_channel_attr_read_longlong(m_channel_accel_x, "calibscale", &valuel); + + result = valuel; + return (ret == 0); +} + +bool IIOWrapper::accel_y_calibscale(int32_t & result) +{ + long long valuel; + + if (!m_channel_accel_y) return false; + + int ret = iio_channel_attr_read_longlong(m_channel_accel_y, "calibscale", &valuel); + + result = valuel; + return (ret == 0); +} + +bool IIOWrapper::accel_z_calibscale(int32_t & result) +{ + long long valuel; + + if (!m_channel_accel_z) return false; + + int ret = iio_channel_attr_read_longlong(m_channel_accel_z, "calibscale", &valuel); + + result = valuel; + return (ret == 0); +} + +bool IIOWrapper::anglvel_x_calibscale(int32_t & result) +{ + long long valuel; + + if (!m_channel_anglvel_x) return false; + + int ret = iio_channel_attr_read_longlong(m_channel_anglvel_x, "calibscale", &valuel); + + result = valuel; + return (ret == 0); +} + +bool IIOWrapper::anglvel_y_calibscale(int32_t & result) +{ + long long valuel; + + if (!m_channel_anglvel_y) return false; + + int ret = iio_channel_attr_read_longlong(m_channel_anglvel_y, "calibscale", &valuel); + + result = valuel; + return (ret == 0); +} + +bool IIOWrapper::anglvel_z_calibscale(int32_t & result) +{ + long long valuel; + + if (!m_channel_anglvel_z) return false; + + int ret = iio_channel_attr_read_longlong(m_channel_anglvel_z, "calibscale", &valuel); + + result = valuel; + return (ret == 0); +} + +bool IIOWrapper::update_accel_calibscale_x(int32_t val) +{ + if (!m_channel_accel_x) return false; + + return (iio_channel_attr_write_longlong(m_channel_accel_x, "calibscale", val) == 0); +} + +bool IIOWrapper::update_accel_calibscale_y(int32_t val) +{ + if (!m_channel_accel_y) return false; + + return (iio_channel_attr_write_longlong(m_channel_accel_y, "calibscale", val) == 0); +} + +bool IIOWrapper::update_accel_calibscale_z(int32_t val) +{ + if (!m_channel_accel_z) return false; + + return (iio_channel_attr_write_longlong(m_channel_accel_z, "calibscale", val) == 0); +} + +bool IIOWrapper::update_anglvel_calibscale_x(int32_t val) +{ + if (!m_channel_anglvel_x) return false; + + return (iio_channel_attr_write_longlong(m_channel_anglvel_x, "calibscale", val) == 0); +} + +bool IIOWrapper::update_anglvel_calibscale_y(int32_t val) +{ + if (!m_channel_anglvel_y) return false; + + return (iio_channel_attr_write_longlong(m_channel_anglvel_y, "calibscale", val) == 0); +} + +bool IIOWrapper::update_anglvel_calibscale_z(int32_t val) +{ + if (!m_channel_anglvel_z) return false; + + return (iio_channel_attr_write_longlong(m_channel_anglvel_z, "calibscale", val) == 0); +} + +#endif + #ifdef ADIS_SNSR_INIT_FAIL bool IIOWrapper::diag_sensor_initialization_failure(bool & result) { @@ -1109,6 +1391,7 @@ bool IIOWrapper::diag_sensor_initialization_failure(bool & result) } #endif +#ifdef ADIS_DATA_PATH_OVERRUN bool IIOWrapper::diag_data_path_overrun(bool & result) { uint32_t reg_val; @@ -1123,7 +1406,26 @@ bool IIOWrapper::diag_data_path_overrun(bool & result) return true; } +#endif +#ifdef ADIS_WDG_TIMER_FLAG +bool IIOWrapper::diag_automatic_reset(bool & result) +{ + uint32_t reg_val; + + if (!m_dev) return false; + + int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + if (ret) return false; + + reg_val = (reg_val & ADIS_WDG_TIMER_FLAG) >> ADIS_WDG_TIMER_FLAG_POS; + result = reg_val; + + return true; +} +#endif + +#ifdef ADIS_FLS_MEM_UPDATE_FAIL bool IIOWrapper::diag_flash_memory_update_error(bool & result) { uint32_t reg_val; @@ -1138,7 +1440,9 @@ bool IIOWrapper::diag_flash_memory_update_error(bool & result) return true; } +#endif +#ifdef ADIS_SPI_COMM_ERR bool IIOWrapper::diag_spi_communication_error(bool & result) { uint32_t reg_val; @@ -1153,7 +1457,26 @@ bool IIOWrapper::diag_spi_communication_error(bool & result) return true; } +#endif +#ifdef ADIS_CRC_ERROR_POS +bool IIOWrapper::diag_crc_error(bool & result) +{ + uint32_t reg_val; + + if (!m_dev) return false; + + int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + if (ret) return false; + + reg_val = (reg_val & ADIS_CRC_ERROR) >> ADIS_CRC_ERROR_POS; + result = reg_val; + + return true; +} +#endif + +#ifdef ADIS_STDBY_MODE bool IIOWrapper::diag_standby_mode(bool & result) { uint32_t reg_val; @@ -1168,7 +1491,9 @@ bool IIOWrapper::diag_standby_mode(bool & result) return true; } +#endif +#ifdef ADIS_SNSR_FAIL bool IIOWrapper::diag_sensor_self_test_error(bool & result) { uint32_t reg_val; @@ -1178,12 +1503,14 @@ bool IIOWrapper::diag_sensor_self_test_error(bool & result) 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; + reg_val = (reg_val & ADIS_SNSR_FAIL) >> ADIS_SNSR_FAIL_POS; result = reg_val; return true; } +#endif +#ifdef ADIS_MEM_FAIL bool IIOWrapper::diag_flash_memory_test_error(bool & result) { uint32_t reg_val; @@ -1198,7 +1525,9 @@ bool IIOWrapper::diag_flash_memory_test_error(bool & result) return true; } +#endif +#ifdef ADIS_CLK_ERR bool IIOWrapper::diag_clock_error(bool & result) { uint32_t reg_val; @@ -1213,6 +1542,7 @@ bool IIOWrapper::diag_clock_error(bool & result) return true; } +#endif #ifdef ADIS_GYRO1_FAIL bool IIOWrapper::diag_gyroscope1_self_test_error(bool & result) @@ -1272,7 +1602,7 @@ bool IIOWrapper::diag_x_axis_gyroscope_failure(bool & result) if (!m_dev) return false; - int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO_X_FAIL) >> ADIS_GYRO_X_FAIL_POS; @@ -1289,7 +1619,7 @@ bool IIOWrapper::diag_y_axis_gyroscope_failure(bool & result) if (!m_dev) return false; - int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO_Y_FAIL) >> ADIS_GYRO_Y_FAIL_POS; @@ -1306,7 +1636,7 @@ bool IIOWrapper::diag_z_axis_gyroscope_failure(bool & result) if (!m_dev) return false; - int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_GYRO_Z_FAIL) >> ADIS_GYRO_Z_FAIL_POS; @@ -1323,7 +1653,7 @@ bool IIOWrapper::diag_x_axis_accelerometer_failure(bool & result) if (!m_dev) return false; - int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ACCEL_X_FAIL) >> ADIS_ACCEL_X_FAIL_POS; @@ -1340,7 +1670,7 @@ bool IIOWrapper::diag_y_axis_accelerometer_failure(bool & result) if (!m_dev) return false; - int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ACCEL_Y_FAIL) >> ADIS_ACCEL_Y_FAIL_POS; @@ -1357,7 +1687,7 @@ bool IIOWrapper::diag_z_axis_accelerometer_failure(bool & result) if (!m_dev) return false; - int ret = iio_device_reg_read(m_dev, ADIS_DIAG_STAT_ADDR, ®_val); + int ret = iio_device_reg_read(m_dev, ADIS_GYRO_ACCEL_FAIL_REG, ®_val); if (ret) return false; reg_val = (reg_val & ADIS_ACCEL_Z_FAIL) >> ADIS_ACCEL_Z_FAIL_POS; @@ -1394,26 +1724,6 @@ bool IIOWrapper::diag_flash_memory_write_count_exceeded_error(bool & result) 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 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) { uint32_t reg_val; @@ -1430,7 +1740,11 @@ bool IIOWrapper::gyroscope_measurement_range(std::string & result) result = "+/-125_degrees_per_sec"; return true; case 1: +#ifdef ADIS1654X + result = "+/-450_degrees_per_sec"; +#else result = "+/-500_degrees_per_sec"; +#endif return true; case 3: result = "+/-2000_degrees_per_sec"; @@ -1458,23 +1772,28 @@ bool IIOWrapper::update_internal_sensor_bandwidth(uint32_t val) } #endif +#ifdef ADIS_PT_OF_PERC_REG_ADDR bool IIOWrapper::point_of_percussion_alignment(uint32_t & result) { if (!m_dev) return false; - int ret = iio_device_reg_read(m_dev, ADIS_MSC_CTRL_ADDR, &result); + int ret = iio_device_reg_read(m_dev, ADIS_PT_OF_PERC_REG_ADDR, &result); if (ret) return false; - result = (result & ADIS_PT_OF_PERG_ALGNMNT) >> ADIS_PT_OF_PERG_ALGNMNT_POS; + result = (result & ADIS_PT_OF_PERC_ALGNMNT) >> ADIS_PT_OF_PERC_ALGNMNT_POS; return true; } +#endif +#ifdef ADIS_PT_OF_PERC_REG_ADDR bool IIOWrapper::update_point_of_percussion_alignment(uint32_t val) { return updateField( - ADIS_MSC_CTRL_ADDR, val << ADIS_PT_OF_PERG_ALGNMNT_POS, ADIS_PT_OF_PERG_ALGNMNT); + ADIS_PT_OF_PERC_REG_ADDR, val << ADIS_PT_OF_PERC_ALGNMNT_POS, ADIS_PT_OF_PERC_ALGNMNT); } +#endif +#ifdef ADIS_MSC_CTRL_ADDR bool IIOWrapper::linear_acceleration_compensation(uint32_t & result) { if (!m_dev) return false; @@ -1485,11 +1804,14 @@ bool IIOWrapper::linear_acceleration_compensation(uint32_t & result) result = (result & ADIS_LN_ACCL_COMP) >> ADIS_LN_ACCL_COMP_POS; return true; } +#endif +#ifdef ADIS_MSC_CTRL_ADDR bool IIOWrapper::update_linear_acceleration_compensation(uint32_t val) { return updateField(ADIS_MSC_CTRL_ADDR, val << ADIS_LN_ACCL_COMP_POS, ADIS_LN_ACCL_COMP); } +#endif #ifdef ADIS_NULL_CNFG_ADDR bool IIOWrapper::bias_correction_time_base_control(uint32_t & result) @@ -1646,6 +1968,7 @@ bool IIOWrapper::flash_memory_update() return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } +#ifdef ADIS_FLASH_MEMORY_TEST bool IIOWrapper::flash_memory_test() { if (!m_dev) return false; @@ -1653,6 +1976,7 @@ bool IIOWrapper::flash_memory_test() uint16_t cmd = ADIS_FLASH_MEMORY_TEST; return (iio_device_reg_write(m_dev, ADIS_GLOB_CMD_ADDR, cmd) == 0); } +#endif bool IIOWrapper::software_reset() { diff --git a/src/imu_control_parameters.cpp b/src/imu_control_parameters.cpp index f5b141a..5c60d4e 100644 --- a/src/imu_control_parameters.cpp +++ b/src/imu_control_parameters.cpp @@ -52,13 +52,26 @@ void ImuControlParameters::declareAdisAttributes() m_attr_current_device.push_back("accel_calibbias_y"); m_attr_current_device.push_back("accel_calibbias_z"); +#ifdef ADIS_HAS_CALIB_SCALE + m_attr_current_device.push_back("anglvel_calibscale_x"); + m_attr_current_device.push_back("anglvel_calibscale_y"); + m_attr_current_device.push_back("anglvel_calibscale_z"); + m_attr_current_device.push_back("accel_calibscale_x"); + m_attr_current_device.push_back("accel_calibscale_y"); + m_attr_current_device.push_back("accel_calibscale_z"); +#endif +#ifndef ADIS1654X m_attr_current_device.push_back("filter_low_pass_3db_frequency"); +#endif #ifdef ADIS_SENS_BW m_attr_current_device.push_back("internal_sensor_bandwidth"); #endif +#ifdef ADIS_PT_OF_PERC_REG_ADDR m_attr_current_device.push_back("point_of_percussion_alignment"); +#endif +#ifdef ADIS_MSC_CTRL_ADDR m_attr_current_device.push_back("linear_acceleration_compensation"); - +#endif #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"); @@ -68,7 +81,14 @@ void ImuControlParameters::declareAdisAttributes() 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 - +#ifdef ADIS1654X + m_attr_current_device.push_back("angvel_x_filter_low_pass_3db"); + m_attr_current_device.push_back("angvel_y_filter_low_pass_3db"); + m_attr_current_device.push_back("angvel_z_filter_low_pass_3db"); + m_attr_current_device.push_back("accel_x_filter_low_pass_3db"); + m_attr_current_device.push_back("accel_y_filter_low_pass_3db"); + m_attr_current_device.push_back("accel_z_filter_low_pass_3db"); +#endif m_attr_current_device.push_back("sampling_frequency"); } @@ -80,6 +100,29 @@ void ImuControlParameters::mapIIOUpdateFunctionsInt32() m_func_map_update_int32_params["anglvel_calibbias_x"] = &IIOWrapper::update_anglvel_calibbias_x; m_func_map_update_int32_params["anglvel_calibbias_y"] = &IIOWrapper::update_anglvel_calibbias_y; m_func_map_update_int32_params["anglvel_calibbias_z"] = &IIOWrapper::update_anglvel_calibbias_z; + +#ifdef ADIS_HAS_CALIB_SCALE + m_func_map_update_int32_params["accel_calibscale_x"] = &IIOWrapper::update_accel_calibscale_x; + m_func_map_update_int32_params["accel_calibscale_y"] = &IIOWrapper::update_accel_calibscale_y; + m_func_map_update_int32_params["accel_calibscale_z"] = &IIOWrapper::update_accel_calibscale_z; + m_func_map_update_int32_params["anglvel_calibscale_x"] = &IIOWrapper::update_anglvel_calibscale_x; + m_func_map_update_int32_params["anglvel_calibscale_y"] = &IIOWrapper::update_anglvel_calibscale_y; + m_func_map_update_int32_params["anglvel_calibscale_z"] = &IIOWrapper::update_anglvel_calibscale_z; +#endif +#ifdef ADIS1654X + m_func_map_update_uint32_params["angvel_x_filter_low_pass_3db"] = + &IIOWrapper::update_angvel_x_filter_low_pass_3db; + m_func_map_update_uint32_params["angvel_y_filter_low_pass_3db"] = + &IIOWrapper::update_angvel_x_filter_low_pass_3db; + m_func_map_update_uint32_params["angvel_z_filter_low_pass_3db"] = + &IIOWrapper::update_angvel_x_filter_low_pass_3db; + m_func_map_update_uint32_params["accel_x_filter_low_pass_3db"] = + &IIOWrapper::update_angvel_x_filter_low_pass_3db; + m_func_map_update_uint32_params["accel_y_filter_low_pass_3db"] = + &IIOWrapper::update_angvel_x_filter_low_pass_3db; + m_func_map_update_uint32_params["accel_z_filter_low_pass_3db"] = + &IIOWrapper::update_angvel_x_filter_low_pass_3db; +#endif } void ImuControlParameters::mapIIOGetFunctionsInt32() @@ -90,20 +133,49 @@ void ImuControlParameters::mapIIOGetFunctionsInt32() m_func_map_get_int32_params["anglvel_calibbias_x"] = &IIOWrapper::anglvel_x_calibbias; m_func_map_get_int32_params["anglvel_calibbias_y"] = &IIOWrapper::anglvel_y_calibbias; m_func_map_get_int32_params["anglvel_calibbias_z"] = &IIOWrapper::anglvel_z_calibbias; + +#ifdef ADIS_HAS_CALIB_SCALE + m_func_map_get_int32_params["accel_calibscale_x"] = &IIOWrapper::accel_x_calibscale; + m_func_map_get_int32_params["accel_calibscale_y"] = &IIOWrapper::accel_y_calibscale; + m_func_map_get_int32_params["accel_calibscale_z"] = &IIOWrapper::accel_z_calibscale; + m_func_map_get_int32_params["anglvel_calibscale_x"] = &IIOWrapper::anglvel_x_calibscale; + m_func_map_get_int32_params["anglvel_calibscale_y"] = &IIOWrapper::anglvel_y_calibscale; + m_func_map_get_int32_params["anglvel_calibscale_z"] = &IIOWrapper::anglvel_z_calibscale; +#endif +#ifdef ADIS1654X + m_func_map_get_uint32_params["angvel_x_filter_low_pass_3db"] = + &IIOWrapper::angvel_x_filter_low_pass_3db; + m_func_map_get_uint32_params["angvel_y_filter_low_pass_3db"] = + &IIOWrapper::angvel_x_filter_low_pass_3db; + m_func_map_get_uint32_params["angvel_z_filter_low_pass_3db"] = + &IIOWrapper::angvel_x_filter_low_pass_3db; + m_func_map_get_uint32_params["accel_x_filter_low_pass_3db"] = + &IIOWrapper::angvel_x_filter_low_pass_3db; + m_func_map_get_uint32_params["accel_y_filter_low_pass_3db"] = + &IIOWrapper::angvel_x_filter_low_pass_3db; + m_func_map_get_uint32_params["accel_z_filter_low_pass_3db"] = + &IIOWrapper::angvel_x_filter_low_pass_3db; +#endif } void ImuControlParameters::mapIIOUpdateFunctionsUint32() { +#ifndef ADIS1654X m_func_map_update_uint32_params["filter_low_pass_3db_frequency"] = &IIOWrapper::update_filter_low_pass_3db_frequency; +#endif #ifdef ADIS_SENS_BW m_func_map_update_uint32_params["internal_sensor_bandwidth"] = &IIOWrapper::update_internal_sensor_bandwidth; #endif +#ifdef ADIS_PT_OF_PERC_REG_ADDR m_func_map_update_uint32_params["point_of_percussion_alignment"] = &IIOWrapper::update_point_of_percussion_alignment; +#endif +#ifdef ADIS_MSC_CTRL_ADDR m_func_map_update_uint32_params["linear_acceleration_compensation"] = &IIOWrapper::update_linear_acceleration_compensation; +#endif #ifdef ADIS_NULL_CNFG_ADDR m_func_map_update_uint32_params["bias_correction_time_base_control"] = &IIOWrapper::update_bias_correction_time_base_control; @@ -124,16 +196,22 @@ void ImuControlParameters::mapIIOUpdateFunctionsUint32() void ImuControlParameters::mapIIOGetFunctionsUint32() { +#ifndef ADIS1654X m_func_map_get_uint32_params["filter_low_pass_3db_frequency"] = &IIOWrapper::filter_low_pass_3db_frequency; +#endif #ifdef ADIS_SENS_BW m_func_map_get_uint32_params["internal_sensor_bandwidth"] = &IIOWrapper::internal_sensor_bandwidth; #endif +#ifdef ADIS_PT_OF_PERC_REG_ADDR m_func_map_get_uint32_params["point_of_percussion_alignment"] = &IIOWrapper::point_of_percussion_alignment; +#endif +#ifdef ADIS_MSC_CTRL_ADDR m_func_map_get_uint32_params["linear_acceleration_compensation"] = &IIOWrapper::linear_acceleration_compensation; +#endif #ifdef ADIS_NULL_CNFG_ADDR m_func_map_get_uint32_params["bias_correction_time_base_control"] = &IIOWrapper::bias_correction_time_base_control; @@ -165,7 +243,9 @@ void ImuControlParameters::mapIIOGetFunctionsDouble() void ImuControlParameters::mapIIOCommandFunctions() { m_func_map_execute_commands["software_reset"] = &IIOWrapper::software_reset; +#ifdef ADIS_FLASH_MEMORY_TEST m_func_map_execute_commands["flash_memory_test"] = &IIOWrapper::flash_memory_test; +#endif m_func_map_execute_commands["flash_memory_update"] = &IIOWrapper::flash_memory_update; m_func_map_execute_commands["sensor_self_test"] = &IIOWrapper::sensor_self_test; m_func_map_execute_commands["factory_calibration_restore"] = @@ -182,9 +262,15 @@ void ImuControlParameters::declareParameterDescription() param_range_calibbias.to_value = 2147483647; param_range_calibbias.step = 1; +#ifndef ADIS1654X auto param_range_0_720 = rcl_interfaces::msg::IntegerRange{}; param_range_0_720.from_value = 0; param_range_0_720.to_value = 720; +#else + auto param_range_100_300 = rcl_interfaces::msg::IntegerRange{}; + param_range_100_300.from_value = 0; + param_range_100_300.to_value = 300; +#endif auto param_range_01 = rcl_interfaces::msg::IntegerRange{}; param_range_01.from_value = 0; @@ -218,8 +304,50 @@ void ImuControlParameters::declareParameterDescription() m_param_description["accel_calibbias_z"] = "z-axis acceleration offset correction"; m_param_constraints_integer["accel_calibbias_z"] = param_range_calibbias; +#ifdef ADIS1654X + m_param_description["angvel_x_filter_low_pass_3db"] = "X angular velocity low pass 3db frequency"; + m_param_constraints_integer["angvel_x_filter_low_pass_3db"] = param_range_100_300; + + m_param_description["angvel_y_filter_low_pass_3db"] = "Y angular velocity low pass 3db frequency"; + m_param_constraints_integer["angvel_y_filter_low_pass_3db"] = param_range_100_300; + + m_param_description["angvel_z_filter_low_pass_3db"] = "Z angular velocity low pass 3db frequency"; + m_param_constraints_integer["angvel_z_filter_low_pass_3db"] = param_range_100_300; + + m_param_description["accel_x_filter_low_pass_3db"] = "X acceleration low pass 3db frequency"; + m_param_constraints_integer["accel_x_filter_low_pass_3db"] = param_range_100_300; + + m_param_description["accel_y_filter_low_pass_3db"] = "Y acceleration low pass 3db frequency"; + m_param_constraints_integer["accel_y_filter_low_pass_3db"] = param_range_100_300; + + m_param_description["accel_z_filter_low_pass_3db"] = "Z acceleration low pass 3db frequency"; + m_param_constraints_integer["accel_z_filter_low_pass_3db"] = param_range_100_300; +#else 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; +#endif + +#ifdef ADIS_HAS_CALIB_SCALE + + m_param_description["anglvel_calibscale_x"] = "x-axis angular velocity scale correction"; + m_param_constraints_integer["anglvel_calibscale_x"] = param_range_calibbias; + + m_param_description["anglvel_calibscale_y"] = "y-axis angular velocity scale correction"; + m_param_constraints_integer["anglvel_calibscale_y"] = param_range_calibbias; + + m_param_description["anglvel_calibscale_z"] = "z-axis angular velocity scale correction"; + m_param_constraints_integer["anglvel_calibscale_z"] = param_range_calibbias; + + m_param_description["accel_calibscale_x"] = "x-axis acceleration scale correction"; + m_param_constraints_integer["accel_calibscale_x"] = param_range_calibbias; + + m_param_description["accel_calibscale_y"] = "y-axis acceleration scale correction"; + m_param_constraints_integer["accel_calibscale_y"] = param_range_calibbias; + + m_param_description["accel_calibscale_z"] = "z-axis acceleration scale correction"; + m_param_constraints_integer["accel_calibscale_z"] = param_range_calibbias; + +#endif #ifdef ADIS_SENS_BW m_param_description["internal_sensor_bandwidth"] = @@ -227,17 +355,18 @@ void ImuControlParameters::declareParameterDescription() "\n1: 370 Hz"; m_param_constraints_integer["internal_sensor_bandwidth"] = param_range_01; #endif - +#ifdef ADIS_PT_OF_PERC_REG_ADDR m_param_description["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; - +#endif +#ifdef ADIS_MSC_CTRL_ADDR m_param_description["linear_acceleration_compensation"] = "\n0: linear acceleration compensation disable" "\n1: linear acceleration compensation enable"; m_param_constraints_integer["linear_acceleration_compensation"] = param_range_01; - +#endif #ifdef ADIS_NULL_CNFG_ADDR auto param_range_0_12 = rcl_interfaces::msg::IntegerRange{}; param_range_0_12.from_value = 0; @@ -288,7 +417,8 @@ void ImuControlParameters::declareParameterDescription() 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)"); + "\n3: measured data is published on /imufullmeasureddata topic " + "(default)"); m_param_constraints_integer["measured_data_topic_selection"] = param_range_03; @@ -350,13 +480,17 @@ void ImuControlParameters::declareParameters() param_desc.description = "\ncommand_to_execute values:" "\nsoftware_reset: performs a software reset on the device" +#ifdef ADIS_FLASH_MEMORY_TEST "\nflash_memory_test: performs a flash memory test on the device" +#endif "\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"; + "\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"); + "\nbias_correction_update: triggers a bias correction, using the bias " + "correction factors"); #endif m_node->declare_parameter("command_to_execute", "no_command", param_desc); @@ -424,8 +558,9 @@ void ImuControlParameters::handleDoubleParamChange() if (!(m_iio_wrapper.*(m_func_map_update_double_params[key]))(requestedValue)) { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "error on set parameter %s with %f value; current value remained %f", ckey, - requestedValue, m_double_current_params[key]); + "error on set parameter %s with %f value; current value " + "remained %f", + ckey, requestedValue, m_double_current_params[key]); } else { // readback value from hardware if (!(m_iio_wrapper.*(m_func_map_get_double_params[key]))(&dDriverParam)) { @@ -438,7 +573,8 @@ void ImuControlParameters::handleDoubleParamChange() if (dDriverParam != requestedValue) { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "Trying to set parameter %s: old value = %f requested value = %f. " + "Trying to set parameter %s: old value = %f " + "requested value = %f. " "Requested value could not be set in hardware, " "instead the following value was set: %f", ckey, m_double_current_params[key], requestedValue, dDriverParam); @@ -446,8 +582,9 @@ void ImuControlParameters::handleDoubleParamChange() } else { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "successfully set parameter %s: old value = %f new value = %f", ckey, - m_double_current_params[key], requestedValue); + "successfully set parameter %s: old " + "value = %f new value = %f", + ckey, m_double_current_params[key], requestedValue); } m_double_current_params[key] = dDriverParam; } @@ -472,8 +609,9 @@ void ImuControlParameters::handleInt32ParamChange() if (!(m_iio_wrapper.*(m_func_map_update_int32_params[key]))(requestedValue)) { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "error on set parameter %s with %d value; current value remained %d", ckey, - requestedValue, m_int32_current_params[key]); + "error on set parameter %s with %d value; current value " + "remained %d", + ckey, requestedValue, m_int32_current_params[key]); } else { // readback value from hardware if (!(m_iio_wrapper.*(m_func_map_get_int32_params[key]))(i32DriverParam)) { @@ -486,7 +624,8 @@ void ImuControlParameters::handleInt32ParamChange() if (i32DriverParam != requestedValue) { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "Trying to set parameter %s: old value = %d requested value = %d. " + "Trying to set parameter %s: old value = %d " + "requested value = %d. " "Requested value could not be set in hardware, " "instead the following value was set: %d", ckey, m_int32_current_params[key], requestedValue, i32DriverParam); @@ -494,8 +633,9 @@ void ImuControlParameters::handleInt32ParamChange() } else { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "successfully set parameter %s: old value = %d new value = %d", ckey, - m_int32_current_params[key], requestedValue); + "successfully set parameter %s: old " + "value = %d new value = %d", + ckey, m_int32_current_params[key], requestedValue); } m_int32_current_params[key] = i32DriverParam; } @@ -536,7 +676,8 @@ void ImuControlParameters::handleUint32ParamChange() if (i64DriverParam != requestedValue) { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "Trying to set parameter %s: old value = %" PRId64 " requested value = %" PRId64 + "Trying to set parameter %s: old value = " + "%" PRId64 " requested value = %" PRId64 ". " "Requested value could not be set in hardware, " "instead the following value was set: %" PRId64, @@ -545,8 +686,9 @@ void ImuControlParameters::handleUint32ParamChange() } else { RCLCPP_INFO( rclcpp::get_logger("rclcpp_imucontrolparameter"), - "successfully set parameter %s: old value = %" PRId64 " new value = %" PRId64, ckey, - m_uint32_current_params[key], requestedValue); + "successfully set parameter %s: old " + "value = %" PRId64 " new value = %" PRId64, + ckey, m_uint32_current_params[key], requestedValue); } m_uint32_current_params[key] = i64DriverParam; } diff --git a/src/imu_diag_data_provider.cpp b/src/imu_diag_data_provider.cpp index 793f5f0..bca6206 100644 --- a/src/imu_diag_data_provider.cpp +++ b/src/imu_diag_data_provider.cpp @@ -35,22 +35,44 @@ bool ImuDiagDataProvider::getData(imu_ros2::msg::ImuDiagData & message) return false; #endif +#ifdef ADIS_DATA_PATH_OVERRUN if (!m_iio_wrapper.diag_data_path_overrun(message.diag_data_path_overrun)) return false; +#endif + +#ifdef ADIS_WDG_TIMER_FLAG + if (!m_iio_wrapper.diag_automatic_reset(message.diag_automatic_reset)) return false; +#endif +#ifdef ADIS_FLS_MEM_UPDATE_FAIL if (!m_iio_wrapper.diag_flash_memory_update_error(message.diag_flash_memory_update_error)) return false; +#endif +#ifdef ADIS_SPI_COMM_ERR if (!m_iio_wrapper.diag_spi_communication_error(message.diag_spi_communication_error)) return false; +#endif +#ifdef ADIS_CRC_ERROR + if (!m_iio_wrapper.diag_crc_error(message.diag_crc_error)) return false; +#endif + +#ifdef ADIS_STDBY_MODE if (!m_iio_wrapper.diag_standby_mode(message.diag_standby_mode)) return false; +#endif +#ifdef ADIS_SNSR_FAIL if (!m_iio_wrapper.diag_sensor_self_test_error(message.diag_sensor_self_test_error)) return false; +#endif +#ifdef ADIS_FLASH_MEMORY_TEST if (!m_iio_wrapper.diag_flash_memory_test_error(message.diag_flash_memory_test_error)) return false; +#endif +#ifdef ADIS_CLK_ERR if (!m_iio_wrapper.diag_clock_error(message.diag_clock_error)) return false; +#endif #ifdef ADIS_GYRO1_FAIL if (!m_iio_wrapper.diag_gyroscope1_self_test_error(message.diag_gyroscope1_self_test_error)) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 1c0e58d..ec9229c 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -29,6 +29,13 @@ 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 "adis16545-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16545_1) +string (COMPARE EQUAL "adis16545-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16545_2) +string (COMPARE EQUAL "adis16545-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16545_3) +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 "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) @@ -62,6 +69,16 @@ elseif (${DEVICE_FOUND_ADIS16465_1} OR set(DEVICE_PATH adis1646x) add_compile_definitions(ADIS1646X) + elseif (${DEVICE_FOUND_ADIS16545_1} OR + ${DEVICE_FOUND_ADIS16545_2} OR + ${DEVICE_FOUND_ADIS16545_3} OR + ${DEVICE_FOUND_ADIS16547_1} OR + ${DEVICE_FOUND_ADIS16547_2} OR + ${DEVICE_FOUND_ADIS16547_3}) +message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.") +set(DEVICE_PATH adis1654x) +add_compile_definitions(ADIS1654X) + elseif (${DEVICE_FOUND_ADIS16575_2} OR ${DEVICE_FOUND_ADIS16575_3} OR ${DEVICE_FOUND_ADIS16576_2} OR @@ -79,6 +96,12 @@ elseif (${DEVICE_FOUND_ADIS16477_1} OR set(DEVICE_PATH adis1647x) add_compile_definitions(ADIS1647X) + elseif (${DEVICE_FOUND_ADIS16545_3}) +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. @@ -92,6 +115,8 @@ else() 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.") diff --git a/test/src/imu_diag_subscriber_test.cpp b/test/src/imu_diag_subscriber_test.cpp index b1d7202..88abc65 100644 --- a/test/src/imu_diag_subscriber_test.cpp +++ b/test/src/imu_diag_subscriber_test.cpp @@ -66,13 +66,33 @@ TEST(ImuDiagSubscriberTest, test_imu__diag_data_publisher) #ifdef ADIS_SNSR_INIT_FAIL ASSERT_TRUE(msg.diag_sensor_initialization_failure == false); #endif +#ifdef ADIS_DATA_PATH_OVERRUN ASSERT_TRUE(msg.diag_data_path_overrun == false); +#endif +#ifdef ADIS_WDG_TIMER_FLAG + ASSERT_TRUE(msg.diag_automatic_reset == false); +#endif +#ifdef ADIS_FLS_MEM_UPDATE_FAIL ASSERT_TRUE(msg.diag_flash_memory_update_error == false); +#endif +#ifdef ADIS_SPI_COMM_ERROR ASSERT_TRUE(msg.diag_spi_communication_error == false); +#endif +#ifdef ADIS_CRC_ERROR + ASSERT_TRUE(msg.diag_crc_error == false); +#endif +#ifdef ADIS_STDBY_MODE ASSERT_TRUE(msg.diag_standby_mode == false); +#endif +#ifdef ADIS_SNSR_FAIL ASSERT_TRUE(msg.diag_sensor_self_test_error == false); +#endif +#ifdef ADIS_MEM_FAIL ASSERT_TRUE(msg.diag_flash_memory_test_error == false); +#endif +#ifdef ADIS_CLK_ERR ASSERT_TRUE(msg.diag_clock_error == false); +#endif #ifdef ADIS_ACCEL_FAIL ASSERT_TRUE(msg.diag_acceleration_self_test_error == false); #endif