From ab78b930e9dde65f16dae1272bb2bee7dee0158b Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Tue, 26 Dec 2023 16:39:37 -0500 Subject: [PATCH] [wpilib] ADIS16470: Add access to all 3 axes (#6074) --- wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp | 253 +++++++--- .../main/native/include/frc/ADIS16470_IMU.h | 159 +++--- .../edu/wpi/first/wpilibj/ADIS16470_IMU.java | 462 +++++++++++++----- 3 files changed, 621 insertions(+), 253 deletions(-) diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 05dac7893d8..4eaa7ccd609 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -63,14 +63,35 @@ inline void ADISReportError(int32_t status, const char* file, int line, * Constructor. */ ADIS16470_IMU::ADIS16470_IMU() - : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {} + : ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {} -ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, +ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, + IMUAxis roll_axis) + : ADIS16470_IMU(yaw_axis, pitch_axis, roll_axis, SPI::Port::kOnboardCS0, + CalibrationTime::_4s) {} + +ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, + IMUAxis roll_axis, SPI::Port port, CalibrationTime cal_time) : m_yaw_axis(yaw_axis), + m_pitch_axis(pitch_axis), + m_roll_axis(roll_axis), m_spi_port(port), m_calibration_time(static_cast(cal_time)), m_simDevice("Gyro:ADIS16470", port) { + if (yaw_axis == kYaw || yaw_axis == kPitch || yaw_axis == kRoll || + pitch_axis == kYaw || pitch_axis == kPitch || pitch_axis == kRoll || + roll_axis == kYaw || roll_axis == kPitch || roll_axis == kRoll) { + REPORT_ERROR( + "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or " + "IMUAxis.kZ as arguments."); + REPORT_ERROR( + "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)"); + yaw_axis = kZ; + pitch_axis = kY; + roll_axis = kX; + } + if (m_simDevice) { m_connected = m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true); @@ -266,17 +287,8 @@ bool ADIS16470_IMU::SwitchToAutoSPI() { m_auto_configured = true; } // Do we need to change auto SPI settings? - switch (m_yaw_axis) { - case kX: - m_spi->SetAutoTransmitData(m_autospi_x_packet, 2); - break; - case kY: - m_spi->SetAutoTransmitData(m_autospi_y_packet, 2); - break; - default: - m_spi->SetAutoTransmitData(m_autospi_z_packet, 2); - break; - } + m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2); + // Configure auto stall time m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1); // Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is @@ -445,7 +457,9 @@ void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) { **/ void ADIS16470_IMU::Reset() { std::scoped_lock sync(m_mutex); - m_integ_angle = 0.0; + m_integ_angle_x = 0.0; + m_integ_angle_y = 0.0; + m_integ_angle_z = 0.0; } void ADIS16470_IMU::Close() { @@ -502,7 +516,7 @@ ADIS16470_IMU::~ADIS16470_IMU() { **/ void ADIS16470_IMU::Acquire() { // Set data packet length - const int dataset_len = 19; // 18 data points + timestamp + const int dataset_len = 27; // 26 data points + timestamp /* Fixed buffer size */ const int BUFFER_SIZE = 4000; @@ -513,7 +527,9 @@ void ADIS16470_IMU::Acquire() { int data_remainder = 0; int data_to_read = 0; uint32_t previous_timestamp = 0; - double delta_angle = 0.0; + double delta_angle_x = 0.0; + double delta_angle_y = 0.0; + double delta_angle_z = 0.0; double gyro_rate_x = 0.0; double gyro_rate_y = 0.0; double gyro_rate_z = 0.0; @@ -562,14 +578,22 @@ void ADIS16470_IMU::Acquire() { m_dt = (buffer[i] - previous_timestamp) / 1000000.0; /* Get delta angle value for selected yaw axis and scale by the elapsed * time (based on timestamp) */ - delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) / - (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - gyro_rate_x = (BuffToShort(&buffer[i + 7]) / 10.0); - gyro_rate_y = (BuffToShort(&buffer[i + 9]) / 10.0); - gyro_rate_z = (BuffToShort(&buffer[i + 11]) / 10.0); - accel_x = (BuffToShort(&buffer[i + 13]) / 800.0); - accel_y = (BuffToShort(&buffer[i + 15]) / 800.0); - accel_z = (BuffToShort(&buffer[i + 17]) / 800.0); + delta_angle_x = + (ToInt(&buffer[i + 3]) * delta_angle_sf) / + (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); + delta_angle_y = + (ToInt(&buffer[i + 7]) * delta_angle_sf) / + (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); + delta_angle_z = + (ToInt(&buffer[i + 11]) * delta_angle_sf) / + (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); + + gyro_rate_x = (BuffToShort(&buffer[i + 15]) / 10.0); + gyro_rate_y = (BuffToShort(&buffer[i + 17]) / 10.0); + gyro_rate_z = (BuffToShort(&buffer[i + 19]) / 10.0); + accel_x = (BuffToShort(&buffer[i + 21]) / 800.0); + accel_y = (BuffToShort(&buffer[i + 23]) / 800.0); + accel_z = (BuffToShort(&buffer[i + 25]) / 800.0); // Convert scaled sensor data to SI units gyro_rate_x_si = gyro_rate_x * deg_to_rad; @@ -611,9 +635,13 @@ void ADIS16470_IMU::Acquire() { if (m_first_run) { /* Don't accumulate first run. previous_timestamp will be "very" old * and the integration will end up way off */ - m_integ_angle = 0.0; + m_integ_angle_x = 0.0; + m_integ_angle_y = 0.0; + m_integ_angle_z = 0.0; } else { - m_integ_angle += delta_angle; + m_integ_angle_x += delta_angle_x; + m_integ_angle_y += delta_angle_y; + m_integ_angle_z += delta_angle_z; } m_gyro_rate_x = gyro_rate_x; m_gyro_rate_y = gyro_rate_y; @@ -634,7 +662,9 @@ void ADIS16470_IMU::Acquire() { data_remainder = 0; data_to_read = 0; previous_timestamp = 0.0; - delta_angle = 0.0; + delta_angle_x = 0.0; + delta_angle_y = 0.0; + delta_angle_z = 0.0; gyro_rate_x = 0.0; gyro_rate_y = 0.0; gyro_rate_z = 0.0; @@ -696,50 +726,143 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle, return compAngle; } -units::degree_t ADIS16470_IMU::GetAngle() const { - switch (m_yaw_axis) { +void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) { + switch (axis) { + case kYaw: + axis = m_yaw_axis; + break; + case kPitch: + axis = m_pitch_axis; + break; + case kRoll: + axis = m_roll_axis; + break; + default: + break; + } + + switch (axis) { + case kX: + SetGyroAngleX(angle); + break; + case kY: + SetGyroAngleY(angle); + break; + case kZ: + SetGyroAngleZ(angle); + break; + default: + break; + } +} + +void ADIS16470_IMU::SetGyroAngleX(units::degree_t angle) { + std::scoped_lock sync(m_mutex); + m_integ_angle_x = angle.value(); +} + +void ADIS16470_IMU::SetGyroAngleY(units::degree_t angle) { + std::scoped_lock sync(m_mutex); + m_integ_angle_y = angle.value(); +} + +void ADIS16470_IMU::SetGyroAngleZ(units::degree_t angle) { + std::scoped_lock sync(m_mutex); + m_integ_angle_z = angle.value(); +} + +units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const { + switch (axis) { + case kYaw: + axis = m_yaw_axis; + break; + case kPitch: + axis = m_pitch_axis; + break; + case kRoll: + axis = m_roll_axis; + break; + default: + break; + } + + switch (axis) { case kX: if (m_simGyroAngleX) { return units::degree_t{m_simGyroAngleX.Get()}; } - break; + { + std::scoped_lock sync(m_mutex); + return units::degree_t{m_integ_angle_x}; + } case kY: if (m_simGyroAngleY) { return units::degree_t{m_simGyroAngleY.Get()}; } - break; + { + std::scoped_lock sync(m_mutex); + return units::degree_t{m_integ_angle_y}; + } case kZ: if (m_simGyroAngleZ) { return units::degree_t{m_simGyroAngleZ.Get()}; } + { + std::scoped_lock sync(m_mutex); + return units::degree_t{m_integ_angle_z}; + } + default: break; } - std::scoped_lock sync(m_mutex); - return units::degree_t{m_integ_angle}; + + return units::degree_t{0.0}; } -units::degrees_per_second_t ADIS16470_IMU::GetRate() const { - if (m_yaw_axis == kX) { - if (m_simGyroRateX) { - return units::degrees_per_second_t{m_simGyroRateX.Get()}; - } - std::scoped_lock sync(m_mutex); - return units::degrees_per_second_t{m_gyro_rate_x}; - } else if (m_yaw_axis == kY) { - if (m_simGyroRateY) { - return units::degrees_per_second_t{m_simGyroRateY.Get()}; - } - std::scoped_lock sync(m_mutex); - return units::degrees_per_second_t{m_gyro_rate_y}; - } else if (m_yaw_axis == kZ) { - if (m_simGyroRateZ) { - return units::degrees_per_second_t{m_simGyroRateZ.Get()}; - } - std::scoped_lock sync(m_mutex); - return units::degrees_per_second_t{m_gyro_rate_z}; - } else { - return 0_deg_per_s; +units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const { + switch (axis) { + case kYaw: + axis = m_yaw_axis; + break; + case kPitch: + axis = m_pitch_axis; + break; + case kRoll: + axis = m_roll_axis; + break; + default: + break; + } + + switch (axis) { + case kX: + if (m_simGyroRateX) { + return units::degrees_per_second_t{m_simGyroRateX.Get()}; + } + { + std::scoped_lock sync(m_mutex); + return units::degrees_per_second_t{m_gyro_rate_x}; + } + case kY: + if (m_simGyroRateY) { + return units::degrees_per_second_t{m_simGyroRateY.Get()}; + } + { + std::scoped_lock sync(m_mutex); + return units::degrees_per_second_t{m_gyro_rate_y}; + } + case kZ: + if (m_simGyroRateZ) { + return units::degrees_per_second_t{m_simGyroRateZ.Get()}; + } + { + std::scoped_lock sync(m_mutex); + return units::degrees_per_second_t{m_gyro_rate_z}; + } + default: + break; } + + return 0_deg_per_s; } units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const { @@ -790,20 +913,12 @@ ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const { return m_yaw_axis; } -int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) { - if (m_yaw_axis == yaw_axis) { - return 1; - } - if (!SwitchToStandardSPI()) { - REPORT_ERROR("Failed to configure/reconfigure standard SPI."); - return 2; - } - m_yaw_axis = yaw_axis; - if (!SwitchToAutoSPI()) { - REPORT_ERROR("Failed to configure/reconfigure auto SPI."); - return 2; - } - return 0; +ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetPitchAxis() const { + return m_pitch_axis; +} + +ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetRollAxis() const { + return m_roll_axis; } int ADIS16470_IMU::GetPort() const { @@ -819,5 +934,5 @@ int ADIS16470_IMU::GetPort() const { void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) { builder.SetSmartDashboardType("ADIS16470 IMU"); builder.AddDoubleProperty( - "Yaw Angle", [=, this] { return GetAngle().value(); }, nullptr); + "Yaw Angle", [=, this] { return GetAngle(kYaw).value(); }, nullptr); } diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index e3b521c2b33..ad8baa0e048 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -69,38 +69,23 @@ class ADIS16470_IMU : public wpi::Sendable, _64s = 11 }; - enum IMUAxis { kX, kY, kZ }; + enum IMUAxis { kX, kY, kZ, kYaw, kPitch, kRoll }; - /** - * @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis - * is set to the IMU Z axis, and calibration time is defaulted to 4 seconds. - */ ADIS16470_IMU(); + ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis); - /** - * @brief Customizable constructor. Allows the SPI port and CS to be - * customized, the yaw axis used for GetAngle() is adjustable, and initial - * calibration time can be modified. - * - * @param yaw_axis Selects the "default" axis to use for GetAngle() and - * GetRate() - * - * @param port The SPI port and CS where the IMU is connected. - * - * @param cal_time The calibration time that should be used on start-up. - */ - explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, + explicit ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, + IMUAxis roll_axis, frc::SPI::Port port, CalibrationTime cal_time); - /** - * @brief Destructor. Kills the acquisition loop and closes the SPI - * peripheral. - */ ~ADIS16470_IMU() override; ADIS16470_IMU(ADIS16470_IMU&&) = default; ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default; + /** + * @brief Configures the decimation rate of the IMU. + */ int ConfigDecRate(uint16_t reg); /** @@ -116,22 +101,59 @@ class ADIS16470_IMU : public wpi::Sendable, int ConfigCalTime(CalibrationTime new_cal_time); /** - * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations. - * - * Resets the gyro accumulations to a heading of zero. This can be used if - * the "zero" orientation of the sensor needs to be changed in runtime. + * @brief Resets the gyro accumulations to a heading of zero. This can be used + * if the "zero" orientation of the sensor needs to be changed in runtime. */ void Reset(); /** - * Returns the yaw axis angle in degrees (CCW positive). + * Allow the designated gyro angle to be set to a given value. This may happen + * with unread values in the buffer, it is suggested that the IMU is not + * moving when this method is run. + * + * @param axis IMUAxis that will be changed + * @param angle The new angle (CCW positive) + */ + void SetGyroAngle(IMUAxis axis, units::degree_t angle); + + /** + * Allow the gyro angle X to be set to a given value. This may happen with + * unread values in the buffer, it is suggested that the IMU is not moving + * when this method is run. + * + * @param angle The new angle (CCW positive) + */ + void SetGyroAngleX(units::degree_t angle); + + /** + * Allow the gyro angle Y to be set to a given value. This may happen with + * unread values in the buffer, it is suggested that the IMU is not moving + * when this method is run. + * + * @param angle The new angle (CCW positive) + */ + void SetGyroAngleY(units::degree_t angle); + + /** + * Allow the gyro angle Z to be set to a given value. This may happen with + * unread values in the buffer, it is suggested that the IMU is not moving + * when this method is run. + * + * @param angle The new angle (CCW positive) + */ + void SetGyroAngleZ(units::degree_t angle); + + /** + * @param axis The IMUAxis whose angle to return + * @return The axis angle (CCW positive) */ - units::degree_t GetAngle() const; + units::degree_t GetAngle(IMUAxis axis) const; /** - * Returns the yaw axis angular rate in degrees per second (CCW positive). + * @param axis The IMUAxis whose rate to return + * @return Axis angular rate (CCW positive) */ - units::degrees_per_second_t GetRate() const; + units::degrees_per_second_t GetRate(IMUAxis axis) const; /** * Returns the acceleration in the X axis. @@ -148,25 +170,60 @@ class ADIS16470_IMU : public wpi::Sendable, */ units::meters_per_second_squared_t GetAccelZ() const; + /** + * Returns the X-axis complementary angle. + */ units::degree_t GetXComplementaryAngle() const; + /** + * Returns the Y-axis complementary angle. + */ units::degree_t GetYComplementaryAngle() const; + /** + * Returns the X-axis filtered acceleration angle. + */ units::degree_t GetXFilteredAccelAngle() const; + /** + * Returns the Y-axis filtered acceleration angle. + */ units::degree_t GetYFilteredAccelAngle() const; + /** + * Returns which axis, kX, kY, or kZ, is set to the yaw axis. + * + * @return IMUAxis Yaw Axis + */ IMUAxis GetYawAxis() const; - int SetYawAxis(IMUAxis yaw_axis); + /** + * Returns which axis, kX, kY, or kZ, is set to the pitch axis. + * + * @return IMUAxis Pitch Axis + */ + IMUAxis GetPitchAxis() const; + + /** + * Returns which axis, kX, kY, or kZ, is set to the roll axis. + * + * @return IMUAxis Roll Axis + */ + IMUAxis GetRollAxis() const; + /** + * Checks the connection status of the IMU. + * + * @return True if the IMU is connected, false otherwise. + */ bool IsConnected() const; - // IMU yaw axis IMUAxis m_yaw_axis; + IMUAxis m_pitch_axis; + IMUAxis m_roll_axis; /** - * Get the SPI port number. + * Gets the SPI port number. * * @return The SPI port number. */ @@ -175,7 +232,7 @@ class ADIS16470_IMU : public wpi::Sendable, void InitSendable(wpi::SendableBuilder& builder) override; private: - /* ADIS16470 Register Map Declaration */ + // Register Map Declaration static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count static constexpr uint8_t DIAG_STAT = 0x02; // Diagnostic and operational status @@ -276,25 +333,15 @@ class ADIS16470_IMU : public wpi::Sendable, static constexpr uint8_t FLSHCNT_HIGH = 0x7E; // Flash update count, upper word - /* ADIS16470 Auto SPI Data Packets */ - static constexpr uint8_t m_autospi_x_packet[16] = { - X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT, - Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT, - Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT}; - - static constexpr uint8_t m_autospi_y_packet[16] = { - Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT, - Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT, - Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT}; - - static constexpr uint8_t m_autospi_z_packet[16] = { - Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT, - Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT, - Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT}; - - /* ADIS16470 Constants */ - static constexpr double delta_angle_sf = - 2160.0 / 2147483648.0; /* 2160 / (2^31) */ + // Auto SPI Data Packet to read all thrre gyro axes. + static constexpr uint8_t m_autospi_allangle_packet[24] = { + X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, Y_DELTANG_OUT, + FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, Z_DELTANG_OUT, FLASH_CNT, + Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT, Y_GYRO_OUT, + FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT, + Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT}; + + static constexpr double delta_angle_sf = 2160.0 / 2147483648.0; static constexpr double rad_to_deg = 57.2957795; static constexpr double deg_to_rad = 0.0174532; static constexpr double grav = 9.81; @@ -350,8 +397,10 @@ class ADIS16470_IMU : public wpi::Sendable, void Close(); - // Integrated gyro value - double m_integ_angle = 0.0; + // Integrated gyro angles. + double m_integ_angle_x = 0.0; + double m_integ_angle_y = 0.0; + double m_integ_angle_z = 0.0; // Instant raw outputs double m_gyro_rate_x = 0.0; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java index 652cc9325fa..8aec21b74ed 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java @@ -122,45 +122,21 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word - private static final byte[] m_autospi_x_packet = { + // Weight between the previous and current gyro angles represents 1 second for the timestamp, + // this is the point at which we ignore the previous angle because it is too old to be of use. + // The IMU timestamp conversion is 1 = 49.02us, the value 1_000_000 is the number of microseconds + // we average over. + private static final double AVERAGE_RATE_SCALING_FACTOR = 49.02 / 1_000_000; + + private static final byte[] m_autospi_allAngle_packet = { X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, - X_GYRO_OUT, - FLASH_CNT, - Y_GYRO_OUT, - FLASH_CNT, - Z_GYRO_OUT, - FLASH_CNT, - X_ACCL_OUT, - FLASH_CNT, - Y_ACCL_OUT, - FLASH_CNT, - Z_ACCL_OUT, - FLASH_CNT - }; - - private static final byte[] m_autospi_y_packet = { Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, - X_GYRO_OUT, - FLASH_CNT, - Y_GYRO_OUT, - FLASH_CNT, - Z_GYRO_OUT, - FLASH_CNT, - X_ACCL_OUT, - FLASH_CNT, - Y_ACCL_OUT, - FLASH_CNT, - Z_ACCL_OUT, - FLASH_CNT - }; - - private static final byte[] m_autospi_z_packet = { Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, @@ -179,18 +155,31 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable { FLASH_CNT }; + /** Calibration times for the ADIS16470. */ public enum CalibrationTime { + /** 32ms calibration */ _32ms(0), + /** 64ms calibration */ _64ms(1), + /** 128ms calibration */ _128ms(2), + /** 256ms calibration */ _256ms(3), + /** 512ms calibration */ _512ms(4), + /** 1 second calibration */ _1s(5), + /** 2 second calibration */ _2s(6), + /** 4 second calibration */ _4s(7), + /** 8 second calibration */ _8s(8), + /** 16 second calibration */ _16s(9), + /** 32 second calibration */ _32s(10), + /** 64 second calibration */ _64s(11); private final int value; @@ -200,10 +189,25 @@ public enum CalibrationTime { } } + /** + * IMU axes. + * + *

kX, kY, and kZ refer to the IMU's X, Y, and Z axes respectively. kYaw, kPitch, and kRoll are + * configured by the user to refer to an X, Y, or Z axis. + */ public enum IMUAxis { + /** The IMU's X axis */ kX, + /** The IMU's Y axis */ kY, - kZ + /** The IMU's Z axis */ + kZ, + /** The user configured yaw axis */ + kYaw, + /** The user configured pitch axis */ + kPitch, + /** The user configured roll axis */ + kRoll, } // Static Constants @@ -212,19 +216,26 @@ public enum IMUAxis { private static final double deg_to_rad = 0.0174532; private static final double grav = 9.81; - // User-specified yaw axis + // User-specified axes private IMUAxis m_yaw_axis; + private IMUAxis m_pitch_axis; + private IMUAxis m_roll_axis; // Instant raw outputs private double m_gyro_rate_x = 0.0; private double m_gyro_rate_y = 0.0; private double m_gyro_rate_z = 0.0; + private double m_average_gyro_rate_x = 0.0; + private double m_average_gyro_rate_y = 0.0; + private double m_average_gyro_rate_z = 0.0; private double m_accel_x = 0.0; private double m_accel_y = 0.0; private double m_accel_z = 0.0; // Integrated gyro angle - private double m_integ_angle = 0.0; + private double m_integ_angle_x = 0.0; + private double m_integ_angle_y = 0.0; + private double m_integ_angle_z = 0.0; // Complementary filter variables private double m_dt = 0.0; @@ -278,18 +289,71 @@ public void run() { } } + /** + * Creates a new ADIS16740 IMU object. The default setup is the onboard SPI port with a + * calibration time of 4 seconds. Yaw, pitch, and roll are kZ, kX, and kY respectively. + */ public ADIS16470_IMU() { - this(IMUAxis.kZ, SPI.Port.kOnboardCS0, CalibrationTime._4s); + this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._4s); } /** + * Creates a new ADIS16740 IMU object. The default setup is the onboard SPI port with a + * calibration time of 4 seconds. + * + *

Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in + * an error. + * * @param yaw_axis The axis that measures the yaw + * @param pitch_axis The axis that measures the pitch + * @param roll_axis The axis that measures the roll + */ + public ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) { + this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._4s); + } + + /** + * Creates a new ADIS16740 IMU object. + * + *

Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in + * an error. + * + * @param yaw_axis The axis that measures the yaw + * @param pitch_axis The axis that measures the pitch + * @param roll_axis The axis that measures the roll * @param port The SPI Port the gyro is plugged into * @param cal_time Calibration time */ @SuppressWarnings("this-escape") - public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) { + public ADIS16470_IMU( + IMUAxis yaw_axis, + IMUAxis pitch_axis, + IMUAxis roll_axis, + SPI.Port port, + CalibrationTime cal_time) { + if (yaw_axis == IMUAxis.kYaw + || yaw_axis == IMUAxis.kPitch + || yaw_axis == IMUAxis.kRoll + || pitch_axis == IMUAxis.kYaw + || pitch_axis == IMUAxis.kPitch + || pitch_axis == IMUAxis.kRoll + || roll_axis == IMUAxis.kYaw + || roll_axis == IMUAxis.kPitch + || roll_axis == IMUAxis.kRoll) { + DriverStation.reportError( + "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or IMUAxis.kZ as arguments.", + false); + DriverStation.reportError( + "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)", false); + yaw_axis = IMUAxis.kZ; + pitch_axis = IMUAxis.kY; + roll_axis = IMUAxis.kX; + } + m_yaw_axis = yaw_axis; + m_pitch_axis = pitch_axis; + m_roll_axis = roll_axis; + m_calibration_time = cal_time.value; m_spi_port = port; @@ -369,6 +433,11 @@ public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) m_connected = true; } + /** + * Checks the connection status of the IMU. + * + * @return True if the IMU is connected, false otherwise. + */ public boolean isConnected() { if (m_simConnected != null) { return m_simConnected.get(); @@ -411,7 +480,7 @@ private static int toInt(int... buf) { /** * Switch to standard SPI mode. * - * @return + * @return True if successful, false otherwise. */ private boolean switchToStandardSPI() { // Check to see whether the acquire thread is active. If so, wait for it to stop @@ -477,7 +546,9 @@ private boolean switchToStandardSPI() { } /** - * @return + * Switch to auto SPI mode. + * + * @return True if successful, false otherwise. */ boolean switchToAutoSPI() { // No SPI port has been set up. Go set one up first. @@ -499,17 +570,7 @@ boolean switchToAutoSPI() { m_auto_configured = true; } // Do we need to change auto SPI settings? - switch (m_yaw_axis) { - case kX: - m_spi.setAutoTransmitData(m_autospi_x_packet, 2); - break; - case kY: - m_spi.setAutoTransmitData(m_autospi_y_packet, 2); - break; - default: - m_spi.setAutoTransmitData(m_autospi_z_packet, 2); - break; - } + m_spi.setAutoTransmitData(m_autospi_allAngle_packet, 2); // Configure auto stall time m_spi.configureAutoStall(5, 1000, 1); // Kick off auto SPI (Note: Device configuration impossible after auto SPI is @@ -561,6 +622,12 @@ public int configCalTime(CalibrationTime new_cal_time) { return 0; } + /** + * Configures the decimation rate of the IMU. + * + * @param reg The new decimation value. + * @return 0 if OK, 2 if error + */ public int configDecRate(int reg) { int m_reg = reg; if (!switchToStandardSPI()) { @@ -568,7 +635,7 @@ public int configDecRate(int reg) { return 2; } if (m_reg > 1999) { - DriverStation.reportError("Attempted to write an invalid deimation value.", false); + DriverStation.reportError("Attempted to write an invalid decimation value.", false); m_reg = 1999; } m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0); @@ -596,28 +663,6 @@ public void calibrate() { } } - /** - * Sets the yaw axis - * - * @param yaw_axis The new yaw axis to use - * @return 1 if the new yaw axis is the same as the current one, 2 if the switch to Standard SPI - * failed, else 0. - */ - public int setYawAxis(IMUAxis yaw_axis) { - if (m_yaw_axis == yaw_axis) { - return 1; - } - if (!switchToStandardSPI()) { - DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); - return 2; - } - m_yaw_axis = yaw_axis; - if (!switchToAutoSPI()) { - DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); - } - return 0; - } - /** * @param reg * @return @@ -650,12 +695,6 @@ private void writeRegister(int reg, int val) { m_spi.write(buf, 2); } - public void reset() { - synchronized (this) { - m_integ_angle = 0.0; - } - } - /** Delete (free) the spi port used for the IMU. */ @Override public void close() { @@ -691,7 +730,7 @@ public void close() { /** */ private void acquire() { // Set data packet length - final int dataset_len = 19; // 18 data points + timestamp + final int dataset_len = 27; // 26 data points + timestamp final int BUFFER_SIZE = 4000; // Set up buffers and variables @@ -700,7 +739,9 @@ private void acquire() { int data_remainder = 0; int data_to_read = 0; double previous_timestamp = 0.0; - double delta_angle = 0.0; + double delta_angle_x = 0.0; + double delta_angle_y = 0.0; + double delta_angle_z = 0.0; double gyro_rate_x = 0.0; double gyro_rate_y = 0.0; double gyro_rate_z = 0.0; @@ -730,8 +771,8 @@ private void acquire() { data_count = m_spi.readAutoReceivedData( - buffer, 0, 0); // Read number of bytes currently stored in the - // buffer + buffer, 0, 0); // Read number of bytes currently stored in the buffer + data_remainder = data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp data_to_read = data_count - data_remainder; // Remove incomplete data from read count @@ -772,18 +813,27 @@ private void acquire() { */ /* - * Get delta angle value for selected yaw axis and scale by the elapsed time + * Get delta angle value for all 3 axes and scale by the elapsed time * (based on timestamp) */ - delta_angle = + delta_angle_x = (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf) / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - gyro_rate_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0); - gyro_rate_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0); - gyro_rate_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0); - accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0); - accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0); - accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0); + delta_angle_y = + (toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf) + / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); + delta_angle_z = + (toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14]) + * delta_angle_sf) + / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); + + gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0); + gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0); + gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0); + + accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0); + accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0); + accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0); // Convert scaled sensor data to SI units (for tilt calculations) // TODO: Should the unit outputs be selectable? @@ -809,6 +859,10 @@ private void acquire() { accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); compAngleX = accelAngleX; compAngleY = accelAngleY; + + m_average_gyro_rate_x = 0.0; + m_average_gyro_rate_y = 0.0; + m_average_gyro_rate_z = 0.0; } else { // Run inclinometer calculations accelAngleX = @@ -830,9 +884,13 @@ private void acquire() { * Don't accumulate first run. previous_timestamp will be "very" old and the * integration will end up way off */ - m_integ_angle = 0.0; + m_integ_angle_x = 0.0; + m_integ_angle_y = 0.0; + m_integ_angle_z = 0.0; } else { - m_integ_angle += delta_angle; + m_integ_angle_x += delta_angle_x; + m_integ_angle_y += delta_angle_y; + m_integ_angle_z += delta_angle_z; } m_gyro_rate_x = gyro_rate_x; m_gyro_rate_y = gyro_rate_y; @@ -844,16 +902,28 @@ private void acquire() { m_compAngleY = compAngleY * rad_to_deg; m_accelAngleX = accelAngleX * rad_to_deg; m_accelAngleY = accelAngleY * rad_to_deg; + m_average_gyro_rate_x += gyro_rate_x; + m_average_gyro_rate_y += gyro_rate_y; + m_average_gyro_rate_z += gyro_rate_z; } m_first_run = false; } + + // The inverse of data to read divided by dataset length, his is the number of iterations + // of the for loop inverted (so multiplication can be used instead of division) + double invTotalIterations = (double) dataset_len / data_to_read; + m_average_gyro_rate_x *= invTotalIterations; + m_average_gyro_rate_y *= invTotalIterations; + m_average_gyro_rate_z *= invTotalIterations; } else { m_thread_idle = true; data_count = 0; data_remainder = 0; data_to_read = 0; previous_timestamp = 0.0; - delta_angle = 0.0; + delta_angle_x = 0.0; + delta_angle_y = 0.0; + delta_angle_z = 0.0; gyro_rate_x = 0.0; gyro_rate_y = 0.0; gyro_rate_z = 0.0; @@ -933,111 +1003,245 @@ private double compFilterProcess(double compAngle, double accelAngle, double ome } /** - * @return Yaw axis angle in degrees (CCW positive) + * Resets the gyro accumulations to a heading of zero. This can be used if the "zero" orientation + * of the sensor needs to be changed in runtime. + */ + public void reset() { + synchronized (this) { + m_integ_angle_x = 0.0; + m_integ_angle_y = 0.0; + m_integ_angle_z = 0.0; + } + } + + /** + * Allow the designated gyro angle to be set to a given value. This may happen with unread values + * in the buffer, it is suggested that the IMU is not moving when this method is run. + * + * @param axis IMUAxis that will be changed + * @param angle A double in degrees (CCW positive) + */ + public void setGyroAngle(IMUAxis axis, double angle) { + switch (axis) { + case kYaw: + axis = m_yaw_axis; + break; + case kPitch: + axis = m_pitch_axis; + break; + case kRoll: + axis = m_roll_axis; + break; + default: + } + + switch (axis) { + case kX: + this.setGyroAngleX(angle); + break; + case kY: + this.setGyroAngleY(angle); + break; + case kZ: + this.setGyroAngleZ(angle); + break; + default: + } + } + + /** + * Allow the gyro angle X to be set to a given value. This may happen with unread values in the + * buffer, it is suggested that the IMU is not moving when this method is run. + * + * @param angle A double in degrees (CCW positive) + */ + public void setGyroAngleX(double angle) { + synchronized (this) { + m_integ_angle_x = angle; + } + } + + /** + * Allow the gyro angle Y to be set to a given value. This may happen with unread values in the + * buffer, it is suggested that the IMU is not moving when this method is run. + * + * @param angle A double in degrees (CCW positive) + */ + public void setGyroAngleY(double angle) { + synchronized (this) { + m_integ_angle_y = angle; + } + } + + /** + * Allow the gyro angle Z to be set to a given value. This may happen with unread values in the + * buffer, it is suggested that the IMU is not moving when this method is run. + * + * @param angle A double in degrees (CCW positive) + */ + public void setGyroAngleZ(double angle) { + synchronized (this) { + m_integ_angle_z = angle; + } + } + + /** + * @param axis The IMUAxis whose angle to return + * @return The axis angle in degrees (CCW positive) */ - public synchronized double getAngle() { - switch (m_yaw_axis) { + public synchronized double getAngle(IMUAxis axis) { + switch (axis) { + case kYaw: + axis = m_yaw_axis; + break; + case kPitch: + axis = m_pitch_axis; + break; + case kRoll: + axis = m_roll_axis; + break; + default: + } + + switch (axis) { case kX: if (m_simGyroAngleX != null) { return m_simGyroAngleX.get(); } - break; + return m_integ_angle_x; case kY: if (m_simGyroAngleY != null) { return m_simGyroAngleY.get(); } - break; + return m_integ_angle_y; case kZ: if (m_simGyroAngleZ != null) { return m_simGyroAngleZ.get(); } - break; + return m_integ_angle_z; + default: } - return m_integ_angle; + + return 0.0; } /** - * @return Yaw axis angular rate in degrees per second (CCW positive) + * @param axis The IMUAxis whose rate to return + * @return Axis angular rate in degrees per second (CCW positive) */ - public synchronized double getRate() { - if (m_yaw_axis == IMUAxis.kX) { - if (m_simGyroRateX != null) { - return m_simGyroRateX.get(); - } - return m_gyro_rate_x; - } else if (m_yaw_axis == IMUAxis.kY) { - if (m_simGyroRateY != null) { - return m_simGyroRateY.get(); - } - return m_gyro_rate_y; - } else if (m_yaw_axis == IMUAxis.kZ) { - if (m_simGyroRateZ != null) { - return m_simGyroRateZ.get(); - } - return m_gyro_rate_z; - } else { - return 0.0; + public synchronized double getRate(IMUAxis axis) { + switch (axis) { + case kYaw: + axis = m_yaw_axis; + break; + case kPitch: + axis = m_pitch_axis; + break; + case kRoll: + axis = m_roll_axis; + break; + default: + } + + switch (axis) { + case kX: + if (m_simGyroRateX != null) { + return m_simGyroRateX.get(); + } + return m_gyro_rate_x; + case kY: + if (m_simGyroRateY != null) { + return m_simGyroRateY.get(); + } + return m_gyro_rate_y; + case kZ: + if (m_simGyroRateZ != null) { + return m_simGyroRateZ.get(); + } + return m_gyro_rate_z; + default: } + return 0.0; } /** - * @return Yaw Axis + * Returns which axis, kX, kY, or kZ, is set to the yaw axis. + * + * @return IMUAxis Yaw Axis */ public IMUAxis getYawAxis() { return m_yaw_axis; } /** - * @return current acceleration in the X axis + * Returns which axis, kX, kY, or kZ, is set to the pitch axis. + * + * @return IMUAxis Pitch Axis + */ + public IMUAxis getPitchAxis() { + return m_pitch_axis; + } + + /** + * Returns which axis, kX, kY, or kZ, is set to the roll axis. + * + * @return IMUAxis Roll Axis + */ + public IMUAxis getRollAxis() { + return m_roll_axis; + } + + /** + * @return The acceleration in the X axis */ public synchronized double getAccelX() { return m_accel_x * 9.81; } /** - * @return current acceleration in the Y axis + * @return The acceleration in the Y axis */ public synchronized double getAccelY() { return m_accel_y * 9.81; } /** - * @return current acceleration in the Z axis + * @return The acceleration in the Z axis */ public synchronized double getAccelZ() { return m_accel_z * 9.81; } /** - * @return X-axis complementary angle + * @return The X-axis complementary angle */ public synchronized double getXComplementaryAngle() { return m_compAngleX; } /** - * @return Y-axis complementary angle + * @return The Y-axis complementary angle */ public synchronized double getYComplementaryAngle() { return m_compAngleY; } /** - * @return X-axis filtered acceleration angle + * @return The X-axis filtered acceleration angle */ public synchronized double getXFilteredAccelAngle() { return m_accelAngleX; } /** - * @return Y-axis filtered acceleration angle + * @return The Y-axis filtered acceleration angle */ public synchronized double getYFilteredAccelAngle() { return m_accelAngleY; } /** - * Get the SPI port number. + * Gets the SPI port number. * * @return The SPI port number. */ @@ -1048,6 +1252,6 @@ public int getPort() { @Override public void initSendable(SendableBuilder builder) { builder.setSmartDashboardType("Gyro"); - builder.addDoubleProperty("Value", this::getAngle, null); + builder.addDoubleProperty("Value", () -> getAngle(m_yaw_axis), null); } }