diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp index 1a3492e16a..b544d30095 100644 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp @@ -81,6 +81,12 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0); m_simGyroAngleZ = m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0); + m_simGyroRateX = + m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0); + m_simGyroRateY = + m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0); + m_simGyroRateZ = + m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0); m_simAccelX = m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0); m_simAccelY = @@ -237,7 +243,7 @@ void ADIS16448_IMU::InitOffsetBuffer(int size) { if (m_offset_buffer != nullptr) { delete[] m_offset_buffer; } - m_offset_buffer = new offset_data[size]; + m_offset_buffer = new OffsetData[size]; // set accumulate count to 0 m_accum_count = 0; @@ -324,23 +330,23 @@ void ADIS16448_IMU::Calibrate() { std::scoped_lock sync(m_mutex); // Calculate the running average int gyroAverageSize = std::min(m_accum_count, m_avg_size); - double m_gyro_accum_x = 0.0; - double m_gyro_accum_y = 0.0; - double m_gyro_accum_z = 0.0; + double accum_gyro_rate_x = 0.0; + double accum_gyro_rate_y = 0.0; + double accum_gyro_rate_z = 0.0; for (int i = 0; i < gyroAverageSize; i++) { - m_gyro_accum_x += m_offset_buffer[i].m_accum_gyro_x; - m_gyro_accum_y += m_offset_buffer[i].m_accum_gyro_y; - m_gyro_accum_z += m_offset_buffer[i].m_accum_gyro_z; + accum_gyro_rate_x += m_offset_buffer[i].gyro_rate_x; + accum_gyro_rate_y += m_offset_buffer[i].gyro_rate_y; + accum_gyro_rate_z += m_offset_buffer[i].gyro_rate_z; } - m_gyro_offset_x = m_gyro_accum_x / gyroAverageSize; - m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize; - m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize; - m_integ_gyro_x = 0.0; - m_integ_gyro_y = 0.0; - m_integ_gyro_z = 0.0; + m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize; + m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize; + m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize; + m_integ_gyro_angle_x = 0.0; + m_integ_gyro_angle_y = 0.0; + m_integ_gyro_angle_z = 0.0; // std::cout << "Avg Size: " << gyroAverageSize << " X off: " << - // m_gyro_offset_x << " Y off: " << m_gyro_offset_y << " Z off: " << - // m_gyro_offset_z << std::endl; + // m_gyro_rate_offset_x << " Y off: " << m_gyro_rate_offset_y << " Z off: " << + // m_gyro_rate_offset_z << std::endl; } /** @@ -384,9 +390,9 @@ void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) { **/ void ADIS16448_IMU::Reset() { std::scoped_lock sync(m_mutex); - m_integ_gyro_x = 0.0; - m_integ_gyro_y = 0.0; - m_integ_gyro_z = 0.0; + m_integ_gyro_angle_x = 0.0; + m_integ_gyro_angle_y = 0.0; + m_integ_gyro_angle_z = 0.0; } void ADIS16448_IMU::Close() { @@ -422,9 +428,6 @@ void ADIS16448_IMU::Acquire() { const int BUFFER_SIZE = 4000; - // struct to store accumulate data - offset_data sample_data; - // This buffer can contain many datasets uint32_t buffer[BUFFER_SIZE]; int data_count = 0; @@ -432,9 +435,9 @@ void ADIS16448_IMU::Acquire() { int data_to_read = 0; int bufferAvgIndex = 0; uint32_t previous_timestamp = 0; - double gyro_x = 0.0; - double gyro_y = 0.0; - double gyro_z = 0.0; + double gyro_rate_x = 0.0; + double gyro_rate_y = 0.0; + double gyro_rate_z = 0.0; double accel_x = 0.0; double accel_y = 0.0; double accel_z = 0.0; @@ -443,9 +446,9 @@ void ADIS16448_IMU::Acquire() { double mag_z = 0.0; double baro = 0.0; double temp = 0.0; - double gyro_x_si = 0.0; - double gyro_y_si = 0.0; - // double gyro_z_si = 0.0; + double gyro_rate_x_si = 0.0; + double gyro_rate_y_si = 0.0; + // double gyro_rate_z_si = 0.0; double accel_x_si = 0.0; double accel_y_si = 0.0; double accel_z_si = 0.0; @@ -502,9 +505,9 @@ void ADIS16448_IMU::Acquire() { // Timestamp is at buffer[i] m_dt = (buffer[i] - previous_timestamp) / 1000000.0; // Split array and scale data - gyro_x = BuffToShort(&buffer[i + 5]) * 0.04; - gyro_y = BuffToShort(&buffer[i + 7]) * 0.04; - gyro_z = BuffToShort(&buffer[i + 9]) * 0.04; + gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04; + gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04; + gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04; accel_x = BuffToShort(&buffer[i + 11]) * 0.833; accel_y = BuffToShort(&buffer[i + 13]) * 0.833; accel_z = BuffToShort(&buffer[i + 15]) * 0.833; @@ -525,9 +528,9 @@ void ADIS16448_IMU::Acquire() { BuffToShort(&buffer[i + 27]) << std::endl; */ // Convert scaled sensor data to SI units - gyro_x_si = gyro_x * deg_to_rad; - gyro_y_si = gyro_y * deg_to_rad; - // gyro_z_si = gyro_z * deg_to_rad; + gyro_rate_x_si = gyro_rate_x * deg_to_rad; + gyro_rate_y_si = gyro_rate_y * deg_to_rad; + // gyro_rate_z_si = gyro_rate_z * deg_to_rad; accel_x_si = accel_x * grav; accel_y_si = accel_y * grav; accel_z_si = accel_z * grav; @@ -554,8 +557,10 @@ void ADIS16448_IMU::Acquire() { (-accel_z_si * -accel_z_si))); accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si); accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si); - compAngleX = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si); - compAngleY = CompFilterProcess(compAngleY, accelAngleY, -gyro_x_si); + compAngleX = + CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); + compAngleY = + CompFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si); } // Update global variables and state @@ -563,27 +568,24 @@ void ADIS16448_IMU::Acquire() { std::scoped_lock sync(m_mutex); // Ignore first, integrated sample if (m_first_run) { - m_integ_gyro_x = 0.0; - m_integ_gyro_y = 0.0; - m_integ_gyro_z = 0.0; + m_integ_gyro_angle_x = 0.0; + m_integ_gyro_angle_y = 0.0; + m_integ_gyro_angle_z = 0.0; } else { // Accumulate gyro for offset calibration - // Build most recent sample data - sample_data.m_accum_gyro_x = gyro_x; - sample_data.m_accum_gyro_y = gyro_y; - sample_data.m_accum_gyro_z = gyro_z; - // Add to buffer + // Add most recent sample data to buffer bufferAvgIndex = m_accum_count % m_avg_size; - m_offset_buffer[bufferAvgIndex] = sample_data; + m_offset_buffer[bufferAvgIndex] = + OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z}; // Increment counter m_accum_count++; } // Don't post accumulated data to the global variables until an // initial gyro offset has been calculated if (!m_start_up_mode) { - m_gyro_x = gyro_x; - m_gyro_y = gyro_y; - m_gyro_z = gyro_z; + m_gyro_rate_x = gyro_rate_x; + m_gyro_rate_y = gyro_rate_y; + m_gyro_rate_z = gyro_rate_z; m_accel_x = accel_x; m_accel_y = accel_y; m_accel_z = accel_z; @@ -598,9 +600,12 @@ void ADIS16448_IMU::Acquire() { m_accelAngleY = accelAngleY * rad_to_deg; // Accumulate gyro for angle integration and publish to global // variables - m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * m_dt; - m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt; - m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt; + m_integ_gyro_angle_x += + (gyro_rate_x - m_gyro_rate_offset_x) * m_dt; + m_integ_gyro_angle_y += + (gyro_rate_y - m_gyro_rate_offset_y) * m_dt; + m_integ_gyro_angle_z += + (gyro_rate_z - m_gyro_rate_offset_z) * m_dt; } } m_first_run = false; @@ -628,9 +633,9 @@ void ADIS16448_IMU::Acquire() { data_remainder = 0; data_to_read = 0; previous_timestamp = 0.0; - gyro_x = 0.0; - gyro_y = 0.0; - gyro_z = 0.0; + gyro_rate_x = 0.0; + gyro_rate_y = 0.0; + gyro_rate_z = 0.0; accel_x = 0.0; accel_y = 0.0; accel_z = 0.0; @@ -639,9 +644,9 @@ void ADIS16448_IMU::Acquire() { mag_z = 0.0; baro = 0.0; temp = 0.0; - gyro_x_si = 0.0; - gyro_y_si = 0.0; - // gyro_z_si = 0.0; + gyro_rate_x_si = 0.0; + gyro_rate_y_si = 0.0; + // gyro_rate_z_si = 0.0; accel_x_si = 0.0; accel_y_si = 0.0; accel_z_si = 0.0; @@ -742,12 +747,25 @@ units::degree_t ADIS16448_IMU::GetAngle() const { } } +units::degrees_per_second_t ADIS16448_IMU::GetRate() const { + switch (m_yaw_axis) { + case kX: + return GetGyroRateX(); + case kY: + return GetGyroRateY(); + case kZ: + return GetGyroRateZ(); + default: + return 0_deg_per_s; + } +} + units::degree_t ADIS16448_IMU::GetGyroAngleX() const { if (m_simGyroAngleX) { return units::degree_t{m_simGyroAngleX.Get()}; } std::scoped_lock sync(m_mutex); - return units::degree_t{m_integ_gyro_x}; + return units::degree_t{m_integ_gyro_angle_x}; } units::degree_t ADIS16448_IMU::GetGyroAngleY() const { @@ -755,7 +773,7 @@ units::degree_t ADIS16448_IMU::GetGyroAngleY() const { return units::degree_t{m_simGyroAngleY.Get()}; } std::scoped_lock sync(m_mutex); - return units::degree_t{m_integ_gyro_y}; + return units::degree_t{m_integ_gyro_angle_y}; } units::degree_t ADIS16448_IMU::GetGyroAngleZ() const { @@ -763,7 +781,31 @@ units::degree_t ADIS16448_IMU::GetGyroAngleZ() const { return units::degree_t{m_simGyroAngleZ.Get()}; } std::scoped_lock sync(m_mutex); - return units::degree_t{m_integ_gyro_z}; + return units::degree_t{m_integ_gyro_angle_z}; +} + +units::degrees_per_second_t ADIS16448_IMU::GetGyroRateX() const { + 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}; +} + +units::degrees_per_second_t ADIS16448_IMU::GetGyroRateY() const { + 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}; +} + +units::degrees_per_second_t ADIS16448_IMU::GetGyroRateZ() const { + 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}; } units::meters_per_second_squared_t ADIS16448_IMU::GetAccelX() const { diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp index 0539609178..3efccc28ee 100644 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp @@ -79,6 +79,12 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0); m_simGyroAngleZ = m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0); + m_simGyroRateX = + m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0); + m_simGyroRateY = + m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0); + m_simGyroRateZ = + m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0); m_simAccelX = m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0); m_simAccelY = @@ -499,15 +505,15 @@ void ADIS16470_IMU::Acquire() { int data_to_read = 0; uint32_t previous_timestamp = 0; double delta_angle = 0.0; - double gyro_x = 0.0; - double gyro_y = 0.0; - double gyro_z = 0.0; + double gyro_rate_x = 0.0; + double gyro_rate_y = 0.0; + double gyro_rate_z = 0.0; double accel_x = 0.0; double accel_y = 0.0; double accel_z = 0.0; - double gyro_x_si = 0.0; - double gyro_y_si = 0.0; - // double gyro_z_si = 0.0; + double gyro_rate_x_si = 0.0; + double gyro_rate_y_si = 0.0; + // double gyro_rate_z_si = 0.0; double accel_x_si = 0.0; double accel_y_si = 0.0; double accel_z_si = 0.0; @@ -561,17 +567,17 @@ void ADIS16470_IMU::Acquire() { * time (based on timestamp) */ delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); - gyro_x = (BuffToShort(&buffer[i + 7]) / 10.0); - gyro_y = (BuffToShort(&buffer[i + 9]) / 10.0); - gyro_z = (BuffToShort(&buffer[i + 11]) / 10.0); + 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); // Convert scaled sensor data to SI units - gyro_x_si = gyro_x * deg_to_rad; - gyro_y_si = gyro_y * deg_to_rad; - // gyro_z_si = gyro_z * deg_to_rad; + gyro_rate_x_si = gyro_rate_x * deg_to_rad; + gyro_rate_y_si = gyro_rate_y * deg_to_rad; + // gyro_rate_z_si = gyro_rate_z * deg_to_rad; accel_x_si = accel_x * grav; accel_y_si = accel_y * grav; accel_z_si = accel_z * grav; @@ -602,8 +608,10 @@ void ADIS16470_IMU::Acquire() { (accel_z_si * accel_z_si))); accelAngleX = FormatAccelRange(accelAngleX, accel_z_si); accelAngleY = FormatAccelRange(accelAngleY, accel_z_si); - compAngleX = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si); - compAngleY = CompFilterProcess(compAngleY, accelAngleY, gyro_x_si); + compAngleX = + CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); + compAngleY = + CompFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si); } // DEBUG: Print accumulated values @@ -619,9 +627,9 @@ void ADIS16470_IMU::Acquire() { } else { m_integ_angle += delta_angle; } - m_gyro_x = gyro_x; - m_gyro_y = gyro_y; - m_gyro_z = gyro_z; + m_gyro_rate_x = gyro_rate_x; + m_gyro_rate_y = gyro_rate_y; + m_gyro_rate_z = gyro_rate_z; m_accel_x = accel_x; m_accel_y = accel_y; m_accel_z = accel_z; @@ -639,15 +647,15 @@ void ADIS16470_IMU::Acquire() { data_to_read = 0; previous_timestamp = 0.0; delta_angle = 0.0; - gyro_x = 0.0; - gyro_y = 0.0; - gyro_z = 0.0; + gyro_rate_x = 0.0; + gyro_rate_y = 0.0; + gyro_rate_z = 0.0; accel_x = 0.0; accel_y = 0.0; accel_z = 0.0; - gyro_x_si = 0.0; - gyro_y_si = 0.0; - // gyro_z_si = 0.0; + gyro_rate_x_si = 0.0; + gyro_rate_y_si = 0.0; + // gyro_rate_z_si = 0.0; accel_x_si = 0.0; accel_y_si = 0.0; accel_z_si = 0.0; @@ -700,17 +708,6 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle, return compAngle; } -/** - * @brief Returns the current integrated angle for the axis specified. - * - * @param m_yaw_axis An enum indicating the axis chosen to act as the yaw axis. - * - * @return The current integrated angle in degrees. - * - * This function returns the most recent integrated angle for the axis chosen by - *m_yaw_axis. This function is most useful in situations where the yaw axis may - *not coincide with the IMU Z axis. - **/ units::degree_t ADIS16470_IMU::GetAngle() const { switch (m_yaw_axis) { case kX: @@ -733,6 +730,30 @@ units::degree_t ADIS16470_IMU::GetAngle() const { return units::degree_t{m_integ_angle}; } +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::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const { if (m_simAccelX) { return units::meters_per_second_squared_t{m_simAccelX.Get()}; diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp index 6687252dda..46c412a4b1 100644 --- a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp @@ -14,6 +14,9 @@ ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) { m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x"); m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y"); m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z"); + m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x"); + m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y"); + m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z"); m_simAccelX = deviceSim.GetDouble("accel_x"); m_simAccelY = deviceSim.GetDouble("accel_y"); m_simAccelZ = deviceSim.GetDouble("accel_z"); @@ -31,6 +34,18 @@ void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) { m_simGyroAngleZ.Set(angle.value()); } +void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) { + m_simGyroRateX.Set(angularRate.value()); +} + +void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) { + m_simGyroRateY.Set(angularRate.value()); +} + +void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) { + m_simGyroRateZ.Set(angularRate.value()); +} + void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) { m_simAccelX.Set(accel.value()); } diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp index 5fb5e0f3c2..40c09c3626 100644 --- a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp @@ -14,6 +14,9 @@ ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) { m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x"); m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y"); m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z"); + m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x"); + m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y"); + m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z"); m_simAccelX = deviceSim.GetDouble("accel_x"); m_simAccelY = deviceSim.GetDouble("accel_y"); m_simAccelZ = deviceSim.GetDouble("accel_z"); @@ -31,6 +34,18 @@ void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) { m_simGyroAngleZ.Set(angle.value()); } +void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) { + m_simGyroRateX.Set(angularRate.value()); +} + +void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) { + m_simGyroRateY.Set(angularRate.value()); +} + +void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) { + m_simGyroRateZ.Set(angularRate.value()); +} + void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) { m_simAccelX.Set(accel.value()); } diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 6dff72f68c..d4f1b8e896 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -133,6 +133,11 @@ class ADIS16448_IMU : public nt::NTSendable, */ units::degree_t GetAngle() const; + /** + * Returns the yaw axis angular rate in degrees per second (CCW positive). + */ + units::degrees_per_second_t GetRate() const; + /** * Returns the accumulated gyro angle in the X axis. */ @@ -148,6 +153,21 @@ class ADIS16448_IMU : public nt::NTSendable, */ units::degree_t GetGyroAngleZ() const; + /** + * Returns the angular rate in the X axis. + */ + units::degrees_per_second_t GetGyroRateX() const; + + /** + * Returns the angular rate in the Y axis. + */ + units::degrees_per_second_t GetGyroRateY() const; + + /** + * Returns the angular rate in the Z axis. + */ + units::degrees_per_second_t GetGyroRateZ() const; + /** * Returns the acceleration in the X axis. */ @@ -259,10 +279,10 @@ class ADIS16448_IMU : public nt::NTSendable, static constexpr double grav = 9.81; /** @brief struct to store offset data */ - struct offset_data { - double m_accum_gyro_x = 0.0; - double m_accum_gyro_y = 0.0; - double m_accum_gyro_z = 0.0; + struct OffsetData { + double gyro_rate_x = 0.0; + double gyro_rate_y = 0.0; + double gyro_rate_z = 0.0; }; bool SwitchToStandardSPI(); @@ -281,9 +301,9 @@ class ADIS16448_IMU : public nt::NTSendable, IMUAxis m_yaw_axis; // Last read values (post-scaling) - double m_gyro_x = 0.0; - double m_gyro_y = 0.0; - double m_gyro_z = 0.0; + double m_gyro_rate_x = 0.0; + double m_gyro_rate_y = 0.0; + double m_gyro_rate_z = 0.0; double m_accel_x = 0.0; double m_accel_y = 0.0; double m_accel_z = 0.0; @@ -299,11 +319,11 @@ class ADIS16448_IMU : public nt::NTSendable, double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0; // vector for storing most recent imu values - offset_data* m_offset_buffer = nullptr; + OffsetData* m_offset_buffer = nullptr; - double m_gyro_offset_x = 0.0; - double m_gyro_offset_y = 0.0; - double m_gyro_offset_z = 0.0; + double m_gyro_rate_offset_x = 0.0; + double m_gyro_rate_offset_y = 0.0; + double m_gyro_rate_offset_z = 0.0; // function to re-init offset buffer void InitOffsetBuffer(int size); @@ -313,9 +333,9 @@ class ADIS16448_IMU : public nt::NTSendable, int m_accum_count = 0; // Integrated gyro values - double m_integ_gyro_x = 0.0; - double m_integ_gyro_y = 0.0; - double m_integ_gyro_z = 0.0; + double m_integ_gyro_angle_x = 0.0; + double m_integ_gyro_angle_y = 0.0; + double m_integ_gyro_angle_z = 0.0; // Complementary filter functions double FormatFastConverge(double compAngle, double accAngle); @@ -344,6 +364,9 @@ class ADIS16448_IMU : public nt::NTSendable, hal::SimDouble m_simGyroAngleX; hal::SimDouble m_simGyroAngleY; hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; hal::SimDouble m_simAccelX; hal::SimDouble m_simAccelY; hal::SimDouble m_simAccelZ; diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index 3c88bb6069..c7052d7b94 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -127,6 +127,11 @@ class ADIS16470_IMU : public nt::NTSendable, */ units::degree_t GetAngle() const; + /** + * Returns the yaw axis angular rate in degrees per second (CCW positive). + */ + units::degrees_per_second_t GetRate() const; + /** * Returns the acceleration in the X axis. */ @@ -342,7 +347,12 @@ class ADIS16470_IMU : public nt::NTSendable, double m_integ_angle = 0.0; // Instant raw outputs - double m_gyro_x, m_gyro_y, m_gyro_z, m_accel_x, m_accel_y, m_accel_z = 0.0; + double m_gyro_rate_x = 0.0; + double m_gyro_rate_y = 0.0; + double m_gyro_rate_z = 0.0; + double m_accel_x = 0.0; + double m_accel_y = 0.0; + double m_accel_z = 0.0; // Complementary filter variables double m_tau = 1.0; @@ -375,6 +385,9 @@ class ADIS16470_IMU : public nt::NTSendable, hal::SimDouble m_simGyroAngleX; hal::SimDouble m_simGyroAngleY; hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; hal::SimDouble m_simAccelX; hal::SimDouble m_simAccelY; hal::SimDouble m_simAccelZ; diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h index 4cb1d91fa9..96186954ad 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h @@ -48,6 +48,27 @@ class ADIS16448_IMUSim { */ void SetGyroAngleZ(units::degree_t angle); + /** + * Sets the X axis angular rate (CCW positive). + * + * @param angularRate The angular rate. + */ + void SetGyroRateX(units::degrees_per_second_t angularRate); + + /** + * Sets the Y axis angular rate (CCW positive). + * + * @param angularRate The angular rate. + */ + void SetGyroRateY(units::degrees_per_second_t angularRate); + + /** + * Sets the Z axis angular rate (CCW positive). + * + * @param angularRate The angular rate. + */ + void SetGyroRateZ(units::degrees_per_second_t angularRate); + /** * Sets the X axis acceleration. * @@ -73,6 +94,9 @@ class ADIS16448_IMUSim { hal::SimDouble m_simGyroAngleX; hal::SimDouble m_simGyroAngleY; hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; hal::SimDouble m_simAccelX; hal::SimDouble m_simAccelY; hal::SimDouble m_simAccelZ; diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h index 00d245d39b..7b43762b50 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h @@ -48,6 +48,27 @@ class ADIS16470_IMUSim { */ void SetGyroAngleZ(units::degree_t angle); + /** + * Sets the X axis angular rate (CCW positive). + * + * @param angularRate The angular rate. + */ + void SetGyroRateX(units::degrees_per_second_t angularRate); + + /** + * Sets the Y axis angular rate (CCW positive). + * + * @param angularRate The angular rate. + */ + void SetGyroRateY(units::degrees_per_second_t angularRate); + + /** + * Sets the Z axis angular rate (CCW positive). + * + * @param angularRate The angular rate. + */ + void SetGyroRateZ(units::degrees_per_second_t angularRate); + /** * Sets the X axis acceleration. * @@ -73,6 +94,9 @@ class ADIS16470_IMUSim { hal::SimDouble m_simGyroAngleX; hal::SimDouble m_simGyroAngleY; hal::SimDouble m_simGyroAngleZ; + hal::SimDouble m_simGyroRateX; + hal::SimDouble m_simGyroRateY; + hal::SimDouble m_simGyroRateZ; hal::SimDouble m_simAccelX; hal::SimDouble m_simAccelY; hal::SimDouble m_simAccelZ; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java index b11072d2a7..ca6b52d3b3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java @@ -129,14 +129,14 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { private IMUAxis m_yaw_axis; /* Offset data storage */ - private double m_accum_gyro_x[]; - private double m_accum_gyro_y[]; - private double m_accum_gyro_z[]; + private double m_offset_data_gyro_rate_x[]; + private double m_offset_data_gyro_rate_y[]; + private double m_offset_data_gyro_rate_z[]; /* Instant raw output variables */ - private double m_gyro_x = 0.0; - private double m_gyro_y = 0.0; - private double m_gyro_z = 0.0; + 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_accel_x = 0.0; private double m_accel_y = 0.0; private double m_accel_z = 0.0; @@ -147,16 +147,16 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { private double m_temp = 0.0; /* IMU gyro offset variables */ - private double m_gyro_offset_x = 0.0; - private double m_gyro_offset_y = 0.0; - private double m_gyro_offset_z = 0.0; + private double m_gyro_rate_offset_x = 0.0; + private double m_gyro_rate_offset_y = 0.0; + private double m_gyro_rate_offset_z = 0.0; private int m_avg_size = 0; private int m_accum_count = 0; /* Integrated gyro angle variables */ - private double m_integ_gyro_x = 0.0; - private double m_integ_gyro_y = 0.0; - private double m_integ_gyro_z = 0.0; + private double m_integ_gyro_angle_x = 0.0; + private double m_integ_gyro_angle_y = 0.0; + private double m_integ_gyro_angle_z = 0.0; /* Complementary filter variables */ private double m_dt = 0.0; @@ -188,6 +188,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { private SimDouble m_simGyroAngleX; private SimDouble m_simGyroAngleY; private SimDouble m_simGyroAngleZ; + private SimDouble m_simGyroRateX; + private SimDouble m_simGyroRateY; + private SimDouble m_simGyroRateZ; private SimDouble m_simAccelX; private SimDouble m_simAccelY; private SimDouble m_simAccelZ; @@ -262,6 +265,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); + m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); + m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); + m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); @@ -532,9 +538,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { m_avg_size = size; synchronized (this) { // Resize vector - m_accum_gyro_x = new double[size]; - m_accum_gyro_y = new double[size]; - m_accum_gyro_z = new double[size]; + m_offset_data_gyro_rate_x = new double[size]; + m_offset_data_gyro_rate_y = new double[size]; + m_offset_data_gyro_rate_z = new double[size]; // Set acculumate count to 0 m_accum_count = 0; } @@ -548,22 +554,23 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { public void calibrate() { synchronized (this) { int gyroAverageSize = Math.min(m_accum_count, m_avg_size); - double m_gyro_accum_x = 0.0; - double m_gyro_accum_y = 0.0; - double m_gyro_accum_z = 0.0; + double accum_gyro_rate_x = 0.0; + double accum_gyro_rate_y = 0.0; + double accum_gyro_rate_z = 0.0; for (int i = 0; i < gyroAverageSize; i++) { - m_gyro_accum_x += m_accum_gyro_x[i]; - m_gyro_accum_y += m_accum_gyro_y[i]; - m_gyro_accum_z += m_accum_gyro_z[i]; + accum_gyro_rate_x += m_offset_data_gyro_rate_x[i]; + accum_gyro_rate_y += m_offset_data_gyro_rate_y[i]; + accum_gyro_rate_z += m_offset_data_gyro_rate_z[i]; } - m_gyro_offset_x = m_gyro_accum_x / gyroAverageSize; - m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize; - m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize; - m_integ_gyro_x = 0.0; - m_integ_gyro_y = 0.0; - m_integ_gyro_z = 0.0; + m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize; + m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize; + m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize; + m_integ_gyro_angle_x = 0.0; + m_integ_gyro_angle_y = 0.0; + m_integ_gyro_angle_z = 0.0; // System.out.println("Avg Size: " + gyroAverageSize + "X Off: " + - // m_gyro_offset_x + "Y Off: " + m_gyro_offset_y + "Z Off: " + m_gyro_offset_z); + // m_gyro_rate_offset_x + "Y Off: " + m_gyro_rate_offset_y + "Z Off: " + + // m_gyro_rate_offset_z); } } @@ -610,9 +617,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { /** {@inheritDoc} */ public void reset() { synchronized (this) { - m_integ_gyro_x = 0.0; - m_integ_gyro_y = 0.0; - m_integ_gyro_z = 0.0; + m_integ_gyro_angle_x = 0.0; + m_integ_gyro_angle_y = 0.0; + m_integ_gyro_angle_z = 0.0; } } @@ -658,9 +665,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { int bufferAvgIndex = 0; double previous_timestamp = 0.0; double delta_angle = 0.0; - double gyro_x = 0.0; - double gyro_y = 0.0; - double gyro_z = 0.0; + double gyro_rate_x = 0.0; + double gyro_rate_y = 0.0; + double gyro_rate_z = 0.0; double accel_x = 0.0; double accel_y = 0.0; double accel_z = 0.0; @@ -669,9 +676,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { double mag_z = 0.0; double baro = 0.0; double temp = 0.0; - double gyro_x_si = 0.0; - double gyro_y_si = 0.0; - double gyro_z_si = 0.0; + double gyro_rate_x_si = 0.0; + double gyro_rate_y_si = 0.0; + double gyro_rate_z_si = 0.0; double accel_x_si = 0.0; double accel_y_si = 0.0; double accel_z_si = 0.0; @@ -728,9 +735,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0; // Scale sensor data - gyro_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04); - gyro_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04); - gyro_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04); + gyro_rate_x = (toShort(buffer[i + 5], buffer[i + 6]) * 0.04); + gyro_rate_y = (toShort(buffer[i + 7], buffer[i + 8]) * 0.04); + gyro_rate_z = (toShort(buffer[i + 9], buffer[i + 10]) * 0.04); accel_x = (toShort(buffer[i + 11], buffer[i + 12]) * 0.833); accel_y = (toShort(buffer[i + 13], buffer[i + 14]) * 0.833); accel_z = (toShort(buffer[i + 15], buffer[i + 16]) * 0.833); @@ -742,9 +749,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { // Convert scaled sensor data to SI units (for tilt calculations) // TODO: Should the unit outputs be selectable? - gyro_x_si = gyro_x * deg_to_rad; - gyro_y_si = gyro_y * deg_to_rad; - gyro_z_si = gyro_z * deg_to_rad; + gyro_rate_x_si = gyro_rate_x * deg_to_rad; + gyro_rate_y_si = gyro_rate_y * deg_to_rad; + gyro_rate_z_si = gyro_rate_z * deg_to_rad; accel_x_si = accel_x * grav; accel_y_si = accel_y * grav; accel_z_si = accel_z * grav; @@ -777,31 +784,31 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si))); accelAngleX = formatAccelRange(accelAngleX, -accel_z_si); accelAngleY = formatAccelRange(accelAngleY, -accel_z_si); - compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_y_si); - compAngleY = compFilterProcess(compAngleY, accelAngleY, -gyro_x_si); + compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); + compAngleY = compFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si); } // Update global variables and state synchronized (this) { // Ignore first, integrated sample if (m_first_run) { - m_integ_gyro_x = 0.0; - m_integ_gyro_y = 0.0; - m_integ_gyro_z = 0.0; + m_integ_gyro_angle_x = 0.0; + m_integ_gyro_angle_y = 0.0; + m_integ_gyro_angle_z = 0.0; } else { // Accumulate gyro for offset calibration // Add to buffer bufferAvgIndex = m_accum_count % m_avg_size; - m_accum_gyro_x[bufferAvgIndex] = gyro_x; - m_accum_gyro_y[bufferAvgIndex] = gyro_y; - m_accum_gyro_z[bufferAvgIndex] = gyro_z; + m_offset_data_gyro_rate_x[bufferAvgIndex] = gyro_rate_x; + m_offset_data_gyro_rate_y[bufferAvgIndex] = gyro_rate_y; + m_offset_data_gyro_rate_z[bufferAvgIndex] = gyro_rate_z; // Increment counter m_accum_count++; } if (!m_start_up_mode) { - m_gyro_x = gyro_x; - m_gyro_y = gyro_y; - m_gyro_z = gyro_z; + m_gyro_rate_x = gyro_rate_x; + m_gyro_rate_y = gyro_rate_y; + m_gyro_rate_z = gyro_rate_z; m_accel_x = accel_x; m_accel_y = accel_y; m_accel_z = accel_z; @@ -815,9 +822,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { m_accelAngleX = accelAngleX * rad_to_deg; m_accelAngleY = accelAngleY * rad_to_deg; // Accumulate gyro for angle integration and publish to global variables - m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * m_dt; - m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt; - m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt; + m_integ_gyro_angle_x += (gyro_rate_x - m_gyro_rate_offset_x) * m_dt; + m_integ_gyro_angle_y += (gyro_rate_y - m_gyro_rate_offset_y) * m_dt; + m_integ_gyro_angle_z += (gyro_rate_z - m_gyro_rate_offset_z) * m_dt; } // System.out.println("Good CRC"); } @@ -853,9 +860,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { data_to_read = 0; previous_timestamp = 0.0; delta_angle = 0.0; - gyro_x = 0.0; - gyro_y = 0.0; - gyro_z = 0.0; + gyro_rate_x = 0.0; + gyro_rate_y = 0.0; + gyro_rate_z = 0.0; accel_x = 0.0; accel_y = 0.0; accel_z = 0.0; @@ -864,9 +871,9 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { mag_z = 0.0; baro = 0.0; temp = 0.0; - gyro_x_si = 0.0; - gyro_y_si = 0.0; - gyro_z_si = 0.0; + gyro_rate_x_si = 0.0; + gyro_rate_y_si = 0.0; + gyro_rate_z_si = 0.0; accel_x_si = 0.0; accel_y_si = 0.0; accel_z_si = 0.0; @@ -950,6 +957,20 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { } } + /** @return Yaw axis angular rate in degrees per second (CCW positive) */ + public synchronized double getRate() { + switch (m_yaw_axis) { + case kX: + return getGyroAngleX(); + case kY: + return getGyroAngleY(); + case kZ: + return getGyroAngleZ(); + default: + return 0.0; + } + } + /** @return Yaw Axis */ public IMUAxis getYawAxis() { return m_yaw_axis; @@ -960,7 +981,7 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { if (m_simGyroAngleX != null) { return m_simGyroAngleX.get(); } - return m_integ_gyro_x; + return m_integ_gyro_angle_x; } /** @return accumulated gyro angle in the Y axis in degrees */ @@ -968,7 +989,7 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { if (m_simGyroAngleY != null) { return m_simGyroAngleY.get(); } - return m_integ_gyro_y; + return m_integ_gyro_angle_y; } /** @return accumulated gyro angle in the Z axis in degrees */ @@ -976,7 +997,31 @@ public class ADIS16448_IMU implements AutoCloseable, NTSendable { if (m_simGyroAngleZ != null) { return m_simGyroAngleZ.get(); } - return m_integ_gyro_z; + return m_integ_gyro_angle_z; + } + + /** @return gyro angular rate in the X axis in degrees per second */ + public synchronized double getGyroRateX() { + if (m_simGyroRateX != null) { + return m_simGyroRateX.get(); + } + return m_gyro_rate_x; + } + + /** @return gyro angular rate in the Y axis in degrees per second */ + public synchronized double getGyroRateY() { + if (m_simGyroRateY != null) { + return m_simGyroRateY.get(); + } + return m_gyro_rate_y; + } + + /** @return gyro angular rate in the Z axis in degrees per second */ + public synchronized double getGyroRateZ() { + if (m_simGyroRateZ != null) { + return m_simGyroRateZ.get(); + } + return m_gyro_rate_z; } /** @return urrent acceleration in the X axis in meters per second squared */ 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 2b840f0745..8bafa1a237 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 @@ -215,9 +215,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { private IMUAxis m_yaw_axis; // Instant raw outputs - private double m_gyro_x = 0.0; - private double m_gyro_y = 0.0; - private double m_gyro_z = 0.0; + 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_accel_x = 0.0; private double m_accel_y = 0.0; private double m_accel_z = 0.0; @@ -255,6 +255,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { private SimDouble m_simGyroAngleX; private SimDouble m_simGyroAngleY; private SimDouble m_simGyroAngleZ; + private SimDouble m_simGyroRateX; + private SimDouble m_simGyroRateY; + private SimDouble m_simGyroRateZ; private SimDouble m_simAccelX; private SimDouble m_simAccelY; private SimDouble m_simAccelZ; @@ -293,6 +296,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); + m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); + m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); + m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); @@ -681,15 +687,15 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { int data_to_read = 0; double previous_timestamp = 0.0; double delta_angle = 0.0; - double gyro_x = 0.0; - double gyro_y = 0.0; - double gyro_z = 0.0; + double gyro_rate_x = 0.0; + double gyro_rate_y = 0.0; + double gyro_rate_z = 0.0; double accel_x = 0.0; double accel_y = 0.0; double accel_z = 0.0; - double gyro_x_si = 0.0; - double gyro_y_si = 0.0; - double gyro_z_si = 0.0; + double gyro_rate_x_si = 0.0; + double gyro_rate_y_si = 0.0; + double gyro_rate_z_si = 0.0; double accel_x_si = 0.0; double accel_y_si = 0.0; double accel_z_si = 0.0; @@ -758,18 +764,18 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { delta_angle = (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_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0); - gyro_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0); - gyro_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0); + 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); // Convert scaled sensor data to SI units (for tilt calculations) // TODO: Should the unit outputs be selectable? - gyro_x_si = gyro_x * deg_to_rad; - gyro_y_si = gyro_y * deg_to_rad; - gyro_z_si = gyro_z * deg_to_rad; + gyro_rate_x_si = gyro_rate_x * deg_to_rad; + gyro_rate_y_si = gyro_rate_y * deg_to_rad; + gyro_rate_z_si = gyro_rate_z * deg_to_rad; accel_x_si = accel_x * grav; accel_y_si = accel_y * grav; accel_z_si = accel_z * grav; @@ -799,8 +805,8 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); accelAngleX = formatAccelRange(accelAngleX, accel_z_si); accelAngleY = formatAccelRange(accelAngleY, accel_z_si); - compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_y_si); - compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_x_si); + compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); + compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si); } synchronized (this) { @@ -814,9 +820,9 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { } else { m_integ_angle += delta_angle; } - m_gyro_x = gyro_x; - m_gyro_y = gyro_y; - m_gyro_z = gyro_z; + m_gyro_rate_x = gyro_rate_x; + m_gyro_rate_y = gyro_rate_y; + m_gyro_rate_z = gyro_rate_z; m_accel_x = accel_x; m_accel_y = accel_y; m_accel_z = accel_z; @@ -834,15 +840,15 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { data_to_read = 0; previous_timestamp = 0.0; delta_angle = 0.0; - gyro_x = 0.0; - gyro_y = 0.0; - gyro_z = 0.0; + gyro_rate_x = 0.0; + gyro_rate_y = 0.0; + gyro_rate_z = 0.0; accel_x = 0.0; accel_y = 0.0; accel_z = 0.0; - gyro_x_si = 0.0; - gyro_y_si = 0.0; - gyro_z_si = 0.0; + gyro_rate_x_si = 0.0; + gyro_rate_y_si = 0.0; + gyro_rate_z_si = 0.0; accel_x_si = 0.0; accel_y_si = 0.0; accel_z_si = 0.0; @@ -934,6 +940,28 @@ public class ADIS16470_IMU implements AutoCloseable, NTSendable { return m_integ_angle; } + /** @return Yaw 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; + } + } + /** @return Yaw Axis */ public IMUAxis getYawAxis() { return m_yaw_axis; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java index d8c96ed7fa..584a2ec671 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java @@ -13,6 +13,9 @@ public class ADIS16448_IMUSim { private final SimDouble m_simGyroAngleX; private final SimDouble m_simGyroAngleY; private final SimDouble m_simGyroAngleZ; + private final SimDouble m_simGyroRateX; + private final SimDouble m_simGyroRateY; + private final SimDouble m_simGyroRateZ; private final SimDouble m_simAccelX; private final SimDouble m_simAccelY; private final SimDouble m_simAccelZ; @@ -27,6 +30,9 @@ public class ADIS16448_IMUSim { m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x"); m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y"); m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z"); + m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x"); + m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y"); + m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z"); m_simAccelX = wrappedSimDevice.getDouble("accel_x"); m_simAccelY = wrappedSimDevice.getDouble("accel_y"); m_simAccelZ = wrappedSimDevice.getDouble("accel_z"); @@ -59,6 +65,33 @@ public class ADIS16448_IMUSim { m_simGyroAngleZ.set(angleDegrees); } + /** + * Sets the X axis angle in degrees per second (CCW positive). + * + * @param angularRateDegreesPerSecond The angular rate. + */ + public void setGyroRateX(double angularRateDegreesPerSecond) { + m_simGyroRateX.set(angularRateDegreesPerSecond); + } + + /** + * Sets the Y axis angle in degrees per second (CCW positive). + * + * @param angularRateDegreesPerSecond The angular rate. + */ + public void setGyroRateY(double angularRateDegreesPerSecond) { + m_simGyroRateY.set(angularRateDegreesPerSecond); + } + + /** + * Sets the Z axis angle in degrees per second (CCW positive). + * + * @param angularRateDegreesPerSecond The angular rate. + */ + public void setGyroRateZ(double angularRateDegreesPerSecond) { + m_simGyroRateZ.set(angularRateDegreesPerSecond); + } + /** * Sets the X axis acceleration in meters per second squared. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java index 53da546259..eb8ceb864e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java @@ -13,6 +13,9 @@ public class ADIS16470_IMUSim { private final SimDouble m_simGyroAngleX; private final SimDouble m_simGyroAngleY; private final SimDouble m_simGyroAngleZ; + private final SimDouble m_simGyroRateX; + private final SimDouble m_simGyroRateY; + private final SimDouble m_simGyroRateZ; private final SimDouble m_simAccelX; private final SimDouble m_simAccelY; private final SimDouble m_simAccelZ; @@ -27,6 +30,9 @@ public class ADIS16470_IMUSim { m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x"); m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y"); m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z"); + m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x"); + m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y"); + m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z"); m_simAccelX = wrappedSimDevice.getDouble("accel_x"); m_simAccelY = wrappedSimDevice.getDouble("accel_y"); m_simAccelZ = wrappedSimDevice.getDouble("accel_z"); @@ -59,6 +65,33 @@ public class ADIS16470_IMUSim { m_simGyroAngleZ.set(angleDegrees); } + /** + * Sets the X axis angle in degrees per second (CCW positive). + * + * @param angularRateDegreesPerSecond The angular rate. + */ + public void setGyroRateX(double angularRateDegreesPerSecond) { + m_simGyroRateX.set(angularRateDegreesPerSecond); + } + + /** + * Sets the Y axis angle in degrees per second (CCW positive). + * + * @param angularRateDegreesPerSecond The angular rate. + */ + public void setGyroRateY(double angularRateDegreesPerSecond) { + m_simGyroRateY.set(angularRateDegreesPerSecond); + } + + /** + * Sets the Z axis angle in degrees per second (CCW positive). + * + * @param angularRateDegreesPerSecond The angular rate. + */ + public void setGyroRateZ(double angularRateDegreesPerSecond) { + m_simGyroRateZ.set(angularRateDegreesPerSecond); + } + /** * Sets the X axis acceleration in meters per second squared. *