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] Refactor and clean up ADIS IMU classes (#6719)

Browse Source
This commit is contained in:
Gold856
2024-07-19 00:09:11 -04:00
committed by GitHub
parent 45823abe86
commit 289d45b081
6 changed files with 495 additions and 1281 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -29,24 +29,17 @@
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
#include "frc/MathUtil.h"
/* Helpful conversion functions */
static inline uint16_t BuffToUShort(const uint32_t* buf) {
return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}
static inline uint8_t BuffToUByte(const uint32_t* buf) {
return static_cast<uint8_t>(buf[0]);
}
static inline int16_t BuffToShort(const uint32_t* buf) {
return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
}
static inline uint16_t ToUShort(const uint8_t* buf) {
return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}
using namespace frc;
namespace {
@@ -100,27 +93,28 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
// Relies on the RIO hardware by default configuring an output as low
// and configuring an input as high Z. The 10k pull-up resistor internal to
// the IMU then forces the reset line high for normal operation.
DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
Wait(10_ms); // Wait 10ms
delete m_reset_out;
DigitalOutput* reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
Wait(10_ms);
delete reset_out;
m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
Wait(500_ms); // Wait 500ms for reset to complete
Wait(500_ms); // Wait for reset to complete
ConfigCalTime(cal_time);
m_spi = new SPI(m_spi_port);
m_spi->SetClockRate(1000000);
m_spi->SetMode(frc::SPI::Mode::kMode3);
m_spi->SetChipSelectActiveLow();
// Configure standard SPI
if (!SwitchToStandardSPI()) {
return;
}
// Set up flash state variable
bool m_needs_flash = false;
bool needsFlash = false;
// Set IMU internal decimation to 1 (output data rate of 819.2 SPS / (1 + 1)
// = 409.6Hz), output bandwidth = 204.8Hz
if (ReadRegister(SMPL_PRD) != 0x0001) {
WriteRegister(SMPL_PRD, 0x0001);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling "
"flash update.");
@@ -129,7 +123,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
// Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP)
if (ReadRegister(MSC_CTRL) != 0x0016) {
WriteRegister(MSC_CTRL, 0x0016);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling "
"flash update.");
@@ -139,7 +133,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
// set IMU scale factor (range)
if (ReadRegister(SENS_AVG) != 0x0400) {
WriteRegister(SENS_AVG, 0x0400);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16448: SENS_AVG register configuration inconsistent! Scheduling "
"flash update.");
@@ -147,7 +141,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
// Clear offset registers
if (ReadRegister(XGYRO_OFF) != 0x0000) {
WriteRegister(XGYRO_OFF, 0x0000);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16448: XGYRO_OFF register configuration inconsistent! "
"Scheduling flash update.");
@@ -155,7 +149,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
if (ReadRegister(YGYRO_OFF) != 0x0000) {
WriteRegister(YGYRO_OFF, 0x0000);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16448: YGYRO_OFF register configuration inconsistent! "
"Scheduling flash update.");
@@ -163,7 +157,7 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
if (ReadRegister(ZGYRO_OFF) != 0x0000) {
WriteRegister(ZGYRO_OFF, 0x0000);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16448: ZGYRO_OFF register configuration inconsistent! "
"Scheduling flash update.");
@@ -171,22 +165,22 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
// If any registers on the IMU don't match the config, trigger a flash
// update
if (m_needs_flash) {
if (needsFlash) {
REPORT_WARNING(
"ADIS16448: Register configuration changed! Starting IMU flash "
"update.");
WriteRegister(GLOB_CMD, 0x0008);
// Wait long enough for the flash update to finish (72ms minimum as per
// Wait long enough for the flash update to finish (75ms minimum as per
// the datasheet)
Wait(0.5_s);
REPORT_WARNING("ADIS16448: Flash update finished!");
m_needs_flash = false;
} else {
REPORT_WARNING(
"ADIS16448: Flash and RAM configuration consistent. No flash update "
"required!");
}
m_auto_interrupt = new DigitalInput(10);
// Configure and enable auto SPI
if (!SwitchToAutoSPI()) {
return;
@@ -202,11 +196,11 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
// Tell the acquire loop that we're done starting up
m_start_up_mode = false;
// Let the user know the IMU was initiallized successfully
// Let the user know the IMU was initialized successfully
REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
// TODO: Find what the proper pin is to turn this LED
// Drive MXP PWM5 (IMU ready LED) low (active low)
// Drive MXP PWM5 (IMU ready LED) low (active low)
m_status_led = new DigitalOutput(19);
}
@@ -232,7 +226,6 @@ ADIS16448_IMU::ADIS16448_IMU(ADIS16448_IMU&& other)
m_mag_z{std::move(other.m_mag_z)},
m_baro{std::move(other.m_baro)},
m_temp{std::move(other.m_temp)},
m_tau{std::move(other.m_tau)},
m_dt{std::move(other.m_dt)},
m_alpha{std::move(other.m_alpha)},
m_compAngleX{std::move(other.m_compAngleX)},
@@ -291,7 +284,6 @@ ADIS16448_IMU& ADIS16448_IMU::operator=(ADIS16448_IMU&& other) {
std::swap(this->m_mag_z, other.m_mag_z);
std::swap(this->m_baro, other.m_baro);
std::swap(this->m_temp, other.m_temp);
std::swap(this->m_tau, other.m_tau);
std::swap(this->m_dt, other.m_dt);
std::swap(this->m_alpha, other.m_alpha);
std::swap(this->m_compAngleX, other.m_compAngleX);
@@ -362,54 +354,34 @@ bool ADIS16448_IMU::SwitchToStandardSPI() {
Wait(10_ms);
}
// Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
if (m_spi != nullptr && m_auto_configured) {
if (m_auto_configured) {
m_spi->StopAuto();
// We need to get rid of all the garbage left in the auto SPI buffer after
// stopping it. Sometimes data magically reappears, so we have to check
// the buffer size a couple of times
// to be sure we got it all. Yuck.
// stopping it.
// Sometimes data magically reappears, so we have to check
// the buffer size a couple of times to be sure we got it all. Yuck.
uint32_t trashBuffer[200];
Wait(100_ms);
int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
while (data_count > 0) {
/* Dequeue 200 at a time, or the remainder of the buffer if less than
* 200 */
// Dequeue 200 at a time, or the remainder of the buffer if less than
// 200
m_spi->ReadAutoReceivedData(trashBuffer, (std::min)(200, data_count),
0_s);
/* Update remaining buffer count */
// Update remaining buffer count
data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
}
}
}
// There doesn't seem to be a SPI port active. Let's try to set one up
if (m_spi == nullptr) {
m_spi = new SPI(m_spi_port);
m_spi->SetClockRate(1000000);
m_spi->SetMode(frc::SPI::Mode::kMode3);
m_spi->SetChipSelectActiveLow();
ReadRegister(PROD_ID); // Dummy read
// Validate the product ID
uint16_t prod_id = ReadRegister(PROD_ID);
if (prod_id != 16448) {
REPORT_ERROR("Could not find ADIS16448!");
Close();
return false;
}
return true;
} else {
// Maybe the SPI port is active, but not in auto SPI mode? Try to read the
// product ID.
ReadRegister(PROD_ID); // Dummy read
uint16_t prod_id = ReadRegister(PROD_ID);
if (prod_id != 16448) {
REPORT_ERROR("Could not find ADIS16448!");
Close();
return false;
} else {
return true;
}
ReadRegister(PROD_ID); // Dummy read
// Validate the product ID
uint16_t prod_id = ReadRegister(PROD_ID);
if (prod_id != 16448) {
REPORT_ERROR("Could not find ADIS16448!");
Close();
return false;
}
return true;
}
void ADIS16448_IMU::InitOffsetBuffer(int size) {
@@ -442,17 +414,6 @@ void ADIS16448_IMU::InitOffsetBuffer(int size) {
*are hard-coded to work only with the ADIS16448 IMU.
**/
bool ADIS16448_IMU::SwitchToAutoSPI() {
// No SPI port has been set up. Go set one up first.
if (m_spi == nullptr) {
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to start/restart auto SPI");
return false;
}
}
// Only set up the interrupt if needed.
if (m_auto_interrupt == nullptr) {
m_auto_interrupt = new DigitalInput(10);
}
// The auto SPI controller gets angry if you try to set up two instances on
// one bus.
if (!m_auto_configured) {
@@ -489,9 +450,6 @@ bool ADIS16448_IMU::SwitchToAutoSPI() {
return true;
}
/**
*
**/
int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
if (m_calibration_time == new_cal_time) {
return 1;
@@ -503,9 +461,6 @@ int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
}
}
/**
*
**/
void ADIS16448_IMU::Calibrate() {
std::scoped_lock sync(m_mutex);
// Calculate the running average
@@ -526,13 +481,6 @@ void ADIS16448_IMU::Calibrate() {
m_integ_gyro_angle_z = 0.0;
}
/**
* This function reads the contents of an 8-bit register location by
*transmitting the register location byte along with a null (0x00) byte using
*the standard WPILib API. The response (two bytes) is read back using the
*WPILib API and joined using a helper function. This function assumes the
*controller is set to standard SPI mode.
**/
uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) {
uint8_t buf[2];
buf[0] = reg & 0x7f;
@@ -541,15 +489,15 @@ uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) {
m_spi->Write(buf, 2);
m_spi->Read(false, buf, 2);
return ToUShort(buf);
return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}
/**
* This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
*via SPI. The upper and lower bytes that make up the 16-bit value are split
*into two unsined, 8-bit values and written to the upper and lower addresses of
*the specified register value. Only the lower (base) address must be specified.
*This function assumes the controller is set to standard SPI mode.
* via SPI. The upper and lower bytes that make up the 16-bit value are split
* into two unsigned, 8-bit values and written to the upper and lower addresses
* of the specified register value. Only the lower (base) address must be
* specified. This function assumes the controller is set to standard SPI mode.
**/
void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
uint8_t buf[2];
@@ -561,10 +509,6 @@ void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
m_spi->Write(buf, 2);
}
/**
* This function resets (zeros) the accumulated (integrated) angle estimates for
*the xgyro, ygyro, and zgyro outputs.
**/
void ADIS16448_IMU::Reset() {
std::scoped_lock sync(m_mutex);
m_integ_gyro_angle_x = 0.0;
@@ -609,126 +553,93 @@ ADIS16448_IMU::~ADIS16448_IMU() {
void ADIS16448_IMU::Acquire() {
// Set data packet length
const int dataset_len = 29; // 18 data points + timestamp
const int BUFFER_SIZE = 4000;
// This buffer can contain many datasets
uint32_t buffer[BUFFER_SIZE];
int data_count = 0;
int data_remainder = 0;
int data_to_read = 0;
int bufferAvgIndex = 0;
uint32_t previous_timestamp = 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 mag_x = 0.0;
double mag_y = 0.0;
double mag_z = 0.0;
double baro = 0.0;
double temp = 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;
double compAngleX = 0.0;
double compAngleY = 0.0;
double accelAngleX = 0.0;
double accelAngleY = 0.0;
while (true) {
// Sleep loop for 10ms (wait for data)
// Wait for data
Wait(10_ms);
if (m_thread_active) {
data_count = m_spi->ReadAutoReceivedData(
buffer, 0,
0_s); // Read number of bytes currently stored in the buffer
data_remainder =
data_count % dataset_len; // Check if frame is incomplete
data_to_read = data_count -
data_remainder; // Remove incomplete data from read count
/* Want to cap the data to read in a single read at the buffer size */
// Read number of bytes currently stored in the buffer
int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s);
// Check if frame is incomplete
int data_remainder = data_count % dataset_len;
// Remove incomplete data from read count
int data_to_read = data_count - data_remainder;
// Want to cap the data to read in a single read at the buffer size
if (data_to_read > BUFFER_SIZE) {
REPORT_WARNING(
"ADIS16448 data processing thread overrun has occurred!");
data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
}
m_spi->ReadAutoReceivedData(buffer, data_to_read,
0_s); // Read data from DMA buffer
// Read data from DMA buffer
m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s);
// Could be multiple data sets in the buffer. Handle each one.
for (int i = 0; i < data_to_read; i += dataset_len) {
// Calculate CRC-16 on each data packet
uint16_t calc_crc = 0xFFFF; // Starting word
uint8_t byte = 0;
uint16_t imu_crc = 0;
for (int k = 5; k < 27;
k += 2) // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP
// (Ignore Status & CRC)
{
byte = BuffToUByte(&buffer[i + k + 1]); // Process LSB
calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
byte = BuffToUByte(&buffer[i + k]); // Process MSB
calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
// Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status
// & CRC)
for (int k = 5; k < 27; k += 2) {
// Process LSB
uint8_t byte = static_cast<uint8_t>(buffer[i + k + 1]);
calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ byte];
// Process MSB
byte = static_cast<uint8_t>(buffer[i + k]);
calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ byte];
}
calc_crc = ~calc_crc; // Complement
calc_crc = static_cast<uint16_t>((calc_crc << 8) |
(calc_crc >> 8)); // Flip LSB & MSB
imu_crc =
BuffToUShort(&buffer[i + 27]); // Extract DUT CRC from data buffer
// Complement
calc_crc = ~calc_crc;
// Flip LSB & MSB
calc_crc = static_cast<uint16_t>((calc_crc << 8) | (calc_crc >> 8));
// Extract DUT CRC from data buffer
uint16_t imu_crc = BuffToUShort(&buffer[i + 27]);
// Compare calculated vs read CRC. Don't update outputs or dt if CRC-16
// is bad
if (calc_crc == imu_crc) {
// Timestamp is at buffer[i]
m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
// Split array and scale data
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;
mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
baro = BuffToShort(&buffer[i + 23]) * 0.02;
temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
// Scale sensor data
double gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
double gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
double gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
double accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
double accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
double accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
double mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
double mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
double mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
double baro = BuffToShort(&buffer[i + 23]) * 0.02;
double temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
// Convert scaled sensor data to SI units
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;
double gyro_rate_x_si = gyro_rate_x * kDegToRad;
double gyro_rate_y_si = gyro_rate_y * kDegToRad;
// double gyro_rate_z_si = gyro_rate_z * kDegToRad;
double accel_x_si = accel_x * kGrav;
double accel_y_si = accel_y * kGrav;
double accel_z_si = accel_z * kGrav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
// Calculate alpha for use with the complementary filter
m_alpha = m_tau / (m_tau + m_dt);
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX =
atan2f(-accel_x_si, std::hypotf(accel_y_si, -accel_z_si));
double accelAngleY =
atan2f(accel_y_si, std::hypotf(-accel_x_si, -accel_z_si));
// Calculate complementary filter
if (m_first_run) {
accelAngleX = atan2f(
-accel_x_si,
sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
accelAngleY =
atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
(-accel_z_si * -accel_z_si)));
compAngleX = accelAngleX;
compAngleY = accelAngleY;
} else {
accelAngleX = atan2f(
-accel_x_si,
sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
accelAngleY =
atan2f(accel_y_si, sqrtf((-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 =
@@ -748,7 +659,7 @@ void ADIS16448_IMU::Acquire() {
} else {
// Accumulate gyro for offset calibration
// Add most recent sample data to buffer
bufferAvgIndex = m_accum_count % m_avg_size;
int bufferAvgIndex = m_accum_count % m_avg_size;
m_offset_buffer[bufferAvgIndex] =
OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z};
// Increment counter
@@ -768,10 +679,10 @@ void ADIS16448_IMU::Acquire() {
m_mag_z = mag_z;
m_baro = baro;
m_temp = temp;
m_compAngleX = compAngleX * rad_to_deg;
m_compAngleY = compAngleY * rad_to_deg;
m_accelAngleX = accelAngleX * rad_to_deg;
m_accelAngleY = accelAngleY * rad_to_deg;
m_compAngleX = compAngleX * kRadToDeg;
m_compAngleY = compAngleY * kRadToDeg;
m_accelAngleX = accelAngleX * kRadToDeg;
m_accelAngleY = accelAngleY * kRadToDeg;
// Accumulate gyro for angle integration and publish to global
// variables
m_integ_gyro_angle_x +=
@@ -787,31 +698,9 @@ void ADIS16448_IMU::Acquire() {
}
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 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;
mag_x = 0.0;
mag_y = 0.0;
mag_z = 0.0;
baro = 0.0;
temp = 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;
compAngleX = 0.0;
compAngleY = 0.0;
accelAngleX = 0.0;
accelAngleY = 0.0;
}
}
}
@@ -826,16 +715,6 @@ double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
return compAngle;
}
double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
while (compAngle >= 2 * std::numbers::pi) {
compAngle = compAngle - 2.0 * std::numbers::pi;
}
while (compAngle < 0.0) {
compAngle = compAngle + 2.0 * std::numbers::pi;
}
return compAngle;
}
double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = std::numbers::pi - accelAngle;
@@ -850,43 +729,35 @@ double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle,
compAngle = FormatFastConverge(compAngle, accelAngle);
compAngle =
m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
compAngle = FormatRange0to2PI(compAngle);
if (compAngle > std::numbers::pi) {
compAngle = compAngle - 2.0 * std::numbers::pi;
}
return compAngle;
return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi);
}
int ADIS16448_IMU::ConfigDecRate(uint16_t decimationRate) {
// Switches the active SPI port to standard SPI mode, writes a new value to
// the DECIMATE register in the IMU, and re-enables auto SPI.
//
// This function enters standard SPI mode, writes a new DECIMATE setting to
// the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
uint16_t writeValue = decimationRate;
uint16_t readbackValue;
// the DECIMATE register in the IMU, adjusts the sample scale factor, and
// re-enables auto SPI.
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
/* Check max */
// Check max
if (decimationRate > 9) {
REPORT_ERROR(
"Attempted to write an invalid decimation value. Capping at 9");
decimationRate = 9;
}
/* Shift decimation setting to correct position and select internal sync */
writeValue = (decimationRate << 8) | 0x1;
// Shift decimation setting to correct position and select internal sync
uint16_t writeValue = (decimationRate << 8) | 0x1;
/* Apply to IMU */
// Apply to IMU
WriteRegister(SMPL_PRD, writeValue);
/* Perform read back to verify write */
readbackValue = ReadRegister(SMPL_PRD);
// Perform read back to verify write
uint16_t readbackValue = ReadRegister(SMPL_PRD);
/* Throw error for invalid write */
// Throw error for invalid write
if (readbackValue != writeValue) {
REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
}
@@ -1059,12 +930,6 @@ int ADIS16448_IMU::GetPort() const {
return m_spi_port;
}
/**
* @brief Builds a Sendable object to push IMU data to the driver station.
*
* This function pushes the most recent angle estimates for all axes to the
*driver station.
**/
void ADIS16448_IMU::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16448 IMU");
builder.AddDoubleProperty(

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

@@ -27,6 +27,7 @@
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
#include "frc/MathUtil.h"
/* Helpful conversion functions */
static inline int32_t ToInt(const uint32_t* buf) {
@@ -38,10 +39,6 @@ static inline int16_t BuffToShort(const uint32_t* buf) {
return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
}
static inline uint16_t ToUShort(const uint8_t* buf) {
return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}
using namespace frc;
namespace {
@@ -59,9 +56,6 @@ inline void ADISReportError(int32_t status, const char* file, int line,
#define REPORT_ERROR(msg) \
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, msg)
/**
* Constructor.
*/
ADIS16470_IMU::ADIS16470_IMU()
: ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_1s) {}
@@ -120,35 +114,37 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
// Relies on the RIO hardware by default configuring an output as low
// and configuring an input as high Z. The 10k pull-up resistor internal to
// the IMU then forces the reset line high for normal operation.
DigitalOutput* m_reset_out =
DigitalOutput* reset_out =
new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
Wait(10_ms); // Wait 10ms
delete m_reset_out;
Wait(10_ms);
delete reset_out;
m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
Wait(500_ms); // Wait 500ms for reset to complete
Wait(500_ms); // Wait for reset to complete
m_spi = new SPI(m_spi_port);
m_spi->SetClockRate(2000000);
m_spi->SetMode(frc::SPI::Mode::kMode3);
m_spi->SetChipSelectActiveLow();
// Configure standard SPI
if (!SwitchToStandardSPI()) {
return;
}
// Set up flash state variable
bool m_needs_flash = false;
bool needsFlash = false;
// Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
// = 400Hz)
if (ReadRegister(DEC_RATE) != 0x0004) {
WriteRegister(DEC_RATE, 0x0004);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16470: DEC_RATE register configuration inconsistent! Scheduling "
"flash update.");
}
// Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
// and PoP
if (ReadRegister(MSC_CTRL) != 0x0001) {
WriteRegister(MSC_CTRL, 0x0001);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling "
"flash update.");
@@ -157,7 +153,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
// Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient)
if (ReadRegister(FILT_CTRL) != 0x0000) {
WriteRegister(FILT_CTRL, 0x0000);
m_needs_flash = true;
needsFlash = true;
REPORT_WARNING(
"ADIS16470: FILT_CTRL register configuration inconsistent! "
"Scheduling flash update.");
@@ -165,7 +161,7 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
// If any registers on the IMU don't match the config, trigger a flash
// update
if (m_needs_flash) {
if (needsFlash) {
REPORT_WARNING(
"ADIS16470: Register configuration changed! Starting IMU flash "
"update.");
@@ -174,7 +170,6 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
// the datasheet)
Wait(0.3_s);
REPORT_WARNING("ADIS16470: Flash update finished!");
m_needs_flash = false;
} else {
REPORT_WARNING(
"ADIS16470: Flash and RAM configuration consistent. No flash update "
@@ -194,12 +189,13 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
// Write offset calibration command to IMU
WriteRegister(GLOB_CMD, 0x0001);
m_auto_interrupt = new DigitalInput(26);
// Configure and enable auto SPI
if (!SwitchToAutoSPI()) {
return;
}
// Let the user know the IMU was initiallized successfully
// Let the user know the IMU was initialized successfully
REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
// Drive SPI CS3 (IMU ready LED) low (active low)
@@ -228,7 +224,6 @@ ADIS16470_IMU::ADIS16470_IMU(ADIS16470_IMU&& other)
m_accel_x{std::move(other.m_accel_x)},
m_accel_y{std::move(other.m_accel_y)},
m_accel_z{std::move(other.m_accel_z)},
m_tau{std::move(other.m_tau)},
m_dt{std::move(other.m_dt)},
m_alpha{std::move(other.m_alpha)},
m_compAngleX{std::move(other.m_compAngleX)},
@@ -278,7 +273,6 @@ ADIS16470_IMU& ADIS16470_IMU::operator=(ADIS16470_IMU&& other) {
std::swap(this->m_accel_x, other.m_accel_x);
std::swap(this->m_accel_y, other.m_accel_y);
std::swap(this->m_accel_z, other.m_accel_z);
std::swap(this->m_tau, other.m_tau);
std::swap(this->m_dt, other.m_dt);
std::swap(this->m_alpha, other.m_alpha);
std::swap(this->m_compAngleX, other.m_compAngleX);
@@ -340,54 +334,33 @@ bool ADIS16470_IMU::SwitchToStandardSPI() {
Wait(10_ms);
}
// Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
if (m_spi != nullptr && m_auto_configured) {
if (m_auto_configured) {
m_spi->StopAuto();
// We need to get rid of all the garbage left in the auto SPI buffer after
// stopping it. Sometimes data magically reappears, so we have to check
// the buffer size a couple of times
// to be sure we got it all. Yuck.
// stopping it.
// Sometimes data magically reappears, so we have to check the buffer size
// a couple of times to be sure we got it all. Yuck.
uint32_t trashBuffer[200];
Wait(100_ms);
int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
while (data_count > 0) {
/* Receive data, max of 200 words at a time (prevent potential segfault)
*/
// Receive data, max of 200 words at a time (prevent potential segfault)
m_spi->ReadAutoReceivedData(trashBuffer, (std::min)(data_count, 200),
0_s);
/*Get the remaining data count */
// Get the remaining data count
data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
}
}
}
// There doesn't seem to be a SPI port active. Let's try to set one up
if (m_spi == nullptr) {
m_spi = new SPI(m_spi_port);
m_spi->SetClockRate(2000000);
m_spi->SetMode(frc::SPI::Mode::kMode3);
m_spi->SetChipSelectActiveLow();
ReadRegister(PROD_ID); // Dummy read
// Validate the product ID
uint16_t prod_id = ReadRegister(PROD_ID);
if (prod_id != 16982 && prod_id != 16470) {
REPORT_ERROR("Could not find ADIS16470!");
Close();
return false;
}
return true;
} else {
// Maybe the SPI port is active, but not in auto SPI mode? Try to read the
// product ID.
ReadRegister(PROD_ID); // Dummy read
uint16_t prod_id = ReadRegister(PROD_ID);
if (prod_id != 16982 && prod_id != 16470) {
REPORT_ERROR("Could not find ADIS16470!");
Close();
return false;
} else {
return true;
}
ReadRegister(PROD_ID); // Dummy read
// Validate the product ID
uint16_t prod_id = ReadRegister(PROD_ID);
if (prod_id != 16982 && prod_id != 16470) {
REPORT_ERROR("Could not find ADIS16470!");
Close();
return false;
}
return true;
}
/**
@@ -407,17 +380,6 @@ bool ADIS16470_IMU::SwitchToStandardSPI() {
*are hard-coded to work only with the ADIS16470 IMU.
**/
bool ADIS16470_IMU::SwitchToAutoSPI() {
// No SPI port has been set up. Go set one up first.
if (m_spi == nullptr) {
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to start/restart auto SPI");
return false;
}
}
// Only set up the interrupt if needed.
if (m_auto_interrupt == nullptr) {
m_auto_interrupt = new DigitalInput(26);
}
// The auto SPI controller gets angry if you try to set up two instances on
// one bus.
if (!m_auto_configured) {
@@ -426,12 +388,11 @@ bool ADIS16470_IMU::SwitchToAutoSPI() {
}
// Do we need to change auto SPI settings?
m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2);
// Configure auto stall time
m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
// Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is
// activated) DR High = Data good (data capture should be triggered on the
// rising edge)
// activated)
// DR High = Data good (data capture should be triggered on the rising edge)
m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
// Check to see if the acquire thread is running. If not, kick one off.
if (!m_thread_idle) {
@@ -488,10 +449,8 @@ int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) {
// Switches the active SPI port to standard SPI mode, writes a new value to
// the DECIMATE register in the IMU, and re-enables auto SPI.
//
// This function enters standard SPI mode, writes a new DECIMATE setting to
// the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
// the DECIMATE register in the IMU, adjusts the sample scale factor, and
// re-enables auto SPI.
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
@@ -500,7 +459,7 @@ int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) {
REPORT_ERROR("Attempted to write an invalid decimation value.");
decimationRate = 1999;
}
m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0);
m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0;
WriteRegister(DEC_RATE, decimationRate);
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
@@ -528,20 +487,6 @@ void ADIS16470_IMU::Calibrate() {
}
}
/**
* @brief Reads the contents of a specified register location over SPI.
*
* @param reg An unsigned, 8-bit register location.
*
* @return An unsigned, 16-bit number representing the contents of the requested
*register location.
*
* This function reads the contents of an 8-bit register location by
*transmitting the register location byte along with a null (0x00) byte using
*the standard WPILib API. The response (two bytes) is read back using the
*WPILib API and joined using a helper function. This function assumes the
*controller is set to standard SPI mode.
**/
uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
uint8_t buf[2];
buf[0] = reg & 0x7f;
@@ -550,24 +495,24 @@ uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
m_spi->Write(buf, 2);
m_spi->Read(false, buf, 2);
return ToUShort(buf);
return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}
/**
* @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
*locations over SPI.
* locations over SPI.
*
* @param reg An unsigned, 8-bit register location.
*
* @param val An unsigned, 16-bit value to be written to the specified register
*location.
* location.
*
* This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
*via SPI. The upper and lower bytes that make up the 16-bit value are split
*into two unsined, 8-bit values and written to the upper and lower addresses of
*the specified register value. Only the lower (base) address must be specified.
*This function assumes the controller is set to standard SPI mode.
**/
* via SPI. The upper and lower bytes that make up the 16-bit value are split
* into two unsigned, 8-bit values and written to the upper and lower addresses
* of the specified register value. Only the lower (base) address must be
* specified. This function assumes the controller is set to standard SPI mode.
*/
void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
uint8_t buf[2];
buf[0] = 0x80 | reg;
@@ -578,12 +523,6 @@ void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
m_spi->Write(buf, 2);
}
/**
* @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
*
* This function resets (zeros) the accumulated (integrated) angle estimates for
*the xgyro, ygyro, and zgyro outputs.
**/
void ADIS16470_IMU::Reset() {
std::scoped_lock sync(m_mutex);
m_integ_angle_x = 0.0;
@@ -646,110 +585,79 @@ ADIS16470_IMU::~ADIS16470_IMU() {
void ADIS16470_IMU::Acquire() {
// Set data packet length
const int dataset_len = 27; // 26 data points + timestamp
/* Fixed buffer size */
const int BUFFER_SIZE = 4000;
// This buffer can contain many datasets
uint32_t buffer[BUFFER_SIZE];
int data_count = 0;
int data_remainder = 0;
int data_to_read = 0;
uint32_t previous_timestamp = 0;
double delta_angle_x = 0.0;
double delta_angle_y = 0.0;
double delta_angle_z = 0.0;
double gyro_rate_x = 0.0;
double gyro_rate_y = 0.0;
double gyro_rate_z = 0.0;
double accel_x = 0.0;
double accel_y = 0.0;
double accel_z = 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;
double compAngleX = 0.0;
double compAngleY = 0.0;
double accelAngleX = 0.0;
double accelAngleY = 0.0;
while (true) {
// Sleep loop for 10ms (wait for data)
// Wait for data
Wait(10_ms);
if (m_thread_active) {
m_thread_idle = false;
data_count = m_spi->ReadAutoReceivedData(
buffer, 0,
0_s); // Read number of bytes currently stored in the buffer
data_remainder =
data_count % dataset_len; // Check if frame is incomplete. Add 1
// because of timestamp
data_to_read = data_count -
data_remainder; // Remove incomplete data from read count
/* Want to cap the data to read in a single read at the buffer size */
// Read number of bytes currently stored in the buffer
int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s);
// Check if frame is incomplete
int data_remainder = data_count % dataset_len;
// Remove incomplete data from read count
int data_to_read = data_count - data_remainder;
// Want to cap the data to read in a single read at the buffer size
if (data_to_read > BUFFER_SIZE) {
REPORT_WARNING(
"ADIS16470 data processing thread overrun has occurred!");
data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
}
m_spi->ReadAutoReceivedData(
buffer, data_to_read,
0_s); // Read data from DMA buffer (only complete sets)
// Read data from DMA buffer (only complete sets)
m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s);
// Could be multiple data sets in the buffer. Handle each one.
for (int i = 0; i < data_to_read; i += dataset_len) {
// Timestamp is at buffer[i]
m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
/* Get delta angle value for selected yaw axis and scale by the elapsed
* time (based on timestamp) */
delta_angle_x =
(ToInt(&buffer[i + 3]) * delta_angle_sf) /
(m_scaled_sample_rate / (buffer[i] - previous_timestamp));
delta_angle_y =
(ToInt(&buffer[i + 7]) * delta_angle_sf) /
(m_scaled_sample_rate / (buffer[i] - previous_timestamp));
delta_angle_z =
(ToInt(&buffer[i + 11]) * delta_angle_sf) /
(m_scaled_sample_rate / (buffer[i] - previous_timestamp));
// Get delta angle value for selected yaw axis and scale by the elapsed
// time (based on timestamp)
double elapsed_time =
m_scaled_sample_rate / (buffer[i] - previous_timestamp);
double delta_angle_x =
ToInt(&buffer[i + 3]) * delta_angle_sf / elapsed_time;
double delta_angle_y =
ToInt(&buffer[i + 7]) * delta_angle_sf / elapsed_time;
double delta_angle_z =
ToInt(&buffer[i + 11]) * delta_angle_sf / elapsed_time;
gyro_rate_x = (BuffToShort(&buffer[i + 15]) / 10.0);
gyro_rate_y = (BuffToShort(&buffer[i + 17]) / 10.0);
gyro_rate_z = (BuffToShort(&buffer[i + 19]) / 10.0);
accel_x = (BuffToShort(&buffer[i + 21]) / 800.0);
accel_y = (BuffToShort(&buffer[i + 23]) / 800.0);
accel_z = (BuffToShort(&buffer[i + 25]) / 800.0);
double gyro_rate_x = BuffToShort(&buffer[i + 15]) / 10.0;
double gyro_rate_y = BuffToShort(&buffer[i + 17]) / 10.0;
double gyro_rate_z = BuffToShort(&buffer[i + 19]) / 10.0;
double accel_x = BuffToShort(&buffer[i + 21]) / 800.0;
double accel_y = BuffToShort(&buffer[i + 23]) / 800.0;
double accel_z = BuffToShort(&buffer[i + 25]) / 800.0;
// Convert scaled sensor data to SI units
gyro_rate_x_si = gyro_rate_x * deg_to_rad;
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;
double gyro_rate_x_si = gyro_rate_x * kDegToRad;
double gyro_rate_y_si = gyro_rate_y * kDegToRad;
// double gyro_rate_z_si = gyro_rate_z * kDegToRad;
double accel_x_si = accel_x * kGrav;
double accel_y_si = accel_y * kGrav;
double accel_z_si = accel_z * kGrav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
m_alpha = m_tau / (m_tau + m_dt);
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX =
atan2f(accel_x_si, std::hypotf(accel_y_si, accel_z_si));
double accelAngleY =
atan2f(accel_y_si, std::hypotf(accel_x_si, accel_z_si));
if (m_first_run) {
accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
(accel_z_si * accel_z_si)));
accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
(accel_z_si * accel_z_si)));
compAngleX = accelAngleX;
compAngleY = accelAngleY;
} else {
// Process X angle
accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
(accel_z_si * accel_z_si)));
accelAngleY = atan2f(accel_y_si, sqrtf((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 =
@@ -760,10 +668,10 @@ void ADIS16470_IMU::Acquire() {
{
std::scoped_lock sync(m_mutex);
/* Push data to global variables */
// Push data to global variables
if (m_first_run) {
/* Don't accumulate first run. previous_timestamp will be "very" old
* and the integration will end up way off */
// Don't accumulate first run. previous_timestamp will be "very" old
// and the integration will end up way off
m_integ_angle_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
@@ -778,38 +686,18 @@ void ADIS16470_IMU::Acquire() {
m_accel_x = accel_x;
m_accel_y = accel_y;
m_accel_z = accel_z;
m_compAngleX = compAngleX * rad_to_deg;
m_compAngleY = compAngleY * rad_to_deg;
m_accelAngleX = accelAngleX * rad_to_deg;
m_accelAngleY = accelAngleY * rad_to_deg;
m_compAngleX = compAngleX * kRadToDeg;
m_compAngleY = compAngleY * kRadToDeg;
m_accelAngleX = accelAngleX * kRadToDeg;
m_accelAngleY = accelAngleY * kRadToDeg;
}
m_first_run = false;
}
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
delta_angle_x = 0.0;
delta_angle_y = 0.0;
delta_angle_z = 0.0;
gyro_rate_x = 0.0;
gyro_rate_y = 0.0;
gyro_rate_z = 0.0;
accel_x = 0.0;
accel_y = 0.0;
accel_z = 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;
previous_timestamp = 0;
compAngleX = 0.0;
compAngleY = 0.0;
accelAngleX = 0.0;
accelAngleY = 0.0;
}
}
}
@@ -824,16 +712,6 @@ double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
return compAngle;
}
double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
while (compAngle >= 2 * std::numbers::pi) {
compAngle = compAngle - 2.0 * std::numbers::pi;
}
while (compAngle < 0.0) {
compAngle = compAngle + 2.0 * std::numbers::pi;
}
return compAngle;
}
double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = std::numbers::pi - accelAngle;
@@ -848,11 +726,7 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
compAngle = FormatFastConverge(compAngle, accelAngle);
compAngle =
m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
compAngle = FormatRange0to2PI(compAngle);
if (compAngle > std::numbers::pi) {
compAngle = compAngle - 2.0 * std::numbers::pi;
}
return compAngle;
return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi);
}
void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) {
@@ -1054,12 +928,6 @@ int ADIS16470_IMU::GetPort() const {
return m_spi_port;
}
/**
* @brief Builds a Sendable object to push IMU data to the driver station.
*
* This function pushes the most recent angle estimates for all axes to the
*driver station.
**/
void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16470 IMU");
builder.AddDoubleProperty(

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

@@ -281,7 +281,7 @@ class ADIS16448_IMU : public wpi::Sendable,
void InitSendable(wpi::SendableBuilder& builder) override;
private:
/** @brief ADIS16448 Register Map Declaration */
// ADIS16448 Register Map Declaration
static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
static constexpr uint8_t XGYRO_OUT = 0x04; // X-axis gyroscope output
static constexpr uint8_t YGYRO_OUT = 0x06; // Y-axis gyroscope output
@@ -338,9 +338,9 @@ class ADIS16448_IMU : public wpi::Sendable,
static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number
/** @brief ADIS16448 Static Constants */
static constexpr double rad_to_deg = 57.2957795;
static constexpr double deg_to_rad = 0.0174532;
static constexpr double grav = 9.81;
static constexpr double kRadToDeg = 57.2957795;
static constexpr double kDegToRad = 0.0174532;
static constexpr double kGrav = 9.81;
/** @brief struct to store offset data */
struct OffsetData {
@@ -382,8 +382,8 @@ class ADIS16448_IMU : public wpi::Sendable,
double m_temp = 0.0;
// Complementary filter variables
double m_tau = 0.5;
double m_dt, m_alpha = 0.0;
static constexpr double kTau = 0.5;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// vector for storing most recent imu values
@@ -408,8 +408,6 @@ class ADIS16448_IMU : public wpi::Sendable,
// Complementary filter functions
double FormatFastConverge(double compAngle, double accAngle);
double FormatRange0to2PI(double compAngle);
double FormatAccelRange(double accelAngle, double accelZ);
double CompFilterProcess(double compAngle, double accelAngle, double omega);
@@ -463,7 +461,7 @@ class ADIS16448_IMU : public wpi::Sendable,
mutable NonMovableMutexWrapper m_mutex;
// CRC-16 Look-Up Table
static constexpr uint16_t adiscrc[256] = {
static constexpr uint16_t m_adiscrc[256] = {
0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,

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

@@ -162,8 +162,10 @@ class ADIS16470_IMU : public wpi::Sendable,
void Calibrate();
/**
* @brief Switches the active SPI port to standard SPI mode, writes a new
* value to the NULL_CNFG register in the IMU, and re-enables auto SPI.
* Configures calibration time.
*
* @param new_cal_time New calibration time
* @return 0 if success, 1 if no change, 2 if error.
*/
int ConfigCalTime(CalibrationTime new_cal_time);
@@ -418,11 +420,11 @@ class ADIS16470_IMU : public wpi::Sendable,
Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
static constexpr double delta_angle_sf = 2160.0 / 2147483648.0;
static constexpr double rad_to_deg = 57.2957795;
static constexpr double deg_to_rad = 0.0174532;
static constexpr double grav = 9.81;
static constexpr double kRadToDeg = 57.2957795;
static constexpr double kDegToRad = 0.0174532;
static constexpr double kGrav = 9.81;
/** @brief Resources **/
// Resources
DigitalInput* m_reset_in = nullptr;
DigitalOutput* m_status_led = nullptr;
@@ -487,15 +489,13 @@ class ADIS16470_IMU : public wpi::Sendable,
double m_accel_z = 0.0;
// Complementary filter variables
double m_tau = 1.0;
double m_dt, m_alpha = 0.0;
static constexpr double kTau = 1.0;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// Complementary filter functions
double FormatFastConverge(double compAngle, double accAngle);
double FormatRange0to2PI(double compAngle);
double FormatAccelRange(double accelAngle, double accelZ);
double CompFilterProcess(double compAngle, double accelAngle, double omega);

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

@@ -18,40 +18,20 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
// CHECKSTYLE.OFF: TypeName
// CHECKSTYLE.OFF: MemberName
// CHECKSTYLE.OFF: SummaryJavadoc
// CHECKSTYLE.OFF: UnnecessaryParentheses
// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder
// CHECKSTYLE.OFF: NonEmptyAtclauseDescription
// CHECKSTYLE.OFF: MissingOverride
// CHECKSTYLE.OFF: AtclauseOrder
// CHECKSTYLE.OFF: LocalVariableName
// CHECKSTYLE.OFF: RedundantModifier
// CHECKSTYLE.OFF: AbbreviationAsWordInName
// CHECKSTYLE.OFF: ParameterName
// CHECKSTYLE.OFF: EmptyCatchBlock
// CHECKSTYLE.OFF: MissingJavadocMethod
// CHECKSTYLE.OFF: MissingSwitchDefault
// CHECKSTYLE.OFF: VariableDeclarationUsageDistance
// CHECKSTYLE.OFF: ArrayTypeStyle
/** This class is for the ADIS16448 IMU that connects to the RoboRIO MXP port. */
@SuppressWarnings({
"unused",
"PMD.RedundantFieldInitializer",
"PMD.ImmutableField",
"PMD.SingularField",
"PMD.CollapsibleIfStatements",
"PMD.MissingOverride",
"PMD.EmptyIfStmt",
"PMD.EmptyStatementNotInLoop"
})
@SuppressWarnings({"PMD.RedundantFieldInitializer", "PMD.ImmutableField"})
public class ADIS16448_IMU implements AutoCloseable, Sendable {
/** ADIS16448 Register Map Declaration */
// ADIS16448 Register Map Declaration
private static final int FLASH_CNT = 0x00; // Flash memory write count
private static final int XGYRO_OUT = 0x04; // X-axis gyroscope output
@@ -96,29 +76,29 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
/** ADIS16448 calibration times. */
public enum CalibrationTime {
/** 32 ms calibration time */
/** 32 ms calibration time. */
_32ms(0),
/** 64 ms calibration time */
/** 64 ms calibration time. */
_64ms(1),
/** 128 ms calibration time */
/** 128 ms calibration time. */
_128ms(2),
/** 256 ms calibration time */
/** 256 ms calibration time. */
_256ms(3),
/** 512 ms calibration time */
/** 512 ms calibration time. */
_512ms(4),
/** 1 s calibration time */
/** 1 s calibration time. */
_1s(5),
/** 2 s calibration time */
/** 2 s calibration time. */
_2s(6),
/** 4 s calibration time */
/** 4 s calibration time. */
_4s(7),
/** 8 s calibration time */
/** 8 s calibration time. */
_8s(8),
/** 16 s calibration time */
/** 16 s calibration time. */
_16s(9),
/** 32 s calibration time */
/** 32 s calibration time. */
_32s(10),
/** 64 s calibration time */
/** 64 s calibration time. */
_64s(11);
private final int value;
@@ -138,20 +118,17 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
kZ
}
// * Static Constants */
private static final double rad_to_deg = 57.2957795;
private static final double deg_to_rad = 0.0174532;
private static final double grav = 9.81;
private static final double kGrav = 9.81;
/* User-specified yaw axis */
// User-specified yaw axis
private IMUAxis m_yaw_axis;
/* Offset data storage */
// Offset data storage
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 */
// Instant raw output variables
private double m_gyro_rate_x = 0.0;
private double m_gyro_rate_y = 0.0;
private double m_gyro_rate_z = 0.0;
@@ -164,41 +141,39 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
private double m_baro = 0.0;
private double m_temp = 0.0;
/* IMU gyro offset variables */
// IMU gyro offset variables
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 */
// Integrated gyro angle variables
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 */
// Complementary filter variables
private double m_dt = 0.0;
private double m_alpha = 0.0;
private double m_tau = 1.0;
private static final double kTau = 1.0;
private double m_compAngleX = 0.0;
private double m_compAngleY = 0.0;
private double m_accelAngleX = 0.0;
private double m_accelAngleY = 0.0;
/* State variables */
// State variables
private volatile boolean m_thread_active = false;
private CalibrationTime m_calibration_time = CalibrationTime._32ms;
private volatile boolean m_first_run = true;
private volatile boolean m_thread_idle = false;
private boolean m_auto_configured = false;
private boolean m_start_up_mode = true;
private boolean m_needs_flash = false;
/* Resources */
// Resources
private SPI m_spi;
private SPI.Port m_spi_port;
private DigitalInput m_auto_interrupt;
private DigitalOutput m_reset_out;
private DigitalInput m_reset_in;
private DigitalOutput m_status_led;
private Thread m_acquire_task;
@@ -216,55 +191,41 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
private SimDouble m_simAccelY;
private SimDouble m_simAccelZ;
/* CRC-16 Look-Up Table */
int[] adiscrc =
new int[] {
0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF,
0x1F3F, 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890,
0x1E3D, 0x09F3, 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992,
0x0102, 0x16CC, 0x0EDD, 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD,
0x1C39, 0x0BF7, 0x13E6, 0x0428, 0x0387, 0x1449, 0x0C58, 0x1B96,
0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8, 0x0B76, 0x1367, 0x04A9,
0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74, 0x1265, 0x05AB,
0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A, 0x1A94,
0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1,
0x060C, 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3,
0x1933, 0x0EFD, 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C,
0x0408, 0x13C6, 0x0BD7, 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7,
0x1B37, 0x0CF9, 0x14E8, 0x0326, 0x0489, 0x1347, 0x0B56, 0x1C98,
0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B, 0x1245, 0x0A54, 0x1D9A,
0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A, 0x156B, 0x02A5,
0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040, 0x178E,
0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3,
0x1123, 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C,
0x0C18, 0x1BD6, 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7,
0x1327, 0x04E9, 0x1CF8, 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488,
0x1225, 0x05EB, 0x1DFA, 0x0A34, 0x0D9B, 0x1A55, 0x0244, 0x158A,
0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4, 0x056A, 0x1D7B, 0x0AB5,
0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060, 0x1871, 0x0FBF,
0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E, 0x1080,
0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD,
0x1429, 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386,
0x0B16, 0x1CD8, 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9,
0x0A14, 0x1DDA, 0x05CB, 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB,
0x152B, 0x02E5, 0x1AF4, 0x0D3A, 0x0A95, 0x1D5B, 0x054A, 0x1284
};
private static class AcquireTask implements Runnable {
private final ADIS16448_IMU imu;
public AcquireTask(final ADIS16448_IMU imu) {
this.imu = imu;
}
@Override
public void run() {
imu.acquire();
}
}
// CRC-16 Look-Up Table
private int[] m_adiscrc = {
0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF,
0x1F3F, 0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890,
0x1E3D, 0x09F3, 0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992,
0x0102, 0x16CC, 0x0EDD, 0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD,
0x1C39, 0x0BF7, 0x13E6, 0x0428, 0x0387, 0x1449, 0x0C58, 0x1B96,
0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8, 0x0B76, 0x1367, 0x04A9,
0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74, 0x1265, 0x05AB,
0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A, 0x1A94,
0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1,
0x060C, 0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3,
0x1933, 0x0EFD, 0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C,
0x0408, 0x13C6, 0x0BD7, 0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7,
0x1B37, 0x0CF9, 0x14E8, 0x0326, 0x0489, 0x1347, 0x0B56, 0x1C98,
0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B, 0x1245, 0x0A54, 0x1D9A,
0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A, 0x156B, 0x02A5,
0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040, 0x178E,
0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3,
0x1123, 0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C,
0x0C18, 0x1BD6, 0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7,
0x1327, 0x04E9, 0x1CF8, 0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488,
0x1225, 0x05EB, 0x1DFA, 0x0A34, 0x0D9B, 0x1A55, 0x0244, 0x158A,
0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4, 0x056A, 0x1D7B, 0x0AB5,
0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060, 0x1871, 0x0FBF,
0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E, 0x1080,
0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD,
0x1429, 0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386,
0x0B16, 0x1CD8, 0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9,
0x0A14, 0x1DDA, 0x05CB, 0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB,
0x152B, 0x02E5, 0x1AF4, 0x0D3A, 0x0A95, 0x1D5B, 0x054A, 0x1284
};
/** Creates a new ADIS16448_IMU object. */
public ADIS16448_IMU() {
@@ -278,12 +239,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
* @param port The SPI Port the gyro is plugged into
* @param cal_time Calibration time
*/
@SuppressWarnings("this-escape")
public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
m_yaw_axis = yaw_axis;
m_spi_port = port;
m_acquire_task = new Thread(new AcquireTask(this));
m_acquire_task = new Thread(this::acquire);
m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value);
if (m_simDevice != null) {
@@ -301,22 +261,26 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
if (m_simDevice == null) {
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
Timer.delay(0.01); // Wait 10ms
m_reset_out.close();
var reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
Timer.delay(0.01);
reset_out.close();
m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
Timer.delay(0.25); // Wait 250ms
Timer.delay(0.25); // Wait for reset to complete
configCalTime(cal_time);
m_spi = new SPI(m_spi_port);
m_spi.setClockRate(1000000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveLow();
if (!switchToStandardSPI()) {
return;
}
boolean needsFlash = false;
// Set IMU internal decimation to 1 (ODR = 819.2 SPS / (1 + 1) = 409.6Hz), BW = 204.8Hz
if (readRegister(SMPL_PRD) != 0x0001) {
writeRegister(SMPL_PRD, 0x0001);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling flash update.",
false);
@@ -325,7 +289,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
// Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP)
if (readRegister(MSC_CTRL) != 0x0016) {
writeRegister(MSC_CTRL, 0x0016);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling flash update.",
false);
@@ -334,7 +298,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
// Disable IMU internal Bartlett filter (204Hz BW is sufficient) and set IMU scale factor
if (readRegister(SENS_AVG) != 0x0400) {
writeRegister(SENS_AVG, 0x0400);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16448: SENS_AVG register configuration inconsistent! Scheduling flash update.",
false);
@@ -342,7 +306,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
// Clear offset registers
if (readRegister(XGYRO_OFF) != 0x0000) {
writeRegister(XGYRO_OFF, 0x0000);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16448: XGYRO_OFF register configuration inconsistent! Scheduling flash update.",
false);
@@ -350,7 +314,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
if (readRegister(YGYRO_OFF) != 0x0000) {
writeRegister(YGYRO_OFF, 0x0000);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16448: YGYRO_OFF register configuration inconsistent! Scheduling flash update.",
false);
@@ -358,26 +322,27 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
if (readRegister(ZGYRO_OFF) != 0x0000) {
writeRegister(ZGYRO_OFF, 0x0000);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16448: ZGYRO_OFF register configuration inconsistent! Scheduling flash update.",
false);
}
// If any registers on the IMU don't match the config, trigger a flash update
if (m_needs_flash) {
if (needsFlash) {
DriverStation.reportWarning(
"ADIS16448: Register configuration changed! Starting IMU flash update.", false);
writeRegister(GLOB_CMD, 0x0008);
// Wait long enough for the flash update to finish (75ms minimum as per the datasheet)
Timer.delay(0.5);
DriverStation.reportWarning("ADIS16448: Flash update finished!", false);
m_needs_flash = false;
} else {
DriverStation.reportWarning(
"ADIS16448: and RAM configuration consistent. No flash update required!", false);
"ADIS16448: Flash and RAM configuration consistent. No flash update required!", false);
}
// Set up the interrupt
m_auto_interrupt = new DigitalInput(10); // MXP DIO0
// Configure standard SPI
if (!switchToAutoSPI()) {
return;
@@ -389,13 +354,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000));
} catch (InterruptedException e) {
}
// Execute calibration routine
// Execute calibration routine and reset accumulated offsets
calibrate();
// Reset accumulated offsets
reset();
// Indicate to the acquire loop that we're done starting up
m_start_up_mode = false;
// Let the user know the IMU was initiallized successfully
// Let the user know the IMU was initialized successfully
DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false);
// Drive MXP PWM5 (IMU ready LED) low (active low)
m_status_led = new DigitalOutput(19);
@@ -418,34 +381,14 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
return m_connected;
}
private static int toUShort(byte[] buf) {
return (((buf[0] & 0xFF) << 8) + ((buf[1] & 0xFF) << 0));
private static int toUShort(int upper, int lower) {
return ((upper & 0xFF) << 8) + (lower & 0xFF);
}
private static int toUByte(int... buf) {
return (buf[0] & 0xFF);
private static int toShort(int upper, int lower) {
return (short) (((upper & 0xFF) << 8) + (lower & 0xFF));
}
private static int toUShort(int... buf) {
return (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
}
/** */
private static long toULong(int sint) {
return sint & 0x00000000FFFFFFFFL;
}
/** */
private static int toShort(int... buf) {
return (short) (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
}
/** */
private static int toInt(int... buf) {
return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF);
}
/** */
private boolean switchToStandardSPI() {
// Check to see whether the acquire thread is active. If so, wait for it to stop
// producing data.
@@ -459,13 +402,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
}
System.out.println("Paused the IMU processing thread successfully!");
// Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
if (m_spi != null && m_auto_configured) {
if (m_auto_configured) {
m_spi.stopAuto();
// We need to get rid of all the garbage left in the auto SPI buffer after
// stopping it.
// Sometimes data magically reappears, so we have to check the buffer size a
// couple of times
// to be sure we got it all. Yuck.
// couple of times to be sure we got it all. Yuck.
int[] trashBuffer = new int[200];
try {
Thread.sleep(100);
@@ -473,57 +415,25 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
}
int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
while (data_count > 0) {
/* Dequeue 200 at a time, or the remainder of the buffer if less than 200 */
// Dequeue 200 at a time, or the remainder of the buffer if less than 200
m_spi.readAutoReceivedData(trashBuffer, Math.min(200, data_count), 0);
/* Update remaining buffer count */
// Update remaining buffer count
data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
}
System.out.println("Paused auto SPI successfully.");
}
}
// There doesn't seem to be a SPI port active. Let's try to set one up
if (m_spi == null) {
System.out.println("Setting up a new SPI port.");
m_spi = new SPI(m_spi_port);
m_spi.setClockRate(1000000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveLow();
readRegister(PROD_ID); // Dummy read
// Validate the product ID
if (readRegister(PROD_ID) != 16448) {
DriverStation.reportError("Could not find ADIS16448", false);
close();
return false;
}
return true;
} else {
// Maybe the SPI port is active, but not in auto SPI mode? Try to read the
// product ID.
readRegister(PROD_ID); // dummy read
if (readRegister(PROD_ID) != 16448) {
DriverStation.reportError("Could not find an ADIS16448", false);
close();
return false;
} else {
return true;
}
readRegister(PROD_ID); // Dummy read
// Validate the product ID
if (readRegister(PROD_ID) != 16448) {
DriverStation.reportError("Could not find ADIS16448", false);
close();
return false;
}
return true;
}
/** */
boolean switchToAutoSPI() {
// No SPI port has been set up. Go set one up first.
if (m_spi == null) {
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to start/restart auto SPI", false);
return false;
}
}
// Only set up the interrupt if needed.
if (m_auto_interrupt == null) {
m_auto_interrupt = new DigitalInput(10); // MXP DIO0
}
// The auto SPI controller gets angry if you try to set up two instances on one
// bus.
if (!m_auto_configured) {
@@ -537,7 +447,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
// Kick off auto SPI (Note: Device configuration impossible after auto SPI is
// activated)
m_spi.startAutoTrigger(m_auto_interrupt, true, false);
// Check to see if the acquire thread is running. If not, kick one off.
if (!m_acquire_task.isAlive()) {
m_first_run = true;
@@ -566,19 +475,14 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
* @return 0 if success, 1 if no change, 2 if error.
*/
public int configDecRate(int decimationRate) {
// Switches the active SPI port to standard SPI mode, writes a new value to
// the DECIMATE register in the IMU, and re-enables auto SPI.
//
// This function enters standard SPI mode, writes a new DECIMATE setting to
// the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
int writeValue;
int readbackValue;
// Switches the active SPI port to standard SPI mode, writes a new value to the DECIMATE
// register in the IMU, adjusts the sample scale factor, and re-enables auto SPI.
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
return 2;
}
/* Check max */
// Check max
if (decimationRate > 9) {
DriverStation.reportError(
"Attempted to write an invalid decimation value. Capping at 9", false);
@@ -590,16 +494,16 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
decimationRate = 0;
}
/* Shift decimation setting to correct position and select internal sync */
writeValue = (decimationRate << 8) | 0x1;
// Shift decimation setting to correct position and select internal sync
int writeValue = (decimationRate << 8) | 0x1;
/* Apply to IMU */
// Apply to IMU
writeRegister(SMPL_PRD, writeValue);
/* Perform read back to verify write */
readbackValue = readRegister(SMPL_PRD);
// Perform read back to verify write
int readbackValue = readRegister(SMPL_PRD);
/* Throw error for invalid write */
// Throw error for invalid write
if (readbackValue != writeValue) {
DriverStation.reportError("ADIS16448 SMPL_PRD write failed.", false);
}
@@ -612,7 +516,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
}
/**
* Configures calibration time
* Configures calibration time.
*
* @param new_cal_time New calibration time
* @return 1 if the new calibration time is the same as the current one else 0
@@ -664,17 +568,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
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_rate_offset_x + "Y Off: " + m_gyro_rate_offset_y + "Z Off: " +
// m_gyro_rate_offset_z);
reset();
}
}
/**
* Sets the yaw axis
* Sets the yaw axis.
*
* @param yaw_axis The new yaw axis to use
* @return 1 if the new yaw axis is the same as the current one else 0.
@@ -689,16 +588,12 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
}
private int readRegister(final int reg) {
// ByteBuffer buf = ByteBuffer.allocateDirect(2);
final byte[] buf = new byte[2];
// buf.order(ByteOrder.BIG_ENDIAN);
buf[0] = (byte) (reg & 0x7f);
buf[1] = (byte) 0;
final byte[] buf = {(byte) (reg & 0x7f), 0};
m_spi.write(buf, 2);
m_spi.read(false, buf, 2);
return toUShort(buf);
return toUShort(buf[0], buf[1]);
}
private void writeRegister(final int reg, final int val) {
@@ -759,7 +654,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
System.out.println("Finished cleaning up after the IMU driver.");
}
/** */
private void acquire() {
// Set data packet length
final int dataset_len = 29; // 18 data points + timestamp
@@ -767,36 +661,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
// Set up buffers and variables
int[] buffer = new int[BUFFER_SIZE];
int data_count = 0;
int data_remainder = 0;
int data_to_read = 0;
int bufferAvgIndex = 0;
double previous_timestamp = 0.0;
double delta_angle = 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 mag_x = 0.0;
double mag_y = 0.0;
double mag_z = 0.0;
double baro = 0.0;
double temp = 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;
double compAngleX = 0.0;
double compAngleY = 0.0;
double accelAngleX = 0.0;
double accelAngleY = 0.0;
while (true) {
// Sleep loop for 5ms
// Wait for data
try {
Thread.sleep(5);
} catch (InterruptedException e) {
@@ -805,91 +674,75 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
if (m_thread_active) {
m_thread_idle = false;
data_count =
m_spi.readAutoReceivedData(
buffer, 0, 0); // Read number of bytes currently stored in the buffer
data_remainder =
data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
data_to_read = data_count - data_remainder; // Remove incomplete data from read count
// Read number of bytes currently stored in the buffer
int data_count = m_spi.readAutoReceivedData(buffer, 0, 0);
// Check if frame is incomplete
int data_remainder = data_count % dataset_len;
// Remove incomplete data from read count
int data_to_read = data_count - data_remainder;
if (data_to_read > BUFFER_SIZE) {
DriverStation.reportWarning(
"ADIS16448 data processing thread overrun has occurred!", false);
data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
}
m_spi.readAutoReceivedData(
buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets)
// Read data from DMA buffer (only complete sets)
m_spi.readAutoReceivedData(buffer, data_to_read, 0);
// Could be multiple data sets in the buffer. Handle each one.
for (int i = 0; i < data_to_read; i += dataset_len) {
// Calculate CRC-16 on each data packet
int calc_crc = 0x0000FFFF; // Starting word
int read_byte = 0;
int imu_crc = 0;
for (int k = 5;
k < 27;
k += 2) { // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status &
// CRC)
read_byte = buffer[i + k + 1]; // Process LSB
calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
read_byte = buffer[i + k]; // Process MSB
calc_crc = (calc_crc >>> 8) ^ adiscrc[(calc_crc & 0x000000FF) ^ read_byte];
int calc_crc = 0xFFFF; // Starting word
// Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status & CRC)
for (int k = 5; k < 27; k += 2) {
// Process LSB
calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ buffer[i + k + 1]];
// Process MSB
calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ buffer[i + k]];
}
calc_crc = ~calc_crc & 0xFFFF; // Complement
calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF; // Flip LSB & MSB
imu_crc = toUShort(buffer[i + 27], buffer[i + 28]); // Extract DUT CRC from data buffer
// Complement
calc_crc = ~calc_crc & 0xFFFF;
// Flip LSB & MSB
calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF;
// Extract DUT CRC from data buffer
int imu_crc = toUShort(buffer[i + 27], buffer[i + 28]);
if (calc_crc == imu_crc) {
// Timestamp is at buffer[i]
m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
// Scale sensor data
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);
mag_x = (toShort(buffer[i + 17], buffer[i + 18]) * 0.1429);
mag_y = (toShort(buffer[i + 19], buffer[i + 20]) * 0.1429);
mag_z = (toShort(buffer[i + 21], buffer[i + 22]) * 0.1429);
baro = (toShort(buffer[i + 23], buffer[i + 24]) * 0.02);
temp = (toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0);
double gyro_rate_x = toShort(buffer[i + 5], buffer[i + 6]) * 0.04;
double gyro_rate_y = toShort(buffer[i + 7], buffer[i + 8]) * 0.04;
double gyro_rate_z = toShort(buffer[i + 9], buffer[i + 10]) * 0.04;
double accel_x = toShort(buffer[i + 11], buffer[i + 12]) * 0.833;
double accel_y = toShort(buffer[i + 13], buffer[i + 14]) * 0.833;
double accel_z = toShort(buffer[i + 15], buffer[i + 16]) * 0.833;
double mag_x = toShort(buffer[i + 17], buffer[i + 18]) * 0.1429;
double mag_y = toShort(buffer[i + 19], buffer[i + 20]) * 0.1429;
double mag_z = toShort(buffer[i + 21], buffer[i + 22]) * 0.1429;
double baro = toShort(buffer[i + 23], buffer[i + 24]) * 0.02;
double temp = toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0;
// Convert scaled sensor data to SI units (for tilt calculations)
// TODO: Should the unit outputs be selectable?
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;
double gyro_rate_x_si = Math.toRadians(gyro_rate_x);
double gyro_rate_y_si = Math.toRadians(gyro_rate_y);
// double gyro_rate_z_si = Math.toRadians(gyro_rate_z);
double accel_x_si = accel_x * kGrav;
double accel_y_si = accel_y * kGrav;
double accel_z_si = accel_z * kGrav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
// Calculate alpha for use with the complementary filter
m_alpha = m_tau / (m_tau + m_dt);
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX = Math.atan2(-accel_x_si, Math.hypot(accel_y_si, -accel_z_si));
double accelAngleY = Math.atan2(accel_y_si, Math.hypot(-accel_x_si, -accel_z_si));
// Calculate complementary filter
if (m_first_run) {
// Set up inclinometer calculations for first run
accelAngleX =
Math.atan2(
-accel_x_si,
Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
accelAngleY =
Math.atan2(
accel_y_si,
Math.sqrt((-accel_x_si * -accel_x_si) + (-accel_z_si * -accel_z_si)));
compAngleX = accelAngleX;
compAngleY = accelAngleY;
} else {
// Run inclinometer calculations
accelAngleX =
Math.atan2(
-accel_x_si,
Math.sqrt((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
accelAngleY =
Math.atan2(
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_rate_y_si);
@@ -900,13 +753,11 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
synchronized (this) {
// Ignore first, integrated sample
if (m_first_run) {
m_integ_gyro_angle_x = 0.0;
m_integ_gyro_angle_y = 0.0;
m_integ_gyro_angle_z = 0.0;
reset();
} else {
// Accumulate gyro for offset calibration
// Add to buffer
bufferAvgIndex = m_accum_count % m_avg_size;
int bufferAvgIndex = m_accum_count % m_avg_size;
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;
@@ -925,79 +776,28 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
m_mag_z = mag_z;
m_baro = baro;
m_temp = temp;
m_compAngleX = compAngleX * rad_to_deg;
m_compAngleY = compAngleY * rad_to_deg;
m_accelAngleX = accelAngleX * rad_to_deg;
m_accelAngleY = accelAngleY * rad_to_deg;
m_compAngleX = Math.toDegrees(compAngleX);
m_compAngleY = Math.toDegrees(compAngleY);
m_accelAngleX = Math.toDegrees(accelAngleX);
m_accelAngleY = Math.toDegrees(accelAngleY);
// Accumulate gyro for angle integration and publish to global variables
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");
}
m_first_run = false;
} else {
// System.out.println("Bad CRC");
/*
* System.out.println("Calc CRC: " + calc_crc);
* System.out.println("IMU CRC: " + imu_crc);
* System.out.println(
* buffer[i] + " " +
* (buffer[i + 1]) + " " + (buffer[i + 2]) + " " +
* (buffer[i + 3]) + " " + (buffer[i + 4]) + " " +
* (buffer[i + 5]) + " " + (buffer[i + 6]) + " " +
* (buffer[i + 7]) + " " + (buffer[i + 8]) + " " +
* (buffer[i + 9]) + " " + (buffer[i + 10]) + " " +
* (buffer[i + 11]) + " " + (buffer[i + 12]) + " " +
* (buffer[i + 13]) + " " + (buffer[i + 14]) + " " +
* (buffer[i + 15]) + " " + (buffer[i + 16]) + " " +
* (buffer[i + 17]) + " " + (buffer[i + 18]) + " " +
* (buffer[i + 19]) + " " + (buffer[i + 20]) + " " +
* (buffer[i + 21]) + " " + (buffer[i + 22]) + " " +
* (buffer[i + 23]) + " " + (buffer[i + 24]) + " " +
* (buffer[i + 25]) + " " + (buffer[i + 26]) + " " +
* (buffer[i + 27]) + " " + (buffer[i + 28]));
*/
}
}
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
delta_angle = 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;
mag_x = 0.0;
mag_y = 0.0;
mag_z = 0.0;
baro = 0.0;
temp = 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;
compAngleX = 0.0;
compAngleY = 0.0;
accelAngleX = 0.0;
accelAngleY = 0.0;
}
}
}
/**
* @param compAngle
* @param accAngle
* @return
*/
private double formatFastConverge(double compAngle, double accAngle) {
if (compAngle > accAngle + Math.PI) {
compAngle = compAngle - 2.0 * Math.PI;
@@ -1007,25 +807,6 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
return compAngle;
}
/**
* @param compAngle
* @return
*/
private double formatRange0to2PI(double compAngle) {
while (compAngle >= 2 * Math.PI) {
compAngle = compAngle - 2.0 * Math.PI;
}
while (compAngle < 0.0) {
compAngle = compAngle + 2.0 * Math.PI;
}
return compAngle;
}
/**
* @param accelAngle
* @param accelZ
* @return
*/
private double formatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = Math.PI - accelAngle;
@@ -1035,20 +816,10 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
return accelAngle;
}
/**
* @param compAngle
* @param accelAngle
* @param omega
* @return
*/
private double compFilterProcess(double compAngle, double accelAngle, double omega) {
compAngle = formatFastConverge(compAngle, accelAngle);
compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
compAngle = formatRange0to2PI(compAngle);
if (compAngle > Math.PI) {
compAngle = compAngle - 2.0 * Math.PI;
}
return compAngle;
return MathUtil.angleModulus(compAngle);
}
/**
@@ -1167,7 +938,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
if (m_simAccelX != null) {
return m_simAccelX.get();
}
return m_accel_x * 9.81;
return m_accel_x * kGrav;
}
/**
@@ -1179,7 +950,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
if (m_simAccelY != null) {
return m_simAccelY.get();
}
return m_accel_y * 9.81;
return m_accel_y * kGrav;
}
/**
@@ -1191,7 +962,7 @@ public class ADIS16448_IMU implements AutoCloseable, Sendable {
if (m_simAccelZ != null) {
return m_simAccelZ.get();
}
return m_accel_z * 9.81;
return m_accel_z * kGrav;
}
/**

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

@@ -2,55 +2,25 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2016. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj;
// import java.lang.FdLibm.Pow;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
// CHECKSTYLE.OFF: TypeName
// CHECKSTYLE.OFF: MemberName
// CHECKSTYLE.OFF: SummaryJavadoc
// CHECKSTYLE.OFF: UnnecessaryParentheses
// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder
// CHECKSTYLE.OFF: NonEmptyAtclauseDescription
// CHECKSTYLE.OFF: MissingOverride
// CHECKSTYLE.OFF: AtclauseOrder
// CHECKSTYLE.OFF: LocalVariableName
// CHECKSTYLE.OFF: RedundantModifier
// CHECKSTYLE.OFF: AbbreviationAsWordInName
// CHECKSTYLE.OFF: ParameterName
// CHECKSTYLE.OFF: EmptyCatchBlock
// CHECKSTYLE.OFF: MissingJavadocMethod
// CHECKSTYLE.OFF: MissingSwitchDefault
// CHECKSTYLE.OFF: VariableDeclarationUsageDistance
// CHECKSTYLE.OFF: ArrayTypeStyle
/** This class is for the ADIS16470 IMU that connects to the RoboRIO SPI port. */
@SuppressWarnings({
"unused",
"PMD.RedundantFieldInitializer",
"PMD.ImmutableField",
"PMD.SingularField",
"PMD.CollapsibleIfStatements",
"PMD.MissingOverride",
"PMD.EmptyIfStmt",
"PMD.EmptyStatementNotInLoop"
})
@SuppressWarnings("PMD.RedundantFieldInitializer")
public class ADIS16470_IMU implements AutoCloseable, Sendable {
/* ADIS16470 Register Map Declaration */
private static final int FLASH_CNT = 0x00; // Flash memory write count
@@ -157,29 +127,29 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
/** ADIS16470 calibration times. */
public enum CalibrationTime {
/** 32 ms calibration time */
/** 32 ms calibration time. */
_32ms(0),
/** 64 ms calibration time */
/** 64 ms calibration time. */
_64ms(1),
/** 128 ms calibration time */
/** 128 ms calibration time. */
_128ms(2),
/** 256 ms calibration time */
/** 256 ms calibration time. */
_256ms(3),
/** 512 ms calibration time */
/** 512 ms calibration time. */
_512ms(4),
/** 1 s calibration time */
/** 1 s calibration time. */
_1s(5),
/** 2 s calibration time */
/** 2 s calibration time. */
_2s(6),
/** 4 s calibration time */
/** 4 s calibration time. */
_4s(7),
/** 8 s calibration time */
/** 8 s calibration time. */
_8s(8),
/** 16 s calibration time */
/** 16 s calibration time. */
_16s(9),
/** 32 s calibration time */
/** 32 s calibration time. */
_32s(10),
/** 64 s calibration time */
/** 64 s calibration time. */
_64s(11);
private final int value;
@@ -196,25 +166,23 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* configured by the user to refer to an X, Y, or Z axis.
*/
public enum IMUAxis {
/** The IMU's X axis */
/** The IMU's X axis. */
kX,
/** The IMU's Y axis */
/** The IMU's Y axis. */
kY,
/** The IMU's Z axis */
/** The IMU's Z axis. */
kZ,
/** The user-configured yaw axis */
/** The user-configured yaw axis. */
kYaw,
/** The user-configured pitch axis */
/** The user-configured pitch axis. */
kPitch,
/** The user-configured roll axis */
/** The user-configured roll axis. */
kRoll,
}
// Static Constants
private static final double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */
private static final double rad_to_deg = 57.2957795;
private static final double deg_to_rad = 0.0174532;
private static final double grav = 9.81;
private static final double kDeltaAngleSf = 2160.0 / 2147483648.0; // 2160 / (2^31)
private static final double kGrav = 9.81;
// User-specified axes
private IMUAxis m_yaw_axis;
@@ -225,9 +193,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
private double m_gyro_rate_x = 0.0;
private double m_gyro_rate_y = 0.0;
private double m_gyro_rate_z = 0.0;
private double m_average_gyro_rate_x = 0.0;
private double m_average_gyro_rate_y = 0.0;
private double m_average_gyro_rate_z = 0.0;
private double m_accel_x = 0.0;
private double m_accel_y = 0.0;
private double m_accel_z = 0.0;
@@ -240,7 +205,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
// Complementary filter variables
private double m_dt = 0.0;
private double m_alpha = 0.0;
private double m_tau = 1.0;
private static final double kTau = 1.0;
private double m_compAngleX = 0.0;
private double m_compAngleY = 0.0;
private double m_accelAngleX = 0.0;
@@ -253,13 +218,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
private volatile boolean m_thread_idle = false;
private boolean m_auto_configured = false;
private double m_scaled_sample_rate = 2500.0;
private boolean m_needs_flash = false;
// Resources
private SPI m_spi;
private SPI.Port m_spi_port;
private DigitalInput m_auto_interrupt;
private DigitalOutput m_reset_out;
private DigitalInput m_reset_in;
private DigitalOutput m_status_led;
private Thread m_acquire_task;
@@ -277,19 +240,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
private SimDouble m_simAccelY;
private SimDouble m_simAccelZ;
private static class AcquireTask implements Runnable {
private ADIS16470_IMU imu;
public AcquireTask(ADIS16470_IMU imu) {
this.imu = imu;
}
@Override
public void run() {
imu.acquire();
}
}
/**
* Creates a new ADIS16740 IMU object.
*
@@ -328,7 +278,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @param port The SPI Port the gyro is plugged into
* @param cal_time Calibration time
*/
@SuppressWarnings("this-escape")
public ADIS16470_IMU(
IMUAxis yaw_axis,
IMUAxis pitch_axis,
@@ -361,7 +310,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
m_calibration_time = cal_time.value;
m_spi_port = port;
m_acquire_task = new Thread(new AcquireTask(this));
m_acquire_task = new Thread(this::acquire);
m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value);
if (m_simDevice != null) {
@@ -382,20 +331,24 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
// Relies on the RIO hardware by default configuring an output as low
// and configuring an input as high Z. The 10k pull-up resistor internal to the
// IMU then forces the reset line high for normal operation.
m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
Timer.delay(0.01); // Wait 10ms
m_reset_out.close();
var reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
Timer.delay(0.01);
reset_out.close();
m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
Timer.delay(0.25); // Wait 250ms for reset to complete
Timer.delay(0.25); // Wait for reset to complete
m_spi = new SPI(m_spi_port);
m_spi.setClockRate(2000000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveLow();
if (!switchToStandardSPI()) {
return;
}
boolean needsFlash = false;
// Set IMU internal decimation to 4 (ODR = 2000 SPS / (4 + 1) = 400Hz), BW = 200Hz
if (readRegister(DEC_RATE) != 0x0004) {
writeRegister(DEC_RATE, 0x0004);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16470: DEC_RATE register configuration inconsistent! Scheduling flash update.",
false);
@@ -404,7 +357,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
// Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and PoP
if (readRegister(MSC_CTRL) != 0x0001) {
writeRegister(MSC_CTRL, 0x0001);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling flash update.",
false);
@@ -413,28 +366,27 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
// Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient)
if (readRegister(FILT_CTRL) != 0x0000) {
writeRegister(FILT_CTRL, 0x0000);
m_needs_flash = true;
needsFlash = true;
DriverStation.reportWarning(
"ADIS16470: FILT_CTRL register configuration inconsistent! Scheduling flash update.",
false);
}
// If any registers on the IMU don't match the config, trigger a flash update
if (m_needs_flash) {
if (needsFlash) {
DriverStation.reportWarning(
"ADIS16470: Register configuration changed! Starting IMU flash update.", false);
writeRegister(GLOB_CMD, 0x0008);
// Wait long enough for the flash update to finish (72ms minimum as per the datasheet)
Timer.delay(0.3);
DriverStation.reportWarning("ADIS16470: Flash update finished!", false);
m_needs_flash = false;
} else {
DriverStation.reportWarning(
"ADIS16470: Flash and RAM configuration consistent. No flash update required!", false);
}
// Configure continuous bias calibration time based on user setting
writeRegister(NULL_CNFG, (m_calibration_time | 0x0700));
writeRegister(NULL_CNFG, m_calibration_time | 0x0700);
// Notify DS that IMU calibration delay is active
DriverStation.reportWarning("ADIS16470: Starting initial calibration delay.", false);
@@ -449,12 +401,14 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
// Write offset calibration command to IMU
writeRegister(GLOB_CMD, 0x0001);
// Configure interrupt on SPI CS1
m_auto_interrupt = new DigitalInput(26);
// Configure and enable auto SPI
if (!switchToAutoSPI()) {
return;
}
// Let the user know the IMU was initiallized successfully
// Let the user know the IMU was initialized successfully
DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false);
// Drive "Ready" LED low
@@ -478,36 +432,12 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
return m_connected;
}
/**
* @param buf
* @return
*/
private static int toUShort(ByteBuffer buf) {
return buf.getShort(0) & 0xFFFF;
private static int toShort(int upper, int lower) {
return (short) (((upper & 0xFF) << 8) + (lower & 0xFF));
}
/**
* @param sint
* @return
*/
private static long toULong(int sint) {
return sint & 0x00000000FFFFFFFFL;
}
/**
* @param buf
* @return
*/
private static int toShort(int... buf) {
return (short) (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF));
}
/**
* @param buf
* @return
*/
private static int toInt(int... buf) {
return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF);
private static int toInt(int msb, int byte2, int byte3, int lsb) {
return (msb & 0xFF) << 24 | (byte2 & 0xFF) << 16 | (byte3 & 0xFF) << 8 | (lsb & 0xFF);
}
/**
@@ -528,13 +458,12 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
System.out.println("Paused the IMU processing thread successfully!");
// Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
if (m_spi != null && m_auto_configured) {
if (m_auto_configured) {
m_spi.stopAuto();
// We need to get rid of all the garbage left in the auto SPI buffer after
// stopping it.
// Sometimes data magically reappears, so we have to check the buffer size a
// couple of times
// to be sure we got it all. Yuck.
// couple of times to be sure we got it all. Yuck.
int[] trashBuffer = new int[200];
try {
Thread.sleep(100);
@@ -548,34 +477,14 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
System.out.println("Paused auto SPI successfully.");
}
}
// There doesn't seem to be a SPI port active. Let's try to set one up
if (m_spi == null) {
System.out.println("Setting up a new SPI port.");
m_spi = new SPI(m_spi_port);
m_spi.setClockRate(2000000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveLow();
readRegister(PROD_ID); // Dummy read
// Validate the product ID
if (readRegister(PROD_ID) != 16982) {
DriverStation.reportError("Could not find ADIS16470", false);
close();
return false;
}
return true;
} else {
// Maybe the SPI port is active, but not in auto SPI mode? Try to read the
// product ID.
readRegister(PROD_ID); // dummy read
if (readRegister(PROD_ID) != 16982) {
DriverStation.reportError("Could not find an ADIS16470", false);
close();
return false;
} else {
return true;
}
readRegister(PROD_ID); // Dummy read
// Validate the product ID
if (readRegister(PROD_ID) != 16982) {
DriverStation.reportError("Could not find an ADIS16470", false);
close();
return false;
}
return true;
}
/**
@@ -584,18 +493,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @return True if successful, false otherwise.
*/
boolean switchToAutoSPI() {
// No SPI port has been set up. Go set one up first.
if (m_spi == null) {
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to start/restart auto SPI", false);
return false;
}
}
// Only set up the interrupt if needed.
if (m_auto_interrupt == null) {
// Configure interrupt on SPI CS1
m_auto_interrupt = new DigitalInput(26);
}
// The auto SPI controller gets angry if you try to set up two instances on one
// bus.
if (!m_auto_configured) {
@@ -610,7 +507,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
// activated)
// DR High = Data good (data capture should be triggered on the rising edge)
m_spi.startAutoTrigger(m_auto_interrupt, true, false);
// Check to see if the acquire thread is running. If not, kick one off.
if (!m_acquire_task.isAlive()) {
m_first_run = true;
@@ -633,10 +529,10 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
/**
* Configures calibration time
* Configures calibration time.
*
* @param new_cal_time New calibration time
* @return 1 if the new calibration time is the same as the current one else 0
* @return 0 if success, 1 if no change, 2 if error.
*/
public int configCalTime(CalibrationTime new_cal_time) {
if (m_calibration_time == new_cal_time.value) {
@@ -647,7 +543,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
return 2;
}
m_calibration_time = new_cal_time.value;
writeRegister(NULL_CNFG, (m_calibration_time | 0x700));
writeRegister(NULL_CNFG, m_calibration_time | 0x700);
if (!switchToAutoSPI()) {
DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
return 2;
@@ -662,11 +558,8 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @return 0 if success, 1 if no change, 2 if error.
*/
public int configDecRate(int decimationRate) {
// Switches the active SPI port to standard SPI mode, writes a new value to
// the DECIMATE register in the IMU, and re-enables auto SPI.
//
// This function enters standard SPI mode, writes a new DECIMATE setting to
// the IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
// Switches the active SPI port to standard SPI mode, writes a new value to the DECIMATE
// register in the IMU, adjusts the sample scale factor, and re-enables auto SPI.
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
return 2;
@@ -675,7 +568,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
DriverStation.reportError("Attempted to write an invalid decimation value.", false);
decimationRate = 1999;
}
m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0);
m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0;
writeRegister(DEC_RATE, decimationRate);
System.out.println("Decimation register: " + readRegister(DEC_RATE));
if (!switchToAutoSPI()) {
@@ -700,35 +593,22 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
}
/**
* @param reg
* @return
*/
private int readRegister(int reg) {
ByteBuffer buf = ByteBuffer.allocateDirect(2);
buf.order(ByteOrder.BIG_ENDIAN);
buf.put(0, (byte) (reg & 0x7f));
buf.put(1, (byte) 0);
final byte[] buf = {(byte) (reg & 0x7f), 0};
m_spi.write(buf, 2);
m_spi.read(false, buf, 2);
return toUShort(buf);
return (buf[0] << 8) & buf[1];
}
/**
* @param reg
* @param val
*/
private void writeRegister(int reg, int val) {
ByteBuffer buf = ByteBuffer.allocateDirect(2);
// low byte
buf.put(0, (byte) (0x80 | reg));
buf.put(1, (byte) (val & 0xff));
final byte[] buf = {(byte) (0x80 | reg), (byte) (val & 0xff)};
m_spi.write(buf, 2);
// high byte
buf.put(0, (byte) (0x81 | reg));
buf.put(1, (byte) (val >> 8));
buf[0] = (byte) (0x81 | reg);
buf[1] = (byte) (val >> 8);
m_spi.write(buf, 2);
}
@@ -764,7 +644,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
System.out.println("Finished cleaning up after the IMU driver.");
}
/** */
private void acquire() {
// Set data packet length
final int dataset_len = 27; // 26 data points + timestamp
@@ -772,32 +651,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
// Set up buffers and variables
int[] buffer = new int[BUFFER_SIZE];
int data_count = 0;
int data_remainder = 0;
int data_to_read = 0;
double previous_timestamp = 0.0;
double delta_angle_x = 0.0;
double delta_angle_y = 0.0;
double delta_angle_z = 0.0;
double gyro_rate_x = 0.0;
double gyro_rate_y = 0.0;
double gyro_rate_z = 0.0;
double accel_x = 0.0;
double accel_y = 0.0;
double accel_z = 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;
double compAngleX = 0.0;
double compAngleY = 0.0;
double accelAngleX = 0.0;
double accelAngleY = 0.0;
while (true) {
// Sleep loop for 10ms
// Wait for data
try {
Thread.sleep(10);
} catch (InterruptedException e) {
@@ -806,108 +664,70 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
if (m_thread_active) {
m_thread_idle = false;
data_count =
m_spi.readAutoReceivedData(
buffer, 0, 0); // Read number of bytes currently stored in the buffer
data_remainder =
data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp
data_to_read = data_count - data_remainder; // Remove incomplete data from read count
/* Want to cap the data to read in a single read at the buffer size */
// Read number of bytes currently stored in the buffer
int data_count = m_spi.readAutoReceivedData(buffer, 0, 0);
// Check if frame is incomplete
int data_remainder = data_count % dataset_len;
// Remove incomplete data from read count
int data_to_read = data_count - data_remainder;
// Want to cap the data to read in a single read at the buffer size
if (data_to_read > BUFFER_SIZE) {
DriverStation.reportWarning(
"ADIS16470 data processing thread overrun has occurred!", false);
data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
}
m_spi.readAutoReceivedData(
buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets)
// Read data from DMA buffer (only complete sets)
m_spi.readAutoReceivedData(buffer, data_to_read, 0);
// Could be multiple data sets in the buffer. Handle each one.
for (int i = 0; i < data_to_read; i += dataset_len) {
// Timestamp is at buffer[i]
m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
// Get delta angle value for all 3 axes and scale by the elapsed time
// (based on timestamp)
double elapsed_time = m_scaled_sample_rate / (buffer[i] - previous_timestamp);
double delta_angle_x =
toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6])
* kDeltaAngleSf
/ elapsed_time;
double delta_angle_y =
toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10])
* kDeltaAngleSf
/ elapsed_time;
double delta_angle_z =
toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14])
* kDeltaAngleSf
/ elapsed_time;
/*
* System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
* buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
* previous_timestamp)) / 100.0));
* // DEBUG: Plot Sub-Array Data in Terminal
* for (int j = 0; j < data_to_read; j++) {
* System.out.print(buffer[j]);
* System.out.print(" ,");
* }
* System.out.println(" ");
* //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5],
* buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] -
* previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," +
* buffer[5] + "," + buffer[6]
* /*toShort(buffer[7], buffer[8]) + "," +
* toShort(buffer[9], buffer[10]) + "," +
* toShort(buffer[11], buffer[12]) + "," +
* toShort(buffer[13], buffer[14]) + "," +
* toShort(buffer[15], buffer[16]) + ","
* + toShort(buffer[17], buffer[18]));
*/
double gyro_rate_x = toShort(buffer[i + 15], buffer[i + 16]) / 10.0;
double gyro_rate_y = toShort(buffer[i + 17], buffer[i + 18]) / 10.0;
double gyro_rate_z = toShort(buffer[i + 19], buffer[i + 20]) / 10.0;
/*
* Get delta angle value for all 3 axes and scale by the elapsed time
* (based on timestamp)
*/
delta_angle_x =
(toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
/ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
delta_angle_y =
(toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf)
/ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
delta_angle_z =
(toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14])
* delta_angle_sf)
/ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0);
gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0);
gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0);
accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0);
accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0);
accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0);
double accel_x = toShort(buffer[i + 21], buffer[i + 22]) / 800.0;
double accel_y = toShort(buffer[i + 23], buffer[i + 24]) / 800.0;
double accel_z = toShort(buffer[i + 25], buffer[i + 26]) / 800.0;
// Convert scaled sensor data to SI units (for tilt calculations)
// TODO: Should the unit outputs be selectable?
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;
double gyro_rate_x_si = Math.toRadians(gyro_rate_x);
double gyro_rate_y_si = Math.toRadians(gyro_rate_y);
// double gyro_rate_z_si = Math.toRadians(gyro_rate_z);
double accel_x_si = accel_x * kGrav;
double accel_y_si = accel_y * kGrav;
double accel_z_si = accel_z * kGrav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
m_alpha = m_tau / (m_tau + m_dt);
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX = Math.atan2(accel_x_si, Math.hypot(accel_y_si, accel_z_si));
double accelAngleY = Math.atan2(accel_y_si, Math.hypot(accel_x_si, accel_z_si));
if (m_first_run) {
// Set up inclinometer calculations for first run
accelAngleX =
Math.atan2(
accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
accelAngleY =
Math.atan2(
accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
compAngleX = accelAngleX;
compAngleY = accelAngleY;
m_average_gyro_rate_x = 0.0;
m_average_gyro_rate_y = 0.0;
m_average_gyro_rate_z = 0.0;
} else {
// Run inclinometer calculations
accelAngleX =
Math.atan2(
accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si)));
accelAngleY =
Math.atan2(
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_rate_y_si);
@@ -915,15 +735,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
synchronized (this) {
/* Push data to global variables */
// Push data to global variables
if (m_first_run) {
/*
* Don't accumulate first run. previous_timestamp will be "very" old and the
* integration will end up way off
*/
m_integ_angle_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
// Don't accumulate first run. previous_timestamp will be "very" old and the
// integration will end up way off
reset();
} else {
m_integ_angle_x += delta_angle_x;
m_integ_angle_y += delta_angle_y;
@@ -935,57 +751,22 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
m_accel_x = accel_x;
m_accel_y = accel_y;
m_accel_z = accel_z;
m_compAngleX = compAngleX * rad_to_deg;
m_compAngleY = compAngleY * rad_to_deg;
m_accelAngleX = accelAngleX * rad_to_deg;
m_accelAngleY = accelAngleY * rad_to_deg;
m_average_gyro_rate_x += gyro_rate_x;
m_average_gyro_rate_y += gyro_rate_y;
m_average_gyro_rate_z += gyro_rate_z;
m_compAngleX = Math.toDegrees(compAngleX);
m_compAngleY = Math.toDegrees(compAngleY);
m_accelAngleX = Math.toDegrees(accelAngleX);
m_accelAngleY = Math.toDegrees(accelAngleY);
}
m_first_run = false;
}
// The inverse of data to read divided by dataset length, his is the number of iterations
// of the for loop inverted (so multiplication can be used instead of division)
double invTotalIterations = (double) dataset_len / data_to_read;
m_average_gyro_rate_x *= invTotalIterations;
m_average_gyro_rate_y *= invTotalIterations;
m_average_gyro_rate_z *= invTotalIterations;
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
delta_angle_x = 0.0;
delta_angle_y = 0.0;
delta_angle_z = 0.0;
gyro_rate_x = 0.0;
gyro_rate_y = 0.0;
gyro_rate_z = 0.0;
accel_x = 0.0;
accel_y = 0.0;
accel_z = 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;
compAngleX = 0.0;
compAngleY = 0.0;
accelAngleX = 0.0;
accelAngleY = 0.0;
}
}
}
/**
* @param compAngle
* @param accAngle
* @return
*/
private double formatFastConverge(double compAngle, double accAngle) {
if (compAngle > accAngle + Math.PI) {
compAngle = compAngle - 2.0 * Math.PI;
@@ -995,25 +776,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
return compAngle;
}
/**
* @param compAngle
* @return
*/
private double formatRange0to2PI(double compAngle) {
while (compAngle >= 2 * Math.PI) {
compAngle = compAngle - 2.0 * Math.PI;
}
while (compAngle < 0.0) {
compAngle = compAngle + 2.0 * Math.PI;
}
return compAngle;
}
/**
* @param accelAngle
* @param accelZ
* @return
*/
private double formatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = Math.PI - accelAngle;
@@ -1023,20 +785,10 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
return accelAngle;
}
/**
* @param compAngle
* @param accelAngle
* @param omega
* @return
*/
private double compFilterProcess(double compAngle, double accelAngle, double omega) {
compAngle = formatFastConverge(compAngle, accelAngle);
compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
compAngle = formatRange0to2PI(compAngle);
if (compAngle > Math.PI) {
compAngle = compAngle - 2.0 * Math.PI;
}
return compAngle;
return MathUtil.angleModulus(compAngle);
}
/**
@@ -1159,27 +911,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @return The Yaw axis angle in degrees (CCW positive).
*/
public synchronized double getAngle() {
return switch (m_yaw_axis) {
case kX -> {
if (m_simGyroAngleX != null) {
yield m_simGyroAngleX.get();
}
yield m_integ_angle_x;
}
case kY -> {
if (m_simGyroAngleY != null) {
yield m_simGyroAngleY.get();
}
yield m_integ_angle_y;
}
case kZ -> {
if (m_simGyroAngleZ != null) {
yield m_simGyroAngleZ.get();
}
yield m_integ_angle_z;
}
default -> 0.0;
};
return getAngle(m_yaw_axis);
}
/**
@@ -1226,27 +958,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @return Yaw axis angular rate in degrees per second (CCW positive).
*/
public synchronized double getRate() {
return switch (m_yaw_axis) {
case kX -> {
if (m_simGyroRateX != null) {
yield m_simGyroRateX.get();
}
yield m_gyro_rate_x;
}
case kY -> {
if (m_simGyroRateY != null) {
yield m_simGyroRateY.get();
}
yield m_gyro_rate_y;
}
case kZ -> {
if (m_simGyroRateZ != null) {
yield m_simGyroRateZ.get();
}
yield m_gyro_rate_z;
}
default -> 0.0;
};
return getRate(m_yaw_axis);
}
/**
@@ -1282,7 +994,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @return The acceleration in the X axis in meters per second squared.
*/
public synchronized double getAccelX() {
return m_accel_x * 9.81;
return m_accel_x * kGrav;
}
/**
@@ -1291,7 +1003,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @return The acceleration in the Y axis in meters per second squared.
*/
public synchronized double getAccelY() {
return m_accel_y * 9.81;
return m_accel_y * kGrav;
}
/**
@@ -1300,7 +1012,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* @return The acceleration in the Z axis in meters per second squared.
*/
public synchronized double getAccelZ() {
return m_accel_z * 9.81;
return m_accel_z * kGrav;
}
/**