Skip to content

Commit a43fb4b

Browse files
rbudai98rbudai98
committed
Add support for adis1655X
Signed-off-by: rbudai <[email protected]>
1 parent 3de7bcf commit a43fb4b

File tree

13 files changed

+261
-25
lines changed

13 files changed

+261
-25
lines changed

‎.gitignore

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
11
# Ignore all txtuser files
22
*.txt.user
33
*.cpp.orig
4+
.vscode

‎CMakeLists.txt

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,10 @@ string (COMPARE EQUAL "adis16547-1" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16547_1)
3636
string (COMPARE EQUAL "adis16547-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16547_2)
3737
string (COMPARE EQUAL "adis16547-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16547_3)
3838

39+
string(COMPARE EQUAL "adis16550" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16550)
40+
string(COMPARE EQUAL "adis16550w" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16550W)
41+
string(COMPARE EQUAL "adis16550W" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16550W)
42+
3943
string (COMPARE EQUAL "adis16575-2" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16575_2)
4044
string (COMPARE EQUAL "adis16575-3" $ENV{DEVICE_ID} DEVICE_FOUND_ADIS16575_3)
4145
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.")
7983
set(DEVICE_PATH adis1654x)
8084
add_compile_definitions(ADIS1654X)
8185

86+
elseif (${DEVICE_FOUND_ADIS16550} OR
87+
${DEVICE_FOUND_ADIS16550W})
88+
message(AUTHOR_WARNING "DEVICE_ID=$ENV{DEVICE_ID} which is a valid value.")
89+
set(DEVICE_PATH adis1655x)
90+
add_compile_definitions(ADIS1655X)
91+
8292
elseif (${DEVICE_FOUND_ADIS16575_2} OR
8393
${DEVICE_FOUND_ADIS16575_3} OR
8494
${DEVICE_FOUND_ADIS16576_2} OR
@@ -111,6 +121,7 @@ else()
111121
adis16507-1, adis16507-2, adis16507-3,
112122
adis16545-1, adis16545-2, adis16545-3,
113123
adis16547-1, adis16547-2, adis16547-3,
124+
adis16550, adis16550W,
114125
adis16575-2, adis16575-3,
115126
adis16576-2, adis16576-3,
116127
adis16577-2, adis16577-3.")

‎config/adis1655x/imu_config.yaml

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
/imu_ros2_node:
2+
ros__parameters:
3+
accel_calibbias_x: 0
4+
accel_calibscale_x: 0
5+
accel_calibbias_y: 0
6+
accel_calibscale_y: 0
7+
accel_calibbias_z: 0
8+
accel_calibscale_z: 0
9+
anglvel_calibbias_x: 0
10+
anglvel_calibscale_x: 0
11+
anglvel_calibbias_y: 0
12+
anglvel_calibscale_y: 0
13+
anglvel_calibbias_z: 0
14+
anglvel_calibscale_z: 0
15+
filter_low_pass_3db_frequency: 100
16+
point_of_percussion_alignment: 1
17+
bias_correction_time_base_control: 10
18+
x_axis_gyroscope_bias_correction_enable: 0
19+
y_axis_gyroscope_bias_correction_enable: 0
20+
z_axis_gyroscope_bias_correction_enable: 0
21+
x_axis_accelerometer_bias_correction_enable: 1
22+
y_axis_accelerometer_bias_correction_enable: 1
23+
z_axis_accelerometer_bias_correction_enable: 1
24+
sampling_frequency: 4000.0
25+
command_to_execute: no_command
26+
measured_data_topic_selection: 3
27+
iio_context_string: ip:192.168.0.1

‎include/imu_ros2/adis1655x/adis1655x_data_access.h

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,113 @@
1+
#ifndef ADIS1655X_DATA_ACCESS_H
2+
#define ADIS1655X_DATA_ACCESS_H
3+
4+
// has delta channels
5+
#define ADIS_HAS_DELTA_BURST
6+
7+
// has calibration scale channel
8+
#define ADIS_HAS_CALIB_SCALE
9+
10+
#define ADIS_FLS_MEM_ENDURANCE 100000
11+
#define ADIS_MAX_SAMP_FREQ 4250.0
12+
13+
// value to add to reg addr per page
14+
#define ADIS_PAGE_ID_VAL 0x80
15+
16+
// global commands
17+
#define ADIS_GLOB_CMD_PAGE_ID 0x00
18+
#define ADIS_GLOB_CMD_ADDR_WITHOUT_PAGE 0x50
19+
#define ADIS_GLOB_CMD_ADDR \
20+
(ADIS_PAGE_ID_VAL * ADIS_PT_OF_PERC_PAGE_ID + ADIS_GLOB_CMD_ADDR_WITHOUT_PAGE)
21+
22+
#define ADIS_SENSOR_SELF_TEST_POS 1
23+
#define ADIS_FLASH_MEMORY_UPDATE_POS 3
24+
#define ADIS_FACTORY_CALIBRATION_RESTORE_POS 2
25+
#define ADIS_SOFTWARE_RESET_CMD_POS 5
26+
27+
#define ADIS_SENSOR_SELF_TEST (1 << ADIS_SENSOR_SELF_TEST_POS)
28+
#define ADIS_FLASH_MEMORY_UPDATE (1 << ADIS_FLASH_MEMORY_UPDATE_POS)
29+
#define ADIS_FACTORY_CALIBRATION_RESTORE (1 << ADIS_FACTORY_CALIBRATION_RESTORE_POS)
30+
#define ADIS_SOFTWARE_RESET_CMD (1 << ADIS_SOFTWARE_RESET_CMD_POS)
31+
32+
// status and error flag indication
33+
#define ADIS_DIAG_STAT_PAGE_ID 0x00
34+
#define ADIS_DIAG_STAT_ADDR_WITHOUT_PAGE 0x0E
35+
#define ADIS_DIAG_STAT_ADDR \
36+
(ADIS_PAGE_ID_VAL * ADIS_DIAG_STAT_PAGE_ID + ADIS_DIAG_STAT_ADDR_WITHOUT_PAGE)
37+
38+
#define ADIS_MEM_FAIL_POS 0
39+
#define ADIS_CRC_ERROR_POS 1
40+
#define ADIS_FLS_MEM_UPDATE_FAIL_POS 2
41+
#define ADIS_SNSR_FAIL_POS 4
42+
#define ADIS_SPI_COMM_ERR_POS 6
43+
#define ADIS_DATA_PATH_OVERRUN_POS 7
44+
#define ADIS_CLK_ERR_POS 10
45+
#define ADIS_WDG_TIMER_FLAG_POS 15
46+
47+
#define ADIS_MEM_FAIL (1 << ADIS_MEM_FAIL_POS)
48+
#define ADIS_CRC_ERROR (1 << ADIS_CRC_ERROR_POS)
49+
#define ADIS_FLS_MEM_UPDATE_FAIL (1 << ADIS_FLS_MEM_UPDATE_FAIL_POS)
50+
#define ADIS_SNSR_FAIL (1 << ADIS_SNSR_FAIL_POS)
51+
#define ADIS_SPI_COMM_ERR (1 << ADIS_SPI_COMM_ERR_POS)
52+
#define ADIS_DATA_PATH_OVERRUN (1 << ADIS_DATA_PATH_OVERRUN_POS)
53+
#define ADIS_CLK_ERR (1 << ADIS_CLK_ERR_POS)
54+
#define ADIS_WDG_TIMER_FLAG (1 << ADIS_WDG_TIMER_FLAG_POS)
55+
56+
// self test error flags
57+
#define ADIS_DIAG_STS_PAGE_ID 0x00
58+
#define ADIS_DIAG_STS_REG_WITHOUT_PAGE 0x0F
59+
#define ADIS_DIAG_STS_REG \
60+
(ADIS_PAGE_ID_VAL * ADIS_DIAG_STS_PAGE_ID + ADIS_DIAG_STS_REG_WITHOUT_PAGE)
61+
62+
#define ADIS_GYRO_ACCEL_FAIL_REG ADIS_DIAG_STS_REG
63+
#define ADIS_GYRO_X_FAIL_POS 0
64+
#define ADIS_GYRO_Y_FAIL_POS 2
65+
#define ADIS_GYRO_Z_FAIL_POS 4
66+
#define ADIS_ACCEL_X_FAIL_POS 6
67+
#define ADIS_ACCEL_Y_FAIL_POS 8
68+
#define ADIS_ACCEL_Z_FAIL_POS 10
69+
70+
#define ADIS_GYRO_X_FAIL (3 << ADIS_GYRO_X_FAIL_POS)
71+
#define ADIS_GYRO_Y_FAIL (3 << ADIS_GYRO_Y_FAIL_POS)
72+
#define ADIS_GYRO_Z_FAIL (3 << ADIS_GYRO_Z_FAIL_POS)
73+
#define ADIS_ACCEL_X_FAIL (3 << ADIS_ACCEL_X_FAIL_POS)
74+
#define ADIS_ACCEL_Y_FAIL (3 << ADIS_ACCEL_Y_FAIL_POS)
75+
#define ADIS_ACCEL_Z_FAIL (3 << ADIS_ACCEL_Z_FAIL_POS)
76+
77+
// measurement range identifier
78+
#define ADIS_RANG_MDL_PAGE_ID 0x00
79+
#define ADIS_RANG_MDL_ADDR_WITHOUT_PAGE 0x10
80+
#define ADIS_RANG_MDL_ADDR \
81+
(ADIS_PAGE_ID_VAL * ADIS_RANG_MDL_PAGE_ID + ADIS_RANG_MDL_ADDR_WITHOUT_PAGE)
82+
83+
// point of percussion
84+
#define ADIS_PT_OF_PERC_PAGE_ID 0x00
85+
#define ADIS_PT_OF_PERC_REG_ADDR_WITHOUT_PAGE 0x52
86+
#define ADIS_PT_OF_PERC_REG_ADDR \
87+
(ADIS_PAGE_ID_VAL * ADIS_PT_OF_PERC_PAGE_ID + ADIS_PT_OF_PERC_REG_ADDR_WITHOUT_PAGE)
88+
#define ADIS_PT_OF_PERC_ALGNMNT_POS 4
89+
#define ADIS_PT_OF_PERC_ALGNMNT (1 << ADIS_PT_OF_PERC_ALGNMNT_POS)
90+
91+
// continuous bias estimation
92+
// #define ADIS_NULL_CNFG_PAGE_ID 0x03
93+
// #define ADIS_NULL_CNFG_ADDR_WITHOUT_PAGE 0x0E
94+
// #define ADIS_NULL_CNFG_ADDR \
95+
// (ADIS_PAGE_ID_VAL * ADIS_NULL_CNFG_PAGE_ID + ADIS_NULL_CNFG_ADDR_WITHOUT_PAGE)
96+
97+
// #define ADIS_TIME_BASE_CONTROL_POS 0
98+
// #define ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS 8
99+
// #define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS 9
100+
// #define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS 10
101+
// #define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS 11
102+
// #define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS 12
103+
// #define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS 13
104+
105+
// #define ADIS_TIME_BASE_CONTROL 0xF
106+
// #define ADIS_X_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_X_AXIS_GYRO_BIAS_CORR_EN_POS)
107+
// #define ADIS_Y_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Y_AXIS_GYRO_BIAS_CORR_EN_POS)
108+
// #define ADIS_Z_AXIS_GYRO_BIAS_CORR_EN (1 << ADIS_Z_AXIS_GYRO_BIAS_CORR_EN_POS)
109+
// #define ADIS_X_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_X_AXIS_ACCEL_BIAS_CORR_EN_POS)
110+
// #define ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Y_AXIS_ACCEL_BIAS_CORR_EN_POS)
111+
// #define ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN (1 << ADIS_Z_AXIS_ACCEL_BIAS_CORR_EN_POS)
112+
113+
#endif

‎include/imu_ros2/adis1657x/adis1657x_data_access.h

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,6 @@
4141

4242
#define ADIS_RANG_MDL_ADDR 0x5E
4343
#define ADIS_GYRO_MEAS_RANG_POS 2
44-
4544
#define ADIS_GYRO_MEAS_RANG (3 << ADIS_GYRO_MEAS_RANG_POS)
4645

4746
#define ADIS_MSC_CTRL_ADDR 0x60

‎include/imu_ros2/adis_data_access.h

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,8 @@ enum adis_device_id
4848
ADIS16547_1,
4949
ADIS16547_2,
5050
ADIS16547_3,
51+
ADIS16550,
52+
ADIS16550W,
5153
ADIS16575_2,
5254
ADIS16575_3,
5355
ADIS16576_2,
@@ -64,6 +66,8 @@ enum adis_device_id
6466
#include "adis1650x/adis1650x_data_access.h"
6567
#elif defined(ADIS1654X)
6668
#include "adis1654x/adis1654x_data_access.h"
69+
#elif defined(ADIS1655X)
70+
#include "adis1655x/adis1655x_data_access.h"
6771
#elif defined(ADIS1657X)
6872
#include "adis1657x/adis1657x_data_access.h"
6973
#else

‎include/imu_ros2/iio_wrapper.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -394,7 +394,7 @@ class IIOWrapper
394394
*/
395395
bool update_accel_calibbias_z(int32_t val);
396396

397-
#ifdef ADIS1654X
397+
#if defined(ADIS1654X) || defined(ADIS1655X)
398398
/**
399399
* @brief Get low pass 3db frequency data for x angular velocity.
400400
* @param result low pass 3db frequency value.

‎launch/imu_with_madgwick_filter_rviz.launch.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ def generate_launch_description():
1919
executable='imu_ros2_node',
2020
parameters=[{'measured_data_topic_selection': 2},
2121
# the IP address of the processing unit to which the IMU is connected to
22-
{'iio_context_string': "ip:0.0.0.0"},],
22+
{'iio_context_string': "ip:10.42.1.83"},],
2323
remappings=[('/imu','/imu/data_raw')],
2424
output='screen'
2525
)

‎msg/adis1655x/ImuDiagData.msg

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
################################################################################
2+
# @file ImuDiagData.msg
3+
# @brief Definition of ImuDiagData message
4+
# @author Robert Budai ([email protected])
5+
################################################################################
6+
# Copyright 2023(c) Analog Devices, Inc.
7+
#
8+
# Licensed under the Apache License, Version 2.0 (the "License");
9+
# you may not use this file except in compliance with the License.
10+
# You may obtain a copy of the License at
11+
#
12+
# http://www.apache.org/licenses/LICENSE-2.0
13+
#
14+
# Unless required by applicable law or agreed to in writing, software
15+
# distributed under the License is distributed on an "AS IS" BASIS,
16+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17+
# See the License for the specific language governing permissions and
18+
# limitations under the License.
19+
################################################################################
20+
21+
# message header
22+
std_msgs/Header header
23+
24+
# if true, one of the datapaths experienced an overrun condition
25+
bool diag_data_path_overrun
26+
27+
# if true, the most recent imu memory flash failed
28+
bool diag_flash_memory_update_error
29+
30+
# if true, sensor automatically reset themselves to clear an issue
31+
bool diag_automatic_reset
32+
33+
# if true, while operating in scaled sync mode, indicates the sampling time is not scaling correctly
34+
bool diag_clock_error
35+
36+
# if true, the most recent imu memory flash failed
37+
bool diag_flash_memory_test_error
38+
39+
# if true, indicates the failure of the inertial sensor
40+
bool diag_sensor_self_test_error
41+
42+
#if true, indicates communication error on SPI interface
43+
bool diag_spi_communication_error
44+
45+
# if true, indicates failure on CRC calculation
46+
bool diag_crc_error
47+
48+
# if true, a failure occurred on x axis gyroscope
49+
bool diag_x_axis_gyroscope_failure
50+
51+
# if true, a failure occurred on y axis gyroscope
52+
bool diag_y_axis_gyroscope_failure
53+
54+
# if true, a failure occurred on z axis gyroscope
55+
bool diag_z_axis_gyroscope_failure
56+
57+
# if true, a failure occurred on x axis accelerometer
58+
bool diag_x_axis_accelerometer_failure
59+
60+
# if true, a failure occurred on y axis accelerometer
61+
bool diag_y_axis_accelerometer_failure
62+
63+
# if true, a failure occurred on z axis accelerometer
64+
bool diag_z_axis_accelerometer_failure
65+
66+
# if true, the imu flash memory was written more times than the data-sheet specified endurance
67+
bool diag_flash_memory_write_count_exceeded_error
68+
69+
# the value of the imu flash writes
70+
uint32 flash_counter

‎src/iio_wrapper.cpp

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -135,8 +135,8 @@ void IIOWrapper::createContext(const char * context)
135135
"adis16470", "adis16475-1", "adis16475-2", "adis16475-3", "adis16477-1", "adis16477-2",
136136
"adis16477-3", "adis16500", "adis16501", "adis16505-1", "adis16505-2", "adis16505-3",
137137
"adis16507-1", "adis16507-2", "adis16507-3", "adis16545-1", "adis16545-2", "adis16545-3",
138-
"adis16547-1", "adis16547-2", "adis16547-3", "adis16575-2", "adis16575-3", "adis16576-2",
139-
"adis16576-3", "adis16577-2", "adis16577-3"};
138+
"adis16547-1", "adis16547-2", "adis16547-3", "adis16550", "adis16550w", "adis16575-2",
139+
"adis16575-3", "adis16576-2", "adis16576-3", "adis16577-2", "adis16577-3"};
140140

141141
uint8_t dev_id = 0;
142142

@@ -1109,7 +1109,7 @@ bool IIOWrapper::update_sampling_frequency(double val)
11091109
return true;
11101110
}
11111111

1112-
#ifdef ADIS1654X
1112+
#if defined(ADIS1654X) || defined(ADIS1655X)
11131113
bool IIOWrapper::angvel_x_filter_low_pass_3db(uint32_t & result)
11141114
{
11151115
long long valuel;
@@ -1734,8 +1734,11 @@ bool IIOWrapper::gyroscope_measurement_range(std::string & result)
17341734
int ret = iio_device_reg_read(m_dev, ADIS_RANG_MDL_ADDR, &reg_val);
17351735
if (ret) return false;
17361736

1737+
#ifdef ADIS1655X
1738+
result = "+/-300_degrees_per_sec";
1739+
return true;
1740+
#else
17371741
reg_val = (reg_val & ADIS_GYRO_MEAS_RANG) >> ADIS_GYRO_MEAS_RANG_POS;
1738-
17391742
switch (reg_val) {
17401743
case 0:
17411744
result = "+/-125_degrees_per_sec";
@@ -1752,7 +1755,8 @@ bool IIOWrapper::gyroscope_measurement_range(std::string & result)
17521755
return true;
17531756
default:
17541757
return false;
1755-
}
1758+
1759+
#endif
17561760
}
17571761

17581762
#ifdef ADIS_SENS_BW

0 commit comments

Comments
 (0)