From d5822b55765f451795cad556f275ecbe06689b37 Mon Sep 17 00:00:00 2001 From: Meet Gandhi Date: Fri, 13 Oct 2023 15:49:10 +0530 Subject: [PATCH] ADIS16470 fixes. 1. Fixed Adis16470::read_register return value on success. 2. Fixed Adis16470::update_burst read return value check (24 instead of 30) 3. Fixed Adis16470::update_burst accl value updates according to the datasheet (Table 25. 16-Bit Accelerometer Data Format Examples) 4. Fixed Adis16470::update_burst temerature offset handling (should be 17 instead of 16) 5. Adis16470::set_bias_estimation_time use fprintf instead of ROSINFO to avoid ROS dependency in non-ROS source. Signed-off-by: Meet Gandhi --- src/adis16470.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/adis16470.cpp b/src/adis16470.cpp index 67cb558..71a90b0 100644 --- a/src/adis16470.cpp +++ b/src/adis16470.cpp @@ -226,8 +226,10 @@ int Adis16470::read_register(char address, int16_t& data) if (size !=3) { perror("read"); + return -1; } data = big_endian_to_short(&buff[1]); + return 0; } /** @@ -301,8 +303,8 @@ int Adis16470::update_burst(void) perror("update_burst"); return -1; } - size = read(fd_, buff, 30); - if (size != 30) + size = read(fd_, buff, 24); + if (size != 24) { perror("update_burst"); return -1; @@ -320,13 +322,13 @@ int Adis16470::update_burst(void) // Z_GYRO_OUT gyro[2] = big_endian_to_short(&buff[9]) * M_PI / 180 / 10.0; // X_ACCL_OUT - accl[0] = big_endian_to_short(&buff[11]) * M_PI / 180 / 10.0; + accl[0] = big_endian_to_short(&buff[11]) * 9.8 * 40.0 / 32000.0; // Y_ACCL_OUT - accl[1] = big_endian_to_short(&buff[13]) * M_PI / 180 / 10.0; + accl[1] = big_endian_to_short(&buff[13]) * 9.8 * 40.0 / 32000.0; // Z_ACCL_OUT - accl[2] = big_endian_to_short(&buff[15]) * M_PI / 180 / 10.0; + accl[2] = big_endian_to_short(&buff[15]) * 9.8 * 40.0 / 32000.0; // TEMP_OUT - temp = big_endian_to_short(&buff[16]) * 0.1; + temp = big_endian_to_short(&buff[17]) * 0.1; return 0; } @@ -376,7 +378,7 @@ int Adis16470::set_bias_estimation_time(int16_t tbc) int16_t dummy = 0; read_register(0x66, dummy); read_register(0x00, tbc); - ROS_INFO("TBC: %04x", tbc); + fprintf(stdout, "TBC: %04x", tbc); return 0; }