Skip to content

Commit

Permalink
ADIS16470: Publish data only when there are no failures
Browse files Browse the repository at this point in the history
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 <[email protected]>
  • Loading branch information
jlaitine committed Dec 20, 2024
1 parent 512fb8e commit 6e0ee8b
Showing 1 changed file with 18 additions and 9 deletions.
27 changes: 18 additions & 9 deletions src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -354,6 +359,10 @@ void ADIS16470::RunImpl()
perf_count(_bad_register_perf);
Reset();
}

} else {
_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
}
}

Expand Down

0 comments on commit 6e0ee8b

Please sign in to comment.