Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +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;