Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 15:08:16 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

NuttX STM32H7 16 bit SPI fixes + ADIS16470 updates for proper 16 bit mode in burst

Browse Source
This commit is contained in:
David Sidrane
2021-03-24 16:29:40 -07:00
committed by GitHub
parent 92910de81d
commit 7f2566dd8a
2 changed files with 24 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/nuttx/NuttX/nuttx
+1 -1
Submodule platforms/nuttx/NuttX/nuttx updated: 7557bfcbe2...828d156df7
src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
+23 -27
View File
@@ -218,29 +218,27 @@ void ADIS16470::RunImpl()
bool success = false; bool success = false;
struct BurstRead { struct BurstRead {
uint8_t cmd[2]; uint16_t cmd;
uint8_t DIAG_STAT[2]; uint16_t DIAG_STAT;
uint8_t X_GYRO_OUT[2]; int16_t X_GYRO_OUT;
uint8_t Y_GYRO_OUT[2]; int16_t Y_GYRO_OUT;
uint8_t Z_GYRO_OUT[2]; int16_t Z_GYRO_OUT;
uint8_t X_ACCL_OUT[2]; int16_t X_ACCL_OUT;
uint8_t Y_ACCL_OUT[2]; int16_t Y_ACCL_OUT;
uint8_t Z_ACCL_OUT[2]; int16_t Z_ACCL_OUT;
uint8_t TEMP_OUT[2]; int16_t TEMP_OUT;
uint8_t DATA_CNTR[2]; uint16_t DATA_CNTR;
uint8_t _padding; // 16 bit SPI mode uint16_t checksum;
uint8_t checksum;
} buffer{}; } buffer{};
// ADIS16470 burst report should be 176 bits // ADIS16470 burst report should be 176 bits
static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16470 report not 176 bits"); static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16470 report not 176 bits");
buffer.cmd[0] = static_cast<uint16_t>(Register::GLOB_CMD);// << 8; buffer.cmd = static_cast<uint16_t>(Register::GLOB_CMD) << 8;
set_frequency(SPI_SPEED_BURST); set_frequency(SPI_SPEED_BURST);
//if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) { if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) {
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
// Calculate checksum and compare // Calculate checksum and compare
@@ -255,7 +253,7 @@ void ADIS16470::RunImpl()
// DATA_CNTR, Bits[15:8] + DATA_CNTR, Bits[7:0] // DATA_CNTR, Bits[15:8] + DATA_CNTR, Bits[7:0]
uint8_t *checksum_helper = (uint8_t *)&buffer.DIAG_STAT; uint8_t *checksum_helper = (uint8_t *)&buffer.DIAG_STAT;
uint8_t checksum = 0; uint16_t checksum = 0;
for (int i = 0; i < 18; i++) { for (int i = 0; i < 18; i++) {
checksum += checksum_helper[i]; checksum += checksum_helper[i];
@@ -266,9 +264,7 @@ void ADIS16470::RunImpl()
perf_count(_bad_transfer_perf); perf_count(_bad_transfer_perf);
} }
uint16_t DIAG_STAT = combine(buffer.DIAG_STAT[0], buffer.DIAG_STAT[1]); if (buffer.DIAG_STAT != DIAG_STAT_BIT::Data_path_overrun) {
if (DIAG_STAT != DIAG_STAT_BIT::Data_path_overrun) {
// Data path overrun. A 1 indicates that one of the // Data path overrun. A 1 indicates that one of the
// data paths have experienced an overrun condition. // data paths have experienced an overrun condition.
// If this occurs, initiate a reset, // If this occurs, initiate a reset,
@@ -278,19 +274,19 @@ void ADIS16470::RunImpl()
} }
// Check all Status/Error Flag Indicators (DIAG_STAT) // Check all Status/Error Flag Indicators (DIAG_STAT)
if (DIAG_STAT != 0) { if (buffer.DIAG_STAT != 0) {
perf_count(_bad_transfer_perf); perf_count(_bad_transfer_perf);
} }
// temperature 1 LSB = 0.1°C // temperature 1 LSB = 0.1°C
const float temperature = combine(buffer.TEMP_OUT[0], buffer.TEMP_OUT[1]) * 0.1f; const float temperature = buffer.TEMP_OUT * 0.1f;
_px4_accel.set_temperature(temperature); _px4_accel.set_temperature(temperature);
_px4_gyro.set_temperature(temperature); _px4_gyro.set_temperature(temperature);
int16_t accel_x = combine(buffer.X_ACCL_OUT[0], buffer.X_ACCL_OUT[1]); int16_t accel_x = buffer.X_ACCL_OUT;
int16_t accel_y = combine(buffer.Y_ACCL_OUT[0], buffer.Y_ACCL_OUT[1]); int16_t accel_y = buffer.Y_ACCL_OUT;
int16_t accel_z = combine(buffer.Z_ACCL_OUT[0], buffer.Z_ACCL_OUT[1]); int16_t accel_z = buffer.Z_ACCL_OUT;
// sensor's frame is +x forward, +y left, +z up // 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) // flip y & z to publish right handed with z down (x forward, y right, z down)
@@ -300,9 +296,9 @@ void ADIS16470::RunImpl()
_px4_accel.update(now, accel_x, accel_y, accel_z); _px4_accel.update(now, accel_x, accel_y, accel_z);
int16_t gyro_x = combine(buffer.X_GYRO_OUT[0], buffer.X_GYRO_OUT[1]); int16_t gyro_x = buffer.X_GYRO_OUT;
int16_t gyro_y = combine(buffer.Y_GYRO_OUT[0], buffer.Y_GYRO_OUT[1]); int16_t gyro_y = buffer.Y_GYRO_OUT;
int16_t gyro_z = combine(buffer.Z_GYRO_OUT[0], buffer.Z_GYRO_OUT[1]); int16_t gyro_z = buffer.Z_GYRO_OUT;
// sensor's frame is +x forward, +y left, +z up // 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) // 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_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;