Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add GetRate() to ADIS classes (#3864)

Browse Source 
The angular rate is treated somewhat like an angle during calibration,
but the datasheet says it's angular rate. The variables were renamed to
make this clearer.
This commit is contained in:
Tyler Veness
2022-01-04 22:26:23 -08:00
committed by GitHub
parent 05d66f862d
commit 22c4da152e
12 changed files with 516 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

160
wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
View File

@@ -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 {

89
wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
View File

@@ -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()};

15
wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
View File

@@ -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());
}

15
wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
View File

@@ -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());
}

51
wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
View File

@@ -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;

15
wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
View File

@@ -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;

24
wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
View File

@@ -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;

24
wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
View File

@@ -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;

177
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
View File

@@ -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 */

80
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
View File

@@ -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;

33
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java
View File

@@ -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.
*

33
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java
View File

@@ -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.
*