diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 1d929f9ecf..20c9f28d9c 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -41,8 +41,6 @@ #define DIR_WRITE 0x80 // ADIS16477 registers - - static constexpr uint8_t DIAG_STAT = 0x02; // Output, system error flags static constexpr uint8_t X_GYRO_LOW = 0x04; // Output, x-axis gyroscope, low word @@ -239,7 +237,7 @@ ADIS16477::probe() return PX4_OK; } - PX4_ERR("PROD_ID attempt 0x73 %d", i); + DEVICE_DEBUG("PROD_ID attempt 0x73 %d", i); up_udelay(T_STALL); } @@ -495,24 +493,51 @@ ADIS16477::measure_trampoline(void *arg) int ADIS16477::measure() { - ADISReport adis_report = {}; - - // ADIS16477 burst report should be 176 bits - //static_assert(sizeof(cmd) + sizeof(report) == (176 / 8), "ADIS16477 report not 176 bits"); - - // start measuring perf_begin(_sample_perf); // Fetch the full set of measurements from the ADIS16477 in one pass (burst read). + ADISReport adis_report; adis_report.cmd = ((GLOB_CMD | DIR_READ) << 8) & 0xff00; + // ADIS16477 burst report should be 176 bits + static_assert(sizeof(adis_report) == (176 / 8), "ADIS16477 report not 176 bits"); + if (OK != transferhword((uint16_t *)&adis_report, ((uint16_t *)&adis_report), sizeof(adis_report) / sizeof(uint16_t))) { + perf_end(_sample_perf); + return -EIO; + } + + // Calculate checksum and compare + + // Checksum = DIAG_STAT, Bits[15:8] + DIAG_STAT, Bits[7:0] + + // X_GYRO_OUT, Bits[15:8] + X_GYRO_OUT, Bits[7:0] + + // Y_GYRO_OUT, Bits[15:8] + Y_GYRO_OUT, Bits[7:0] + + // Z_GYRO_OUT, Bits[15:8] + Z_GYRO_OUT, Bits[7:0] + + // X_ACCL_OUT, Bits[15:8] + X_ACCL_OUT, Bits[7:0] + + // Y_ACCL_OUT, Bits[15:8] + Y_ACCL_OUT, Bits[7:0] + + // Z_ACCL_OUT, Bits[15:8] + Z_ACCL_OUT, Bits[7:0] + + // TEMP_OUT, Bits[15:8] + TEMP_OUT, Bits[7:0] + + // DATA_CNTR, Bits[15:8] + DATA_CNTR, Bits[7:0] + uint8_t *checksum_helper = (uint8_t *)&adis_report.diag_stat; + + uint8_t checksum = 0; + + for (int i = 0; i < 18; i++) { + checksum += checksum_helper[i]; + } + + if (adis_report.checksum != checksum) { + PX4_ERR("adis_report.checksum: %X vs calculated: %X", adis_report.checksum, checksum); + perf_event_count(_bad_transfers); + perf_end(_sample_perf); return -EIO; } + // Check all Status/Error Flag Indicators (DIAG_STAT) if (adis_report.diag_stat != 0) { - //PX4_INFO("status: %X", adis_report.diag_stat); perf_event_count(_bad_transfers); + perf_end(_sample_perf); + return -EIO; } publish_gyro(adis_report); @@ -526,25 +551,21 @@ ADIS16477::measure() bool ADIS16477::publish_accel(const ADISReport &report) { - const bool x_valid = (report.accel_x != 0) && (report.accel_x != INT16_MAX); - const bool y_valid = (report.accel_y != 0) && (report.accel_y != INT16_MAX); - const bool z_valid = (report.accel_z != 0) && (report.accel_z != INT16_MAX); - - if (!x_valid || !y_valid || !z_valid) { - return false; - } - accel_report arb = {}; arb.timestamp = hrt_absolute_time(); + arb.device_id = _device_id.devid; arb.error_count = perf_event_count(_bad_transfers); - float xraw_f = report.accel_x * _accel_range_scale; - float yraw_f = report.accel_y * _accel_range_scale; - float zraw_f = report.accel_z * _accel_range_scale; - + // raw sensor readings arb.x_raw = report.accel_x; arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; + + float xraw_f = report.accel_x * _accel_range_scale; + float yraw_f = report.accel_y * _accel_range_scale; + float zraw_f = report.accel_z * _accel_range_scale; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -565,16 +586,11 @@ ADIS16477::publish_accel(const ADISReport &report) arb.y_integral = aval_integrated(1); arb.z_integral = aval_integrated(2); - arb.scaling = _accel_range_scale; - arb.range_m_s2 = _accel_range_m_s2; - /* Temperature report: */ // temperature 1 LSB = 0.1°C + arb.temperature_raw = report.temp; arb.temperature = report.temp * 0.1; - /* return device ID */ - arb.device_id = _device_id.devid; - if (accel_notify) { orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } @@ -585,19 +601,14 @@ ADIS16477::publish_accel(const ADISReport &report) bool ADIS16477::publish_gyro(const ADISReport &report) { - const bool x_valid = (report.gyro_x != 0) && (report.gyro_x != INT16_MAX); - const bool y_valid = (report.gyro_y != 0) && (report.gyro_y != INT16_MAX); - const bool z_valid = (report.gyro_z != 0) && (report.gyro_z != INT16_MAX); - - if (!x_valid || !y_valid || !z_valid) { - return false; - } - gyro_report grb = {}; grb.timestamp = hrt_absolute_time(); + grb.device_id = _gyro->_device_id.devid; grb.error_count = perf_event_count(_bad_transfers); /* Gyro report: */ + grb.scaling = math::radians(_gyro_range_scale); + grb.range_rad_s = _gyro_range_rad_s; grb.x_raw = report.gyro_x; grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; @@ -626,12 +637,10 @@ ADIS16477::publish_gyro(const ADISReport &report) grb.y_integral = gval_integrated(1); grb.z_integral = gval_integrated(2); - grb.scaling = math::radians(_gyro_range_scale); - grb.range_rad_s = _gyro_range_rad_s; - /* Temperature report: */ // temperature 1 LSB = 0.1°C - grb.temperature = report.temp * 0.1; + grb.temperature_raw = report.temp; + grb.temperature = report.temp * 0.1f; if (gyro_notify) { orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index 3c3aeb5a5e..43e208ef4c 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -140,20 +140,8 @@ private: int16_t accel_z; uint16_t temp; uint16_t DATA_CNTR; - uint16_t checksum; - - ADISReport(): - cmd(0), - diag_stat(0), - gyro_x(0), - gyro_y(0), - gyro_z(0), - accel_x(0), - accel_y(0), - accel_z(0), - temp(0), - DATA_CNTR(0) - {} + uint8_t checksum; + uint8_t _padding; // 16 bit SPI mode }; #pragma pack(pop)