From ae91f3b303664acbdec7be3cf6d6bf271fd5bfab Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 20 Dec 2024 11:47:59 +0200 Subject: [PATCH] ADIS16470: Publish data only when there are no failures Publishing data also in error cases may cause irregular rate at bootup, leading to wrong sample rate detection in "sensors" module Signed-off-by: Jukka Laitinen --- .../analog_devices/adis16470/ADIS16470.cpp | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp index 71de4791beea..15ee4c57ad65 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp @@ -234,6 +234,14 @@ void ADIS16470::RunImpl() ScheduleDelayed(SAMPLE_INTERVAL_US * 2); } + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + bool success = false; struct BurstRead { @@ -302,26 +310,23 @@ void ADIS16470::RunImpl() _px4_gyro.set_temperature(temperature); - int16_t accel_x = buffer.X_ACCL_OUT; - int16_t accel_y = buffer.Y_ACCL_OUT; - int16_t accel_z = buffer.Z_ACCL_OUT; + accel_x = buffer.X_ACCL_OUT; + accel_y = buffer.Y_ACCL_OUT; + accel_z = buffer.Z_ACCL_OUT; // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; - _px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z); - + gyro_x = buffer.X_GYRO_OUT; + gyro_y = buffer.Y_GYRO_OUT; + gyro_z = buffer.Z_GYRO_OUT; - int16_t gyro_x = buffer.X_GYRO_OUT; - int16_t gyro_y = buffer.Y_GYRO_OUT; - int16_t gyro_z = buffer.Z_GYRO_OUT; // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; - _px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z); success = true; @@ -353,7 +358,16 @@ void ADIS16470::RunImpl() // register check failed, force reset perf_count(_bad_register_perf); Reset(); + return; } + + } + + if (success) { + // Publish data if there was no errors + + _px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z); + _px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z); } }