Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add ADIS IMUs (#3777)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Matteo Kimura <mateus.sakata@gmail.com>
This commit is contained in:
Thad House
2021-12-30 19:43:53 -08:00
committed by GitHub
parent 315be873c4
commit 1f59ff72f9
7 changed files with 4418 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
styleguide/spotbugs-exclude.xml
View File

@@ -80,6 +80,14 @@
<Bug pattern="UC_USELESS_VOID_METHOD" />
<Class name="edu.wpi.first.wpilibj.templates.timed.Robot" />
</Match>
<Match>
<Bug pattern="URF_UNREAD_FIELD" />
<Class name="edu.wpi.first.wpilibj.ADIS16448_IMU" />
</Match>
<Match>
<Bug pattern="URF_UNREAD_FIELD" />
<Class name="edu.wpi.first.wpilibj.ADIS16470_IMU" />
</Match>
<Match>
<Bug pattern="URF_UNREAD_PUBLIC_OR_PROTECTED_FIELD" />
</Match>

857
wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp Normal file
View File

@@ -0,0 +1,857 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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) 2016-2020 Analog Devices Inc. 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. */
/* */
/* Modified by Juan Chong - frcsupport@analog.com */
/*----------------------------------------------------------------------------*/
#include "frc/ADIS16448_IMU.h"
#include <frc/DigitalInput.h>
#include <frc/DigitalOutput.h>
#include <frc/DigitalSource.h>
#include <frc/DriverStation.h>
#include <frc/SPI.h>
#include <frc/Timer.h>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <string>
#include <hal/HAL.h>
#include <networktables/NTSendableBuilder.h>
#include <wpi/numbers>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.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 {
template <typename S, typename... Args>
inline void ADISReportError(int32_t status, const char* file, int line,
const char* function, const S& format,
Args&&... args) {
frc::ReportErrorV(status, file, line, function, format,
fmt::make_args_checked<Args...>(format, args...));
}
} // namespace
#define REPORT_WARNING(msg) \
ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
#define REPORT_ERROR(msg) \
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
ADIS16448_IMU::ADIS16448_IMU() : ADIS16448_IMU(kZ, SPI::Port::kMXP, 4) {}
ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
uint16_t cal_time)
: m_yaw_axis(yaw_axis), m_spi_port(port) {
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
// 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;
new DigitalInput(18); // Set MXP DIO8 high
Wait(500_ms); // Wait 500ms for reset to complete
ConfigCalTime(cal_time);
// Configure standard SPI
if (!SwitchToStandardSPI()) {
return;
}
// Set IMU internal decimation to 819.2 SPS
WriteRegister(SMPL_PRD, 0x0001);
// Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
WriteRegister(MSC_CTRL, 0x0016);
// Disable IMU internal Bartlett filter
WriteRegister(SENS_AVG, 0x0400);
// Clear offset registers
WriteRegister(XGYRO_OFF, 0x0000);
WriteRegister(YGYRO_OFF, 0x0000);
WriteRegister(ZGYRO_OFF, 0x0000);
// Configure and enable auto SPI
if (!SwitchToAutoSPI()) {
return;
}
// Notify DS that IMU calibration delay is active
REPORT_WARNING("ADIS16448 IMU Detected. Starting initial calibration delay.");
// Wait for whatever time the user set as the start-up delay
Wait(static_cast<double>(m_calibration_time) * 1.2_s);
// Execute calibration routine
Calibrate();
// Reset accumulated offsets
Reset();
// Tell the acquire loop that we're done starting up
m_start_up_mode = false;
// Let the user know the IMU was initiallized 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)
new DigitalOutput(19);
// Report usage and post data to DS
HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
wpi::SendableRegistry::AddLW(this, "ADIS16448", port);
}
/**
* @brief Switches to standard SPI operation. Primarily used when exiting auto
*SPI mode.
*
* @return A boolean indicating the success or failure of setting up the SPI
*peripheral in standard SPI mode.
*
* This function switches the active SPI port to standard SPI and is used
*primarily when exiting auto SPI. Exiting auto SPI is required to read or write
*using SPI since the auto SPI configuration, once active, locks the SPI message
*being transacted. This function also verifies that the SPI port is operating
*in standard SPI mode by reading back the IMU product ID.
**/
bool ADIS16448_IMU::SwitchToStandardSPI() {
// Check to see whether the acquire thread is active. If so, wait for it to
// stop producing data.
if (m_thread_active) {
m_thread_active = false;
while (!m_thread_idle) {
Wait(10_ms);
}
std::cout << "Paused the IMU processing thread successfully!" << std::endl;
// Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
if (m_spi != nullptr && 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.
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 */
m_spi->ReadAutoReceivedData(trashBuffer, std::min(200, data_count),
0_s);
/* Update remaining buffer count */
data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
}
std::cout << "Paused the auto SPI successfully!" << std::endl;
}
}
// There doesn't seem to be a SPI port active. Let's try to set one up
if (m_spi == nullptr) {
std::cout << "Setting up a new SPI port." << std::endl;
m_spi = new SPI(m_spi_port);
m_spi->SetClockRate(1000000);
m_spi->SetMSBFirst();
m_spi->SetSampleDataOnTrailingEdge();
m_spi->SetClockActiveLow();
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;
}
}
}
void ADIS16448_IMU::InitOffsetBuffer(int size) {
// avoid exceptions in the case of bad arguments
if (size < 1) {
size = 1;
}
// set average size to size (correct bad values)
m_avg_size = size;
// resize vector
if (m_offset_buffer != nullptr) {
delete[] m_offset_buffer;
}
m_offset_buffer = new offset_data[size];
// set accumulate count to 0
m_accum_count = 0;
}
/**
* This function switches the active SPI port to auto SPI and is used primarily
*when exiting standard SPI. Auto SPI is required to asynchronously read data
*over SPI as it utilizes special FPGA hardware to react to an external data
*ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
*can be read by the CPU asynchronously. Standard SPI transactions are
* impossible on the selected SPI port once auto SPI is enabled. The stall
*settings, GPIO interrupt pin, and data packet settings used in this function
*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) {
m_spi->InitAuto(8200);
m_auto_configured = true;
}
// Set auto SPI packet data and size
m_spi->SetAutoTransmitData({{GLOB_CMD}}, 27);
// Configure auto stall time
m_spi->ConfigureAutoStall(HAL_SPI_kMXP, 100, 1000, 255);
// Kick off DMA SPI (Note: Device configration impossible after SPI DMA 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_thread_idle) {
m_first_run = true;
m_thread_active = true;
// Set up circular buffer
InitOffsetBuffer(m_avg_size);
// Kick off acquire thread
m_acquire_task = std::thread(&ADIS16448_IMU::Acquire, this);
std::cout << "New IMU Processing thread activated!" << std::endl;
} else {
m_first_run = true;
m_thread_active = true;
std::cout << "Old IMU Processing thread re-activated!" << std::endl;
}
// Looks like the thread didn't start for some reason. Abort.
/*
if(!m_thread_idle) {
REPORT_ERROR("Failed to start/restart the acquire() thread.");
Close();
return false;
}
*/
return true;
}
/**
*
**/
int ADIS16448_IMU::ConfigCalTime(int new_cal_time) {
if (m_calibration_time == new_cal_time) {
return 1;
} else {
m_calibration_time = static_cast<uint16_t>(new_cal_time);
m_avg_size = m_calibration_time * 819;
InitOffsetBuffer(m_avg_size);
return 0;
}
}
/**
*
**/
void ADIS16448_IMU::Calibrate() {
std::scoped_lock sync(m_mutex);
// Calculate the running average
int gyroAverageSize = std::min(m_accum_count, m_avg_size);
double m_gyro_accum_x = 0.0;
double m_gyro_accum_y = 0.0;
double m_gyro_accum_z = 0.0;
for (int i = 0; i < gyroAverageSize; i++) {
m_gyro_accum_x += m_offset_buffer[i].m_accum_gyro_x;
m_gyro_accum_y += m_offset_buffer[i].m_accum_gyro_y;
m_gyro_accum_z += m_offset_buffer[i].m_accum_gyro_z;
}
m_gyro_offset_x = m_gyro_accum_x / gyroAverageSize;
m_gyro_offset_y = m_gyro_accum_y / gyroAverageSize;
m_gyro_offset_z = m_gyro_accum_z / gyroAverageSize;
m_integ_gyro_x = 0.0;
m_integ_gyro_y = 0.0;
m_integ_gyro_z = 0.0;
// std::cout << "Avg Size: " << gyroAverageSize << " X off: " <<
// m_gyro_offset_x << " Y off: " << m_gyro_offset_y << " Z off: " <<
// m_gyro_offset_z << std::endl;
}
int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
if (m_yaw_axis == yaw_axis) {
return 1;
} else {
m_yaw_axis = yaw_axis;
Reset();
return 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;
buf[1] = 0;
m_spi->Write(buf, 2);
m_spi->Read(false, buf, 2);
return ToUShort(buf);
}
/**
* 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.
**/
void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
uint8_t buf[2];
buf[0] = 0x80 | reg;
buf[1] = val & 0xff;
m_spi->Write(buf, 2);
buf[0] = 0x81 | reg;
buf[1] = val >> 8;
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_x = 0.0;
m_integ_gyro_y = 0.0;
m_integ_gyro_z = 0.0;
}
void ADIS16448_IMU::Close() {
if (m_thread_active) {
m_thread_active = false;
if (m_acquire_task.joinable()) {
m_acquire_task.join();
}
}
if (m_spi != nullptr) {
if (m_auto_configured) {
m_spi->StopAuto();
}
delete m_spi;
m_auto_configured = false;
if (m_auto_interrupt != nullptr) {
delete m_auto_interrupt;
m_auto_interrupt = nullptr;
}
m_spi = nullptr;
}
delete[] m_offset_buffer;
std::cout << "Finished cleaning up after the IMU driver." << std::endl;
}
ADIS16448_IMU::~ADIS16448_IMU() {
Close();
}
void ADIS16448_IMU::Acquire() {
// Set data packet length
const int dataset_len = 29; // 18 data points + timestamp
const int BUFFER_SIZE = 4000;
// struct to store accumulate data
offset_data sample_data;
// This buffer can contain many datasets
uint32_t buffer[BUFFER_SIZE];
int data_count = 0;
int data_remainder = 0;
int data_to_read = 0;
int bufferAvgIndex = 0;
uint32_t previous_timestamp = 0;
double gyro_x = 0.0;
double gyro_y = 0.0;
double gyro_z = 0.0;
double 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_x_si = 0.0;
double gyro_y_si = 0.0;
// double gyro_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(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 */
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
// 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];
}
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
// 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_x = BuffToShort(&buffer[i + 5]) * 0.04;
gyro_y = BuffToShort(&buffer[i + 7]) * 0.04;
gyro_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;
/*std::cout << BuffToShort(&buffer[i + 3]) << "," <<
BuffToShort(&buffer[i + 5]) << "," << BuffToShort(&buffer[i + 7]) <<
"," << BuffToShort(&buffer[i + 9]) << "," << BuffToShort(&buffer[i +
11]) << "," << BuffToShort(&buffer[i + 13]) << "," <<
BuffToShort(&buffer[i + 15]) << "," << BuffToShort(&buffer[i + 17]) <<
"," << BuffToShort(&buffer[i + 19]) << "," << BuffToShort(&buffer[i +
21]) << "," << BuffToShort(&buffer[i + 23]) << "," <<
BuffToShort(&buffer[i + 25]) << "," <<
BuffToShort(&buffer[i + 27]) << std::endl; */
// Convert scaled sensor data to SI units
gyro_x_si = gyro_x * deg_to_rad;
gyro_y_si = gyro_y * deg_to_rad;
// gyro_z_si = gyro_z * deg_to_rad;
accel_x_si = accel_x * grav;
accel_y_si = accel_y * grav;
accel_z_si = accel_z * grav;
// 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);
// 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 = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
compAngleY = CompFilterProcess(compAngleY, accelAngleY, -gyro_x_si);
}
// Update global variables and state
{
std::scoped_lock sync(m_mutex);
// Ignore first, integrated sample
if (m_first_run) {
m_integ_gyro_x = 0.0;
m_integ_gyro_y = 0.0;
m_integ_gyro_z = 0.0;
} else {
// Accumulate gyro for offset calibration
// Build most recent sample data
sample_data.m_accum_gyro_x = gyro_x;
sample_data.m_accum_gyro_y = gyro_y;
sample_data.m_accum_gyro_z = gyro_z;
// Add to buffer
bufferAvgIndex = m_accum_count % m_avg_size;
m_offset_buffer[bufferAvgIndex] = sample_data;
// Increment counter
m_accum_count++;
}
// Don't post accumulated data to the global variables until an
// initial gyro offset has been calculated
if (!m_start_up_mode) {
m_gyro_x = gyro_x;
m_gyro_y = gyro_y;
m_gyro_z = gyro_z;
m_accel_x = accel_x;
m_accel_y = accel_y;
m_accel_z = accel_z;
m_mag_x = mag_x;
m_mag_y = mag_y;
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;
// Accumulate gyro for angle integration and publish to global
// variables
m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * m_dt;
m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * m_dt;
m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * m_dt;
}
}
m_first_run = false;
} else {
/*
// Print notification when crc fails and bad data is rejected
std::cout << "IMU Data CRC Mismatch Detected." << std::endl;
std::cout << "Calculated CRC: " << calc_crc << std::endl;
std::cout << "Read CRC: " << imu_crc << std::endl;
// DEBUG: Plot sub-array data in terminal
std::cout << BuffToUShort(&buffer[i + 3]) << "," <<
BuffToUShort(&buffer[i + 5]) << "," << BuffToUShort(&buffer[i + 7]) <<
"," << BuffToUShort(&buffer[i + 9]) << "," << BuffToUShort(&buffer[i +
11]) << "," << BuffToUShort(&buffer[i + 13]) << "," <<
BuffToUShort(&buffer[i + 15]) << "," << BuffToUShort(&buffer[i + 17])
<< "," << BuffToUShort(&buffer[i + 19]) << "," <<
BuffToUShort(&buffer[i + 21]) << "," << BuffToUShort(&buffer[i + 23])
<< "," << BuffToUShort(&buffer[i + 25]) << "," <<
BuffToUShort(&buffer[i + 27]) << std::endl; */
}
}
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
gyro_x = 0.0;
gyro_y = 0.0;
gyro_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_x_si = 0.0;
gyro_y_si = 0.0;
// gyro_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;
}
}
}
/* Complementary filter functions */
double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
if (compAngle > accAngle + wpi::numbers::pi) {
compAngle = compAngle - 2.0 * wpi::numbers::pi;
} else if (accAngle > compAngle + wpi::numbers::pi) {
compAngle = compAngle + 2.0 * wpi::numbers::pi;
}
return compAngle;
}
double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
while (compAngle >= 2 * wpi::numbers::pi) {
compAngle = compAngle - 2.0 * wpi::numbers::pi;
}
while (compAngle < 0.0) {
compAngle = compAngle + 2.0 * wpi::numbers::pi;
}
return compAngle;
}
double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = wpi::numbers::pi - accelAngle;
} else if (accelZ > 0.0 && accelAngle < 0.0) {
accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
}
return accelAngle;
}
double ADIS16448_IMU::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 > wpi::numbers::pi) {
compAngle = compAngle - 2.0 * wpi::numbers::pi;
}
return compAngle;
}
int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
uint16_t writeValue = DecimationSetting;
uint16_t readbackValue;
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
/* Check max */
if (DecimationSetting > 9) {
REPORT_ERROR("Attemted to write an invalid decimation value. Capping at 9");
DecimationSetting = 9;
}
/* Shift decimation setting to correct position and select internal sync */
writeValue = (DecimationSetting << 8) | 0x1;
/* Apply to IMU */
WriteRegister(SMPL_PRD, writeValue);
/* Perform read back to verify write */
readbackValue = ReadRegister(SMPL_PRD);
/* Throw error for invalid write */
if (readbackValue != writeValue) {
REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
}
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
return 2;
}
return 0;
}
/**
* This function returns the most recent integrated angle for the axis chosen by
*m_yaw_axis. This function is most useful in situations where the yaw axis may
*not coincide with the IMU Z axis.
**/
double ADIS16448_IMU::GetAngle() const {
switch (m_yaw_axis) {
case kX:
return GetGyroAngleX();
case kY:
return GetGyroAngleY();
case kZ:
return GetGyroAngleZ();
default:
return 0.0;
}
}
double ADIS16448_IMU::GetRate() const {
switch (m_yaw_axis) {
case kX:
return GetGyroInstantX();
case kY:
return GetGyroInstantY();
case kZ:
return GetGyroInstantZ();
default:
return 0.0;
}
}
ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
return m_yaw_axis;
}
double ADIS16448_IMU::GetGyroAngleX() const {
std::scoped_lock sync(m_mutex);
return m_integ_gyro_x;
}
double ADIS16448_IMU::GetGyroAngleY() const {
std::scoped_lock sync(m_mutex);
return m_integ_gyro_y;
}
double ADIS16448_IMU::GetGyroAngleZ() const {
std::scoped_lock sync(m_mutex);
return m_integ_gyro_z;
}
double ADIS16448_IMU::GetGyroInstantX() const {
std::scoped_lock sync(m_mutex);
return m_gyro_x;
}
double ADIS16448_IMU::GetGyroInstantY() const {
std::scoped_lock sync(m_mutex);
return m_gyro_y;
}
double ADIS16448_IMU::GetGyroInstantZ() const {
std::scoped_lock sync(m_mutex);
return m_gyro_z;
}
double ADIS16448_IMU::GetAccelInstantX() const {
std::scoped_lock sync(m_mutex);
return m_accel_x;
}
double ADIS16448_IMU::GetAccelInstantY() const {
std::scoped_lock sync(m_mutex);
return m_accel_y;
}
double ADIS16448_IMU::GetAccelInstantZ() const {
std::scoped_lock sync(m_mutex);
return m_accel_z;
}
double ADIS16448_IMU::GetMagInstantX() const {
std::scoped_lock sync(m_mutex);
return m_mag_x;
}
double ADIS16448_IMU::GetMagInstantY() const {
std::scoped_lock sync(m_mutex);
return m_mag_y;
}
double ADIS16448_IMU::GetMagInstantZ() const {
std::scoped_lock sync(m_mutex);
return m_mag_z;
}
double ADIS16448_IMU::GetXComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return m_compAngleX;
}
double ADIS16448_IMU::GetYComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return m_compAngleY;
}
double ADIS16448_IMU::GetXFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return m_accelAngleX;
}
double ADIS16448_IMU::GetYFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return m_accelAngleY;
}
double ADIS16448_IMU::GetBarometricPressure() const {
std::scoped_lock sync(m_mutex);
return m_baro;
}
double ADIS16448_IMU::GetTemperature() const {
std::scoped_lock sync(m_mutex);
return m_temp;
}
/**
* @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(nt::NTSendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16448 IMU");
auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
builder.SetUpdateTable(
[=]() { nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle()); });
}

795
wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp Normal file
View File

@@ -0,0 +1,795 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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) 2016-2020 Analog Devices Inc. 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. */
/* */
/* Juan Chong - frcsupport@analog.com */
/*----------------------------------------------------------------------------*/
#include <frc/ADIS16470_IMU.h>
#include <frc/DigitalInput.h>
#include <frc/DigitalSource.h>
#include <frc/DriverStation.h>
#include <frc/Timer.h>
#include <cmath>
#include <iostream>
#include <string>
#include <hal/HAL.h>
#include <networktables/NTSendableBuilder.h>
#include <wpi/numbers>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
/* Helpful conversion functions */
static inline int32_t ToInt(const uint32_t* buf) {
return static_cast<int32_t>((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) |
buf[3]);
}
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 {
template <typename S, typename... Args>
inline void ADISReportError(int32_t status, const char* file, int line,
const char* function, const S& format,
Args&&... args) {
frc::ReportErrorV(status, file, line, function, format,
fmt::make_args_checked<Args...>(format, args...));
}
} // namespace
#define REPORT_WARNING(msg) \
ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
#define REPORT_ERROR(msg) \
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
/**
* Constructor.
*/
ADIS16470_IMU::ADIS16470_IMU()
: ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, ADIS16470CalibrationTime::_4s) {
}
ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
ADIS16470CalibrationTime cal_time)
: m_yaw_axis(yaw_axis),
m_spi_port(port),
m_calibration_time(static_cast<uint16_t>(cal_time)) {
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
// 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(27); // Drive SPI CS2 (IMU RST) low
Wait(10_ms); // Wait 10ms
delete m_reset_out;
new DigitalInput(27); // Set SPI CS2 (IMU RST) high
Wait(500_ms); // Wait 500ms for reset to complete
// Configure standard SPI
if (!SwitchToStandardSPI()) {
return;
}
// Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
// 400Hz)
WriteRegister(DEC_RATE, 0x0004);
// Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
// PoP
WriteRegister(MSC_CTRL, 0x0001);
// Configure IMU internal Bartlett filter
WriteRegister(FILT_CTRL, 0x0000);
// Configure continuous bias calibration time based on user setting
WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
// Notify DS that IMU calibration delay is active
REPORT_WARNING("ADIS16470 IMU Detected. Starting initial calibration delay.");
// Wait for samples to accumulate internal to the IMU (110% of user-defined
// time)
Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});
// Write offset calibration command to IMU
WriteRegister(GLOB_CMD, 0x0001);
// Configure and enable auto SPI
if (!SwitchToAutoSPI()) {
return;
}
// Let the user know the IMU was initiallized successfully
REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
// Drive SPI CS3 (IMU ready LED) low (active low)
new DigitalOutput(28);
// Report usage and post data to DS
HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
}
/**
* @brief Switches to standard SPI operation. Primarily used when exiting auto
*SPI mode.
*
* @return A boolean indicating the success or failure of setting up the SPI
*peripheral in standard SPI mode.
*
* This function switches the active SPI port to standard SPI and is used
*primarily when exiting auto SPI. Exiting auto SPI is required to read or write
*using SPI since the auto SPI configuration, once active, locks the SPI message
*being transacted. This function also verifies that the SPI port is operating
*in standard SPI mode by reading back the IMU product ID.
**/
bool ADIS16470_IMU::SwitchToStandardSPI() {
// Check to see whether the acquire thread is active. If so, wait for it to
// stop producing data.
if (m_thread_active) {
m_thread_active = false;
while (!m_thread_idle) {
Wait(10_ms);
}
std::cout << "Paused the IMU processing thread successfully!" << std::endl;
// Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
if (m_spi != nullptr && 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.
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)
*/
m_spi->ReadAutoReceivedData(trashBuffer, std::min(data_count, 200),
0_s);
/*Get the reamining data count */
data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
}
std::cout << "Paused the auto SPI successfully!" << std::endl;
}
}
// There doesn't seem to be a SPI port active. Let's try to set one up
if (m_spi == nullptr) {
std::cout << "Setting up a new SPI port." << std::endl;
m_spi = new SPI(m_spi_port);
m_spi->SetClockRate(2000000);
m_spi->SetMSBFirst();
m_spi->SetSampleDataOnTrailingEdge();
m_spi->SetClockActiveLow();
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;
}
}
}
/**
* @brief Switches to auto SPI operation. Primarily used when exiting standard
*SPI mode.
*
* @return A boolean indicating the success or failure of setting up the SPI
*peripheral in auto SPI mode.
*
* This function switches the active SPI port to auto SPI and is used primarily
*when exiting standard SPI. Auto SPI is required to asynchronously read data
*over SPI as it utilizes special FPGA hardware to react to an external data
*ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
*can be read by the CPU asynchronously. Standard SPI transactions are
* impossible on the selected SPI port once auto SPI is enabled. The stall
*settings, GPIO interrupt pin, and data packet settings used in this function
*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) {
m_spi->InitAuto(8200);
m_auto_configured = true;
}
// Do we need to change auto SPI settings?
switch (m_yaw_axis) {
case kX:
m_spi->SetAutoTransmitData(m_autospi_x_packet, 2);
break;
case kY:
m_spi->SetAutoTransmitData(m_autospi_y_packet, 2);
break;
default:
m_spi->SetAutoTransmitData(m_autospi_z_packet, 2);
break;
}
// Configure auto stall time
m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
// Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
// 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) {
m_first_run = true;
m_thread_active = true;
m_acquire_task = std::thread(&ADIS16470_IMU::Acquire, this);
std::cout << "New IMU Processing thread activated!" << std::endl;
} else {
m_first_run = true;
m_thread_active = true;
std::cout << "Old IMU Processing thread re-activated!" << std::endl;
}
// Looks like the thread didn't start for some reason. Abort.
/*
if(!m_thread_idle) {
REPORT_ERROR("Failed to start/restart the acquire() thread.");
Close();
return false;
}
*/
return true;
}
/**
* @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.
*
* @param new_cal_time Calibration time to be set.
*
* @return An int indicating the success or failure of writing the new NULL_CNFG
*setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
*Failure
*
* This function enters standard SPI mode, writes a new NULL_CNFG setting to the
*IMU, and re-enters auto SPI mode. This function does not include a blocking
*sleep, so the user must keep track of the elapsed offset calibration time
* themselves. After waiting the configured calibration time, the Calibrate()
*function should be called to activate the new offset calibration.
**/
int ADIS16470_IMU::ConfigCalTime(ADIS16470CalibrationTime new_cal_time) {
if (m_calibration_time == static_cast<uint16_t>(new_cal_time)) {
return 1;
}
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
m_calibration_time = static_cast<uint16_t>(new_cal_time);
WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
return 2;
}
return 0;
}
/**
* @brief 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.
*
* @param reg Decimation value to be set.
*
* @return An int indicating the success or failure of writing the new DECIMATE
*setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
*Failure
*
* 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 ADIS16470_IMU::ConfigDecRate(uint16_t reg) {
uint16_t m_reg = reg;
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
if (m_reg > 1999) {
REPORT_ERROR("Attempted to write an invalid decimation value.");
m_reg = 1999;
}
m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
WriteRegister(DEC_RATE, m_reg);
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
return 2;
}
return 0;
}
/**
* @brief Switches the active SPI port to standard SPI mode, writes the command
*to activate the new null configuration, and re-enables auto SPI.
*
* This function enters standard SPI mode, writes 0x0001 to the GLOB_CMD
*register (thus making the new offset active in the IMU), and re-enters auto
*SPI mode. This function does not include a blocking sleep, so the user must
*keep track of the elapsed offset calibration time themselves.
**/
void ADIS16470_IMU::Calibrate() {
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
}
WriteRegister(GLOB_CMD, 0x0001);
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
}
}
int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
if (m_yaw_axis == yaw_axis) {
return 1;
}
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
m_yaw_axis = yaw_axis;
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
return 2;
}
return 0;
}
/**
* @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;
buf[1] = 0;
m_spi->Write(buf, 2);
m_spi->Read(false, buf, 2);
return ToUShort(buf);
}
/**
* @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
*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.
*
* 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.
**/
void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
uint8_t buf[2];
buf[0] = 0x80 | reg;
buf[1] = val & 0xff;
m_spi->Write(buf, 2);
buf[0] = 0x81 | reg;
buf[1] = val >> 8;
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 = 0.0;
}
void ADIS16470_IMU::Close() {
if (m_thread_active) {
m_thread_active = false;
if (m_acquire_task.joinable()) {
m_acquire_task.join();
}
}
if (m_spi != nullptr) {
if (m_auto_configured) {
m_spi->StopAuto();
}
delete m_spi;
m_auto_configured = false;
if (m_auto_interrupt != nullptr) {
delete m_auto_interrupt;
m_auto_interrupt = nullptr;
}
m_spi = nullptr;
}
std::cout << "Finished cleaning up after the IMU driver." << std::endl;
}
ADIS16470_IMU::~ADIS16470_IMU() {
Close();
}
/**
* @brief Main acquisition loop. Typically called asynchronously and free-wheels
*while the robot code is active.
*
* This is the main acquisiton loop for the IMU. During each iteration, data
*read using auto SPI is extracted from the FPGA FIFO, split, scaled, and
*integrated. Each X, Y, and Z value is 32-bits split across 4 indices (bytes)
*in the buffer. Auto SPI puts one byte in each index location. Each index is
*32-bits wide because the timestamp is an unsigned 32-bit int. The timestamp is
*always located at the beginning of the frame. Two indices (request_1 and
*request_2 below) are always invalid (garbage) and can be disregarded.
*
* Data order: [timestamp, request_1, request_2, x_1, x_2, x_3, x_4, y_1, y_2,
*y_3, y_4, z_1, z_2, z_3, z_4] x = delta x (32-bit x_1 = highest bit) y = delta
*y (32-bit y_1 = highest bit) z = delta z (32-bit z_1 = highest bit)
*
* Complementary filter code was borrowed from
*https://github.com/tcleg/Six_Axis_Complementary_Filter
**/
void ADIS16470_IMU::Acquire() {
// Set data packet length
const int dataset_len = 19; // 18 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 = 0.0;
double gyro_x = 0.0;
double gyro_y = 0.0;
double gyro_z = 0.0;
double accel_x = 0.0;
double accel_y = 0.0;
double accel_z = 0.0;
double gyro_x_si = 0.0;
double gyro_y_si = 0.0;
// double gyro_z_si = 0.0;
double 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(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 */
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)
/*
// DEBUG: Print buffer size and contents to terminal
std::cout << "Start - " << data_count << "," << data_remainder << "," <<
data_to_read << std::endl; for (int m = 0; m < data_to_read - 1; m++ )
{
std::cout << buffer[m] << ",";
}
std::cout << " " << std::endl;
std::cout << "End" << std::endl;
std::cout << "Reading " << data_count << " bytes." << std::endl;
*/
// 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 = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
(m_scaled_sample_rate / (buffer[i] - previous_timestamp));
gyro_x = (BuffToShort(&buffer[i + 7]) / 10.0);
gyro_y = (BuffToShort(&buffer[i + 9]) / 10.0);
gyro_z = (BuffToShort(&buffer[i + 11]) / 10.0);
accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
// Convert scaled sensor data to SI units
gyro_x_si = gyro_x * deg_to_rad;
gyro_y_si = gyro_y * deg_to_rad;
// gyro_z_si = gyro_z * deg_to_rad;
accel_x_si = accel_x * grav;
accel_y_si = accel_y * grav;
accel_z_si = accel_z * grav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
/*
// DEBUG: Print timestamp and delta values
std::cout << previous_timestamp << "," << delta_x << "," << delta_y <<
"," << delta_z << std::endl;
*/
m_alpha = m_tau / (m_tau + m_dt);
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 = CompFilterProcess(compAngleX, accelAngleX, -gyro_y_si);
compAngleY = CompFilterProcess(compAngleY, accelAngleY, gyro_x_si);
}
// DEBUG: Print accumulated values
// std::cout << m_compAngleX << "," << m_compAngleY << std::endl;
{
std::scoped_lock sync(m_mutex);
/* 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 = 0.0;
} else {
m_integ_angle += delta_angle;
}
m_gyro_x = gyro_x;
m_gyro_y = gyro_y;
m_gyro_z = gyro_z;
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_first_run = false;
}
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
delta_angle = 0.0;
gyro_x = 0.0;
gyro_y = 0.0;
gyro_z = 0.0;
accel_x = 0.0;
accel_y = 0.0;
accel_z = 0.0;
gyro_x_si = 0.0;
gyro_y_si = 0.0;
// gyro_z_si = 0.0;
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;
}
}
}
/* Complementary filter functions */
double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
if (compAngle > accAngle + wpi::numbers::pi) {
compAngle = compAngle - 2.0 * wpi::numbers::pi;
} else if (accAngle > compAngle + wpi::numbers::pi) {
compAngle = compAngle + 2.0 * wpi::numbers::pi;
}
return compAngle;
}
double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
while (compAngle >= 2 * wpi::numbers::pi) {
compAngle = compAngle - 2.0 * wpi::numbers::pi;
}
while (compAngle < 0.0) {
compAngle = compAngle + 2.0 * wpi::numbers::pi;
}
return compAngle;
}
double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = wpi::numbers::pi - accelAngle;
} else if (accelZ > 0.0 && accelAngle < 0.0) {
accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
}
return accelAngle;
}
double ADIS16470_IMU::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 > wpi::numbers::pi) {
compAngle = compAngle - 2.0 * wpi::numbers::pi;
}
return compAngle;
}
/**
* @brief Returns the current integrated angle for the axis specified.
*
* @param m_yaw_axis An enum indicating the axis chosen to act as the yaw axis.
*
* @return The current integrated angle in degrees.
*
* This function returns the most recent integrated angle for the axis chosen by
*m_yaw_axis. This function is most useful in situations where the yaw axis may
*not coincide with the IMU Z axis.
**/
double ADIS16470_IMU::GetAngle() const {
std::scoped_lock sync(m_mutex);
return m_integ_angle;
}
double ADIS16470_IMU::GetRate() const {
std::scoped_lock sync(m_mutex);
switch (m_yaw_axis) {
case kX:
return m_gyro_x;
case kY:
return m_gyro_y;
case kZ:
return m_gyro_z;
default:
return 0.0;
}
}
ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
return m_yaw_axis;
}
double ADIS16470_IMU::GetGyroInstantX() const {
std::scoped_lock sync(m_mutex);
return m_gyro_x;
}
double ADIS16470_IMU::GetGyroInstantY() const {
std::scoped_lock sync(m_mutex);
return m_gyro_y;
}
double ADIS16470_IMU::GetGyroInstantZ() const {
std::scoped_lock sync(m_mutex);
return m_gyro_z;
}
double ADIS16470_IMU::GetAccelInstantX() const {
std::scoped_lock sync(m_mutex);
return m_accel_x;
}
double ADIS16470_IMU::GetAccelInstantY() const {
std::scoped_lock sync(m_mutex);
return m_accel_y;
}
double ADIS16470_IMU::GetAccelInstantZ() const {
std::scoped_lock sync(m_mutex);
return m_accel_z;
}
double ADIS16470_IMU::GetXComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return m_compAngleX;
}
double ADIS16470_IMU::GetYComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return m_compAngleY;
}
double ADIS16470_IMU::GetXFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return m_accelAngleX;
}
double ADIS16470_IMU::GetYFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return m_accelAngleY;
}
/**
* @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(nt::NTSendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16470 IMU");
auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
builder.SetUpdateTable(
[=]() { nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle()); });
}

378
wpilibc/src/main/native/include/frc/ADIS16448_IMU.h Normal file
View File

@@ -0,0 +1,378 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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) 2016-2020 Analog Devices Inc. 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. */
/* */
/* Modified by Juan Chong - frcsupport@analog.com */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/DigitalInput.h>
#include <frc/DigitalOutput.h>
#include <frc/DigitalSource.h>
#include <frc/SPI.h>
#include <stdint.h>
#include <atomic>
#include <memory>
#include <thread>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableHelper.h>
// Not always defined in cmath (not part of standard)
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
namespace frc {
/** @brief 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
static constexpr uint8_t ZGYRO_OUT = 0x08; // Z-axis gyroscope output
static constexpr uint8_t XACCL_OUT = 0x0A; // X-axis accelerometer output
static constexpr uint8_t YACCL_OUT = 0x0C; // Y-axis accelerometer output
static constexpr uint8_t ZACCL_OUT = 0x0E; // Z-axis accelerometer output
static constexpr uint8_t XMAGN_OUT = 0x10; // X-axis magnetometer output
static constexpr uint8_t YMAGN_OUT = 0x12; // Y-axis magnetometer output
static constexpr uint8_t ZMAGN_OUT = 0x14; // Z-axis magnetometer output
static constexpr uint8_t BARO_OUT =
0x16; // Barometer pressure measurement, high word
static constexpr uint8_t TEMP_OUT = 0x18; // Temperature output
static constexpr uint8_t XGYRO_OFF =
0x1A; // X-axis gyroscope bias offset factor
static constexpr uint8_t YGYRO_OFF =
0x1C; // Y-axis gyroscope bias offset factor
static constexpr uint8_t ZGYRO_OFF =
0x1E; // Z-axis gyroscope bias offset factor
static constexpr uint8_t XACCL_OFF =
0x20; // X-axis acceleration bias offset factor
static constexpr uint8_t YACCL_OFF =
0x22; // Y-axis acceleration bias offset factor
static constexpr uint8_t ZACCL_OFF =
0x24; // Z-axis acceleration bias offset factor
static constexpr uint8_t XMAGN_HIC =
0x26; // X-axis magnetometer, hard iron factor
static constexpr uint8_t YMAGN_HIC =
0x28; // Y-axis magnetometer, hard iron factor
static constexpr uint8_t ZMAGN_HIC =
0x2A; // Z-axis magnetometer, hard iron factor
static constexpr uint8_t XMAGN_SIC =
0x2C; // X-axis magnetometer, soft iron factor
static constexpr uint8_t YMAGN_SIC =
0x2E; // Y-axis magnetometer, soft iron factor
static constexpr uint8_t ZMAGN_SIC =
0x30; // Z-axis magnetometer, soft iron factor
static constexpr uint8_t GPIO_CTRL = 0x32; // GPIO control
static constexpr uint8_t MSC_CTRL = 0x34; // MISC control
static constexpr uint8_t SMPL_PRD =
0x36; // Sample clock/Decimation filter control
static constexpr uint8_t SENS_AVG = 0x38; // Digital filter control
static constexpr uint8_t SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter
static constexpr uint8_t DIAG_STAT = 0x3C; // System status
static constexpr uint8_t GLOB_CMD = 0x3E; // System command
static constexpr uint8_t ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold
static constexpr uint8_t ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold
static constexpr uint8_t ALM_SMPL1 = 0x44; // Alarm 1 sample size
static constexpr uint8_t ALM_SMPL2 = 0x46; // Alarm 2 sample size
static constexpr uint8_t ALM_CTRL = 0x48; // Alarm control
static constexpr uint8_t LOT_ID1 = 0x52; // Lot identification number
static constexpr uint8_t LOT_ID2 = 0x54; // Lot identification number
static constexpr uint8_t PROD_ID = 0x56; // Product identifier
static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number
/** @brief ADIS16448 Static Constants */
const double rad_to_deg = 57.2957795;
const double deg_to_rad = 0.0174532;
const double grav = 9.81;
/** @brief struct to store offset data */
struct offset_data {
double m_accum_gyro_x = 0.0;
double m_accum_gyro_y = 0.0;
double m_accum_gyro_z = 0.0;
};
/**
* Use DMA SPI to read rate, acceleration, and magnetometer data from the
* ADIS16448 IMU and return the robots heading relative to a starting position,
* AHRS, and instant measurements
*
* The ADIS16448 gyro angle outputs track the robot's heading based on the
* starting position. As the robot rotates the new heading is computed by
* integrating the rate of rotation returned by the IMU. When the class is
* instantiated, a short calibration routine is performed where the IMU samples
* the gyros while at rest to determine the initial offset. This is subtracted
* from each sample to determine the heading.
*
* This class is for the ADIS16448 IMU connected via the SPI port available on
* the RoboRIO MXP port.
*/
class ADIS16448_IMU : public nt::NTSendable,
public wpi::SendableHelper<ADIS16448_IMU> {
public:
enum IMUAxis { kX, kY, kZ };
/**
* IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
* AHRS computation.
*/
ADIS16448_IMU();
/**
* IMU constructor on the specified MXP port and orientation.
*
* @param yaw_axis The axis where gravity is present. Valid options are kX,
* kY, and kZ
* @param port The SPI port where the IMU is connected.
* @param cal_time The calibration time that should be used on start-up.
*/
explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port, uint16_t cal_time);
~ADIS16448_IMU() override;
ADIS16448_IMU(ADIS16448_IMU&&) = default;
ADIS16448_IMU& operator=(ADIS16448_IMU&&) = default;
/**
* Initialize the IMU.
*
* Perform gyro offset calibration by collecting data for a number of seconds
* and computing the center value. The center value is subtracted from
* subsequent measurements.
*
* It's important to make sure that the robot is not moving while the
* centering calculations are in progress, this is typically done when the
* robot is first turned on while it's sitting at rest before the match
* starts.
*
* The calibration routine can be triggered by the user during runtime.
*/
void Calibrate();
/**
* Configures the calibration time used for the next calibrate.
*
* @param new_cal_time The calibration time that should be used
*/
int ConfigCalTime(int new_cal_time);
/**
* Reset the gyro.
*
* Resets the gyro accumulations to a heading of zero. This can be used if
* there is significant drift in the gyro and it needs to be recalibrated
* after running.
*/
void Reset();
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by
* offset calibration and built-in IMU calibration. The angle is continuous,
* that is it will continue from 360->361 degrees. This allows algorithms
* that wouldn't want to see a discontinuity in the gyro output as it sweeps
* from 360 to 0 on the second time around. The axis returned by this
* function is adjusted fased on the configured yaw_axis.
*
* @return the current heading of the robot in degrees. This heading is based
* on integration of the returned rate from the gyro.
*/
double GetAngle() const;
/**
* Return the rate of rotation of the yaw_axis gyro.
*
* The rate is based on the most recent reading of the gyro value
*
* @return the current rate in degrees per second
*/
double GetRate() const;
double GetGyroAngleX() const;
double GetGyroAngleY() const;
double GetGyroAngleZ() const;
double GetGyroInstantX() const;
double GetGyroInstantY() const;
double GetGyroInstantZ() const;
double GetAccelInstantX() const;
double GetAccelInstantY() const;
double GetAccelInstantZ() const;
double GetXComplementaryAngle() const;
double GetYComplementaryAngle() const;
double GetXFilteredAccelAngle() const;
double GetYFilteredAccelAngle() const;
double GetMagInstantX() const;
double GetMagInstantY() const;
double GetMagInstantZ() const;
double GetBarometricPressure() const;
double GetTemperature() const;
IMUAxis GetYawAxis() const;
int SetYawAxis(IMUAxis yaw_axis);
int ConfigDecRate(uint16_t DecimationRate);
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
bool SwitchToStandardSPI();
bool SwitchToAutoSPI();
uint16_t ReadRegister(uint8_t reg);
void WriteRegister(uint8_t reg, uint16_t val);
void Acquire();
void Close();
// User-specified yaw axis
IMUAxis m_yaw_axis;
// Last read values (post-scaling)
double m_gyro_x = 0.0;
double m_gyro_y = 0.0;
double m_gyro_z = 0.0;
double m_accel_x = 0.0;
double m_accel_y = 0.0;
double m_accel_z = 0.0;
double m_mag_x = 0.0;
double m_mag_y = 0.0;
double m_mag_z = 0.0;
double m_baro = 0.0;
double m_temp = 0.0;
// Complementary filter variables
double m_tau = 0.5;
double m_dt, m_alpha = 0.0;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// vector for storing most recent imu values
offset_data* m_offset_buffer = nullptr;
double m_gyro_offset_x = 0.0;
double m_gyro_offset_y = 0.0;
double m_gyro_offset_z = 0.0;
// function to re-init offset buffer
void InitOffsetBuffer(int size);
// Accumulated gyro values (for offset calculation)
int m_avg_size = 0;
int m_accum_count = 0;
// Integrated gyro values
double m_integ_gyro_x = 0.0;
double m_integ_gyro_y = 0.0;
double m_integ_gyro_z = 0.0;
// 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);
// State and resource variables
volatile bool m_thread_active = false;
volatile bool m_first_run = true;
volatile bool m_thread_idle = false;
volatile bool m_start_up_mode = true;
bool m_auto_configured = false;
SPI::Port m_spi_port;
uint16_t m_calibration_time;
SPI* m_spi = nullptr;
DigitalInput* m_auto_interrupt = nullptr;
std::thread m_acquire_task;
struct NonMovableMutexWrapper {
wpi::mutex mutex;
NonMovableMutexWrapper() = default;
NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
return *this;
}
void lock() { mutex.lock(); }
void unlock() { mutex.unlock(); }
bool try_lock() noexcept { return mutex.try_lock(); }
};
mutable NonMovableMutexWrapper m_mutex;
// CRC-16 Look-Up Table
static constexpr uint16_t 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,
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};
};
} // namespace frc

390
wpilibc/src/main/native/include/frc/ADIS16470_IMU.h Normal file
View File

@@ -0,0 +1,390 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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) 2016-2020 Analog Devices Inc. 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. */
/* */
/* Juan Chong - frcsupport@analog.com */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/DigitalInput.h>
#include <frc/DigitalOutput.h>
#include <frc/DigitalSource.h>
#include <frc/SPI.h>
#include <stdint.h>
#include <atomic>
#include <memory>
#include <thread>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableHelper.h>
namespace frc {
/* ADIS16470 Calibration Time Enum Class */
enum class ADIS16470CalibrationTime {
_32ms = 0,
_64ms = 1,
_128ms = 2,
_256ms = 3,
_512ms = 4,
_1s = 5,
_2s = 6,
_4s = 7,
_8s = 8,
_16s = 9,
_32s = 10,
_64s = 11
};
/* ADIS16470 Register Map Declaration */
static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
static constexpr uint8_t DIAG_STAT = 0x02; // Diagnostic and operational status
static constexpr uint8_t X_GYRO_LOW =
0x04; // X-axis gyroscope output, lower word
static constexpr uint8_t X_GYRO_OUT =
0x06; // X-axis gyroscope output, upper word
static constexpr uint8_t Y_GYRO_LOW =
0x08; // Y-axis gyroscope output, lower word
static constexpr uint8_t Y_GYRO_OUT =
0x0A; // Y-axis gyroscope output, upper word
static constexpr uint8_t Z_GYRO_LOW =
0x0C; // Z-axis gyroscope output, lower word
static constexpr uint8_t Z_GYRO_OUT =
0x0E; // Z-axis gyroscope output, upper word
static constexpr uint8_t X_ACCL_LOW =
0x10; // X-axis accelerometer output, lower word
static constexpr uint8_t X_ACCL_OUT =
0x12; // X-axis accelerometer output, upper word
static constexpr uint8_t Y_ACCL_LOW =
0x14; // Y-axis accelerometer output, lower word
static constexpr uint8_t Y_ACCL_OUT =
0x16; // Y-axis accelerometer output, upper word
static constexpr uint8_t Z_ACCL_LOW =
0x18; // Z-axis accelerometer output, lower word
static constexpr uint8_t Z_ACCL_OUT =
0x1A; // Z-axis accelerometer output, upper word
static constexpr uint8_t TEMP_OUT =
0x1C; // Temperature output (internal, not calibrated)
static constexpr uint8_t TIME_STAMP = 0x1E; // PPS mode time stamp
static constexpr uint8_t X_DELTANG_LOW =
0x24; // X-axis delta angle output, lower word
static constexpr uint8_t X_DELTANG_OUT =
0x26; // X-axis delta angle output, upper word
static constexpr uint8_t Y_DELTANG_LOW =
0x28; // Y-axis delta angle output, lower word
static constexpr uint8_t Y_DELTANG_OUT =
0x2A; // Y-axis delta angle output, upper word
static constexpr uint8_t Z_DELTANG_LOW =
0x2C; // Z-axis delta angle output, lower word
static constexpr uint8_t Z_DELTANG_OUT =
0x2E; // Z-axis delta angle output, upper word
static constexpr uint8_t X_DELTVEL_LOW =
0x30; // X-axis delta velocity output, lower word
static constexpr uint8_t X_DELTVEL_OUT =
0x32; // X-axis delta velocity output, upper word
static constexpr uint8_t Y_DELTVEL_LOW =
0x34; // Y-axis delta velocity output, lower word
static constexpr uint8_t Y_DELTVEL_OUT =
0x36; // Y-axis delta velocity output, upper word
static constexpr uint8_t Z_DELTVEL_LOW =
0x38; // Z-axis delta velocity output, lower word
static constexpr uint8_t Z_DELTVEL_OUT =
0x3A; // Z-axis delta velocity output, upper word
static constexpr uint8_t XG_BIAS_LOW =
0x40; // X-axis gyroscope bias offset correction, lower word
static constexpr uint8_t XG_BIAS_HIGH =
0x42; // X-axis gyroscope bias offset correction, upper word
static constexpr uint8_t YG_BIAS_LOW =
0x44; // Y-axis gyroscope bias offset correction, lower word
static constexpr uint8_t YG_BIAS_HIGH =
0x46; // Y-axis gyroscope bias offset correction, upper word
static constexpr uint8_t ZG_BIAS_LOW =
0x48; // Z-axis gyroscope bias offset correction, lower word
static constexpr uint8_t ZG_BIAS_HIGH =
0x4A; // Z-axis gyroscope bias offset correction, upper word
static constexpr uint8_t XA_BIAS_LOW =
0x4C; // X-axis accelerometer bias offset correction, lower word
static constexpr uint8_t XA_BIAS_HIGH =
0x4E; // X-axis accelerometer bias offset correction, upper word
static constexpr uint8_t YA_BIAS_LOW =
0x50; // Y-axis accelerometer bias offset correction, lower word
static constexpr uint8_t YA_BIAS_HIGH =
0x52; // Y-axis accelerometer bias offset correction, upper word
static constexpr uint8_t ZA_BIAS_LOW =
0x54; // Z-axis accelerometer bias offset correction, lower word
static constexpr uint8_t ZA_BIAS_HIGH =
0x56; // Z-axis accelerometer bias offset correction, upper word
static constexpr uint8_t FILT_CTRL = 0x5C; // Filter control
static constexpr uint8_t MSC_CTRL = 0x60; // Miscellaneous control
static constexpr uint8_t UP_SCALE = 0x62; // Clock scale factor, PPS mode
static constexpr uint8_t DEC_RATE =
0x64; // Decimation rate control (output data rate)
static constexpr uint8_t NULL_CNFG = 0x66; // Auto-null configuration control
static constexpr uint8_t GLOB_CMD = 0x68; // Global commands
static constexpr uint8_t FIRM_REV = 0x6C; // Firmware revision
static constexpr uint8_t FIRM_DM =
0x6E; // Firmware revision date, month and day
static constexpr uint8_t FIRM_Y = 0x70; // Firmware revision date, year
static constexpr uint8_t PROD_ID = 0x72; // Product identification
static constexpr uint8_t SERIAL_NUM =
0x74; // Serial number (relative to assembly lot)
static constexpr uint8_t USER_SCR1 = 0x76; // User scratch register 1
static constexpr uint8_t USER_SCR2 = 0x78; // User scratch register 2
static constexpr uint8_t USER_SCR3 = 0x7A; // User scratch register 3
static constexpr uint8_t FLSHCNT_LOW = 0x7C; // Flash update count, lower word
static constexpr uint8_t FLSHCNT_HIGH = 0x7E; // Flash update count, upper word
/* ADIS16470 Auto SPI Data Packets */
static constexpr uint8_t m_autospi_x_packet[16] = {
X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
static constexpr uint8_t m_autospi_y_packet[16] = {
Y_DELTANG_OUT, FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
static constexpr uint8_t m_autospi_z_packet[16] = {
Z_DELTANG_OUT, FLASH_CNT, Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT,
Y_GYRO_OUT, FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
/* ADIS16470 Constants */
const double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */
const double rad_to_deg = 57.2957795;
const double deg_to_rad = 0.0174532;
const double grav = 9.81;
/**
* Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
* return the robot's heading relative to a starting position and instant
* measurements
*
* The ADIS16470 gyro angle outputs track the robot's heading based on the
* starting position. As the robot rotates the new heading is computed by
* integrating the rate of rotation returned by the IMU. When the class is
* instantiated, a short calibration routine is performed where the IMU samples
* the gyros while at rest to determine the initial offset. This is subtracted
* from each sample to determine the heading.
*
* This class is for the ADIS16470 IMU connected via the primary SPI port
* available on the RoboRIO.
*/
class ADIS16470_IMU : public nt::NTSendable,
public wpi::SendableHelper<ADIS16470_IMU> {
public:
enum IMUAxis { kX, kY, kZ };
/**
* @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis
* is set to the IMU Z axis, and calibration time is defaulted to 4 seconds.
*/
ADIS16470_IMU();
/**
* @brief Customizable constructor. Allows the SPI port and CS to be
* customized, the yaw axis used for GetAngle() is adjustable, and initial
* calibration time can be modified.
*
* @param yaw_axis Selects the "default" axis to use for GetAngle() and
* GetRate()
*
* @param port The SPI port and CS where the IMU is connected.
*
* @param cal_time The calibration time that should be used on start-up.
*/
explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
ADIS16470CalibrationTime cal_time);
/**
* @brief Destructor. Kills the acquisiton loop and closes the SPI peripheral.
*/
~ADIS16470_IMU() override;
ADIS16470_IMU(ADIS16470_IMU&&) = default;
ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
int ConfigDecRate(uint16_t reg);
/**
* @brief Switches the active SPI port to standard SPI mode, writes the
* command to activate the new null configuration, and re-enables auto SPI.
*/
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.
*/
int ConfigCalTime(ADIS16470CalibrationTime new_cal_time);
/**
* @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
*
* Resets the gyro accumulations to a heading of zero. This can be used if
* the "zero" orientation of the sensor needs to be changed in runtime.
*/
void Reset();
/**
* @brief Returns the current integrated angle for the axis specified.
*
* The angle is based on the current accumulator value corrected by
* offset calibration and built-in IMU calibration. The angle is continuous,
* that is it will continue from 360->361 degrees. This allows algorithms
* that wouldn't want to see a discontinuity in the gyro output as it sweeps
* from 360 to 0 on the second time around. The axis returned by this
* function is adjusted based on the configured yaw_axis.
*
* @return the current heading of the robot in degrees. This heading is based
* on integration of the returned rate from the gyro.
*/
double GetAngle() const;
double GetRate() const;
double GetGyroInstantX() const;
double GetGyroInstantY() const;
double GetGyroInstantZ() const;
double GetAccelInstantX() const;
double GetAccelInstantY() const;
double GetAccelInstantZ() const;
double GetXComplementaryAngle() const;
double GetYComplementaryAngle() const;
double GetXFilteredAccelAngle() const;
double GetYFilteredAccelAngle() const;
IMUAxis GetYawAxis() const;
int SetYawAxis(IMUAxis yaw_axis);
// IMU yaw axis
IMUAxis m_yaw_axis;
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
/**
* @brief Switches to standard SPI operation. Primarily used when exiting auto
* SPI mode.
*
* @return A boolean indicating the success or failure of setting up the SPI
* peripheral in standard SPI mode.
*/
bool SwitchToStandardSPI();
/**
* @brief Switches to auto SPI operation. Primarily used when exiting standard
* SPI mode.
*
* @return A boolean indicating the success or failure of setting up the SPI
* peripheral in auto SPI mode.
*/
bool SwitchToAutoSPI();
/**
* @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.
*/
uint16_t ReadRegister(uint8_t reg);
/**
* @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
* 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.
*/
void WriteRegister(uint8_t reg, uint16_t val);
/**
* @brief Main acquisition loop. Typically called asynchronously and
* free-wheels while the robot code is active.
*/
void Acquire();
void Close();
// Integrated gyro value
double m_integ_angle = 0.0;
// Instant raw outputs
double m_gyro_x, m_gyro_y, m_gyro_z, m_accel_x, m_accel_y, m_accel_z = 0.0;
// Complementary filter variables
double m_tau = 1.0;
double m_dt, m_alpha = 0.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);
// State and resource variables
volatile bool m_thread_active = false;
volatile bool m_first_run = true;
volatile bool m_thread_idle = false;
bool m_auto_configured = false;
SPI::Port m_spi_port;
uint16_t m_calibration_time;
SPI* m_spi = nullptr;
DigitalInput* m_auto_interrupt = nullptr;
double m_scaled_sample_rate = 2500.0; // Default sample rate setting
std::thread m_acquire_task;
struct NonMovableMutexWrapper {
wpi::mutex mutex;
NonMovableMutexWrapper() = default;
NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
return *this;
}
void lock() { mutex.lock(); }
void unlock() { mutex.unlock(); }
bool try_lock() noexcept { return mutex.try_lock(); }
};
mutable NonMovableMutexWrapper m_mutex;
};
} // namespace frc

1021
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java Normal file
View File

File diff suppressed because it is too large Load Diff

969
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java Normal file
View File

@@ -0,0 +1,969 @@
// Copyright (c) FIRST and other WPILib contributors.
// 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.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.wpilibj.interfaces.Gyro;
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"
})
public class ADIS16470_IMU implements Gyro, NTSendable {
/* ADIS16470 Register Map Declaration */
private static final int FLASH_CNT = 0x00; // Flash memory write count
private static final int DIAG_STAT = 0x02; // Diagnostic and operational status
private static final int X_GYRO_LOW = 0x04; // X-axis gyroscope output, lower word
private static final int X_GYRO_OUT = 0x06; // X-axis gyroscope output, upper word
private static final int Y_GYRO_LOW = 0x08; // Y-axis gyroscope output, lower word
private static final int Y_GYRO_OUT = 0x0A; // Y-axis gyroscope output, upper word
private static final int Z_GYRO_LOW = 0x0C; // Z-axis gyroscope output, lower word
private static final int Z_GYRO_OUT = 0x0E; // Z-axis gyroscope output, upper word
private static final int X_ACCL_LOW = 0x10; // X-axis accelerometer output, lower word
private static final int X_ACCL_OUT = 0x12; // X-axis accelerometer output, upper word
private static final int Y_ACCL_LOW = 0x14; // Y-axis accelerometer output, lower word
private static final int Y_ACCL_OUT = 0x16; // Y-axis accelerometer output, upper word
private static final int Z_ACCL_LOW = 0x18; // Z-axis accelerometer output, lower word
private static final int Z_ACCL_OUT = 0x1A; // Z-axis accelerometer output, upper word
private static final int TEMP_OUT = 0x1C; // Temperature output (internal, not calibrated)
private static final int TIME_STAMP = 0x1E; // PPS mode time stamp
private static final int X_DELTANG_LOW = 0x24; // X-axis delta angle output, lower word
private static final int X_DELTANG_OUT = 0x26; // X-axis delta angle output, upper word
private static final int Y_DELTANG_LOW = 0x28; // Y-axis delta angle output, lower word
private static final int Y_DELTANG_OUT = 0x2A; // Y-axis delta angle output, upper word
private static final int Z_DELTANG_LOW = 0x2C; // Z-axis delta angle output, lower word
private static final int Z_DELTANG_OUT = 0x2E; // Z-axis delta angle output, upper word
private static final int X_DELTVEL_LOW = 0x30; // X-axis delta velocity output, lower word
private static final int X_DELTVEL_OUT = 0x32; // X-axis delta velocity output, upper word
private static final int Y_DELTVEL_LOW = 0x34; // Y-axis delta velocity output, lower word
private static final int Y_DELTVEL_OUT = 0x36; // Y-axis delta velocity output, upper word
private static final int Z_DELTVEL_LOW = 0x38; // Z-axis delta velocity output, lower word
private static final int Z_DELTVEL_OUT = 0x3A; // Z-axis delta velocity output, upper word
private static final int XG_BIAS_LOW =
0x40; // X-axis gyroscope bias offset correction, lower word
private static final int XG_BIAS_HIGH =
0x42; // X-axis gyroscope bias offset correction, upper word
private static final int YG_BIAS_LOW =
0x44; // Y-axis gyroscope bias offset correction, lower word
private static final int YG_BIAS_HIGH =
0x46; // Y-axis gyroscope bias offset correction, upper word
private static final int ZG_BIAS_LOW =
0x48; // Z-axis gyroscope bias offset correction, lower word
private static final int ZG_BIAS_HIGH =
0x4A; // Z-axis gyroscope bias offset correction, upper word
private static final int XA_BIAS_LOW =
0x4C; // X-axis accelerometer bias offset correction, lower word
private static final int XA_BIAS_HIGH =
0x4E; // X-axis accelerometer bias offset correction, upper word
private static final int YA_BIAS_LOW =
0x50; // Y-axis accelerometer bias offset correction, lower word
private static final int YA_BIAS_HIGH =
0x52; // Y-axis accelerometer bias offset correction, upper word
private static final int ZA_BIAS_LOW =
0x54; // Z-axis accelerometer bias offset correction, lower word
private static final int ZA_BIAS_HIGH =
0x56; // Z-axis accelerometer bias offset correction, upper word
private static final int FILT_CTRL = 0x5C; // Filter control
private static final int MSC_CTRL = 0x60; // Miscellaneous control
private static final int UP_SCALE = 0x62; // Clock scale factor, PPS mode
private static final int DEC_RATE = 0x64; // Decimation rate control (output data rate)
private static final int NULL_CNFG = 0x66; // Auto-null configuration control
private static final int GLOB_CMD = 0x68; // Global commands
private static final int FIRM_REV = 0x6C; // Firmware revision
private static final int FIRM_DM = 0x6E; // Firmware revision date, month and day
private static final int FIRM_Y = 0x70; // Firmware revision date, year
private static final int PROD_ID = 0x72; // Product identification
private static final int SERIAL_NUM = 0x74; // Serial number (relative to assembly lot)
private static final int USER_SCR1 = 0x76; // User scratch register 1
private static final int USER_SCR2 = 0x78; // User scratch register 2
private static final int USER_SCR3 = 0x7A; // User scratch register 3
private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word
private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word
private static final byte[] m_autospi_x_packet = {
X_DELTANG_OUT,
FLASH_CNT,
X_DELTANG_LOW,
FLASH_CNT,
X_GYRO_OUT,
FLASH_CNT,
Y_GYRO_OUT,
FLASH_CNT,
Z_GYRO_OUT,
FLASH_CNT,
X_ACCL_OUT,
FLASH_CNT,
Y_ACCL_OUT,
FLASH_CNT,
Z_ACCL_OUT,
FLASH_CNT
};
private static final byte[] m_autospi_y_packet = {
Y_DELTANG_OUT,
FLASH_CNT,
Y_DELTANG_LOW,
FLASH_CNT,
X_GYRO_OUT,
FLASH_CNT,
Y_GYRO_OUT,
FLASH_CNT,
Z_GYRO_OUT,
FLASH_CNT,
X_ACCL_OUT,
FLASH_CNT,
Y_ACCL_OUT,
FLASH_CNT,
Z_ACCL_OUT,
FLASH_CNT
};
private static final byte[] m_autospi_z_packet = {
Z_DELTANG_OUT,
FLASH_CNT,
Z_DELTANG_LOW,
FLASH_CNT,
X_GYRO_OUT,
FLASH_CNT,
Y_GYRO_OUT,
FLASH_CNT,
Z_GYRO_OUT,
FLASH_CNT,
X_ACCL_OUT,
FLASH_CNT,
Y_ACCL_OUT,
FLASH_CNT,
Z_ACCL_OUT,
FLASH_CNT
};
public enum IMUAxis {
kX,
kY,
kZ
}
public enum ADIS16470CalibrationTime {
_32ms(0),
_64ms(1),
_128ms(2),
_256ms(3),
_512ms(4),
_1s(5),
_2s(6),
_4s(7),
_8s(8),
_16s(9),
_32s(10),
_64s(11);
private int value;
private ADIS16470CalibrationTime(int value) {
this.value = value;
}
}
// 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;
// User-specified yaw axis
private IMUAxis m_yaw_axis;
// Instant raw outputs
private double m_gyro_x = 0.0;
private double m_gyro_y = 0.0;
private double m_gyro_z = 0.0;
private double m_accel_x = 0.0;
private double m_accel_y = 0.0;
private double m_accel_z = 0.0;
// Integrated gyro angle
private double m_integ_angle = 0.0;
// Complementary filter variables
private double m_dt = 0.0;
private double m_alpha = 0.0;
private double m_tau = 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
private volatile boolean m_thread_active = false;
private int m_calibration_time = 0;
private volatile boolean m_first_run = true;
private volatile boolean m_thread_idle = false;
private boolean m_auto_configured = false;
private double m_scaled_sample_rate = 2500.0;
// 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;
private static class AcquireTask implements Runnable {
private ADIS16470_IMU imu;
public AcquireTask(ADIS16470_IMU imu) {
this.imu = imu;
}
@Override
public void run() {
imu.acquire();
}
}
public ADIS16470_IMU() {
this(IMUAxis.kZ, SPI.Port.kOnboardCS0, ADIS16470CalibrationTime._4s);
}
/**
* @param yaw_axis The axis that measures the yaw
* @param port The SPI Port the gyro is plugged into
* @param cal_time Calibration time
*/
public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, ADIS16470CalibrationTime cal_time) {
m_yaw_axis = yaw_axis;
m_calibration_time = cal_time.value;
m_spi_port = port;
m_acquire_task = new Thread(new AcquireTask(this));
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
// 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();
m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
Timer.delay(0.25); // Wait 250ms for reset to complete
if (!switchToStandardSPI()) {
return;
}
// Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
// 400Hz)
writeRegister(DEC_RATE, 4);
// Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
// PoP
writeRegister(MSC_CTRL, 1);
// Configure IMU internal Bartlett filter
writeRegister(FILT_CTRL, 0);
// Configure continuous bias calibration time based on user setting
writeRegister(NULL_CNFG, (m_calibration_time | 0x0700));
// Notify DS that IMU calibration delay is active
DriverStation.reportWarning(
"ADIS16470 IMU Detected. Starting initial calibration delay.", false);
// Wait for samples to accumulate internal to the IMU (110% of user-defined
// time)
try {
Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000));
} catch (InterruptedException e) {
}
// Write offset calibration command to IMU
writeRegister(GLOB_CMD, 0x0001);
// Configure and enable auto SPI
if (!switchToAutoSPI()) {
return;
}
// Let the user know the IMU was initiallized successfully
DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false);
// Drive "Ready" LED low
m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low
// Report usage and post data to DS
HAL.report(tResourceType.kResourceType_ADIS16470, 0);
}
/**
* @param buf
* @return
*/
private static int toUShort(ByteBuffer buf) {
return (buf.getShort(0)) & 0xFFFF;
}
/**
* @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) << 0));
}
/**
* @param buf
* @return
*/
private static int toInt(int... buf) {
return (int)
((buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF));
}
/**
* Switch to standard SPI mode.
*
* @return
*/
private boolean switchToStandardSPI() {
// Check to see whether the acquire thread is active. If so, wait for it to stop
// producing data.
if (m_thread_active) {
m_thread_active = false;
while (!m_thread_idle) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
}
}
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) {
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.
int[] trashBuffer = new int[200];
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0);
while (data_count > 0) {
m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0);
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(2000000);
m_spi.setMSBFirst();
m_spi.setSampleDataOnTrailingEdge();
m_spi.setClockActiveLow();
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;
}
}
}
/** @return */
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) {
m_spi.initAuto(8200);
m_auto_configured = true;
}
// Do we need to change auto SPI settings?
switch (m_yaw_axis) {
case kX:
m_spi.setAutoTransmitData(m_autospi_x_packet, 2);
break;
case kY:
m_spi.setAutoTransmitData(m_autospi_y_packet, 2);
break;
default:
m_spi.setAutoTransmitData(m_autospi_z_packet, 2);
break;
}
// Configure auto stall time
m_spi.configureAutoStall(5, 1000, 1);
// Kick off auto SPI (Note: Device configration impossible after auto SPI is
// 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;
m_thread_active = true;
m_acquire_task.start();
System.out.println("Processing thread activated!");
} else {
// The thread was running, re-init run variables and start it up again.
m_first_run = true;
m_thread_active = true;
System.out.println("Processing thread activated!");
}
// Looks like the thread didn't start for some reason. Abort.
if (!m_acquire_task.isAlive()) {
DriverStation.reportError("Failed to start/restart the acquire() thread.", false);
close();
return false;
}
return true;
}
/**
* 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
*/
public int configCalTime(ADIS16470CalibrationTime new_cal_time) {
if (m_calibration_time == new_cal_time.value) {
return 1;
}
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
return 2;
}
m_calibration_time = new_cal_time.value;
writeRegister(NULL_CNFG, (m_calibration_time | 0x700));
if (!switchToAutoSPI()) {
DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
return 2;
}
return 0;
}
public int configDecRate(int reg) {
int m_reg = reg;
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
return 2;
}
if (m_reg > 1999) {
DriverStation.reportError("Attemted to write an invalid deimation value.", false);
m_reg = 1999;
}
m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
writeRegister(DEC_RATE, m_reg);
System.out.println("Decimation register: " + readRegister(DEC_RATE));
if (!switchToAutoSPI()) {
DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
return 2;
}
return 0;
}
/** {@inheritDoc} */
@Override
public void calibrate() {
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
}
writeRegister(GLOB_CMD, 0x0001);
if (!switchToAutoSPI()) {
DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
}
;
}
/**
* Sets the yaw axis
*
* @param yaw_axis The new yaw axis to use
* @return 1 if the new yaw axis is the same as the current one, 2 if the switch to Standard SPI
* failed, else 0.
*/
public int setYawAxis(IMUAxis yaw_axis) {
if (m_yaw_axis == yaw_axis) {
return 1;
}
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
return 2;
}
m_yaw_axis = yaw_axis;
if (!switchToAutoSPI()) {
DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false);
}
return 0;
}
/**
* @param reg
* @return
*/
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);
m_spi.write(buf, 2);
m_spi.read(false, buf, 2);
return toUShort(buf);
}
/**
* @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));
m_spi.write(buf, 2);
// high byte
buf.put(0, (byte) (0x81 | reg));
buf.put(1, (byte) (val >> 8));
m_spi.write(buf, 2);
}
/** {@inheritDoc} */
public void reset() {
synchronized (this) {
m_integ_angle = 0.0;
}
}
/** Delete (free) the spi port used for the IMU. */
@Override
public void close() {
if (m_thread_active) {
m_thread_active = false;
try {
if (m_acquire_task != null) {
m_acquire_task.join();
m_acquire_task = null;
}
} catch (InterruptedException e) {
}
if (m_spi != null) {
if (m_auto_configured) {
m_spi.stopAuto();
}
m_spi.close();
m_auto_configured = false;
if (m_auto_interrupt != null) {
m_auto_interrupt.close();
m_auto_interrupt = null;
}
m_spi = null;
}
}
System.out.println("Finished cleaning up after the IMU driver.");
}
/** */
private void acquire() {
// Set data packet length
final int dataset_len = 19; // 18 data points + timestamp
final int BUFFER_SIZE = 4000;
// 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 = 0.0;
double gyro_x = 0.0;
double gyro_y = 0.0;
double gyro_z = 0.0;
double accel_x = 0.0;
double accel_y = 0.0;
double accel_z = 0.0;
double gyro_x_si = 0.0;
double gyro_y_si = 0.0;
double gyro_z_si = 0.0;
double 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
try {
Thread.sleep(10);
} catch (InterruptedException e) {
}
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 */
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)
// 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 = ((double) buffer[i] - previous_timestamp) / 1000000.0;
/*
* 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]));
*/
/*
* Get delta angle value for selected yaw axis and scale by the elapsed time
* (based on timestamp)
*/
delta_angle =
(toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
/ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
gyro_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0);
gyro_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0);
gyro_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0);
accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0);
accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0);
accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0);
// Convert scaled sensor data to SI units (for tilt calculations)
// TODO: Should the unit outputs be selectable?
gyro_x_si = gyro_x * deg_to_rad;
gyro_y_si = gyro_y * deg_to_rad;
gyro_z_si = gyro_z * deg_to_rad;
accel_x_si = accel_x * grav;
accel_y_si = accel_y * grav;
accel_z_si = accel_z * grav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
m_alpha = m_tau / (m_tau + m_dt);
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_y_si);
compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_x_si);
}
synchronized (this) {
/* 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 = 0.0;
} else {
m_integ_angle += delta_angle;
}
m_gyro_x = gyro_x;
m_gyro_y = gyro_y;
m_gyro_z = gyro_z;
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_first_run = false;
}
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
delta_angle = 0.0;
gyro_x = 0.0;
gyro_y = 0.0;
gyro_z = 0.0;
accel_x = 0.0;
accel_y = 0.0;
accel_z = 0.0;
gyro_x_si = 0.0;
gyro_y_si = 0.0;
gyro_z_si = 0.0;
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;
} else if (accAngle > compAngle + Math.PI) {
compAngle = compAngle + 2.0 * Math.PI;
}
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;
} else if (accelZ > 0.0 && accelAngle < 0.0) {
accelAngle = 2.0 * Math.PI + accelAngle;
}
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;
}
/** {@inheritDoc} */
public synchronized double getAngle() {
return m_integ_angle;
}
/** {@inheritDoc} */
public synchronized double getRate() {
switch (m_yaw_axis) {
case kX:
return m_gyro_x;
case kY:
return m_gyro_y;
case kZ:
return m_gyro_z;
}
return 0.0;
}
/** @return Yaw Axis */
public IMUAxis getYawAxis() {
return m_yaw_axis;
}
/** @return current gyro angle in the X direction */
public synchronized double getGyroInstantX() {
return m_gyro_x;
}
/** @return current gyro angle in the Y axis */
public synchronized double getGyroInstantY() {
return m_gyro_y;
}
/** @return current gyro angle in the Z axis */
public synchronized double getGyroInstantZ() {
return m_gyro_z;
}
/** @return current acceleration in the X axis */
public synchronized double getAccelInstantX() {
return m_accel_x;
}
/** @return current acceleration in the Y axis */
public synchronized double getAccelInstantY() {
return m_accel_y;
}
/** @return current acceleration in the Z axis */
public synchronized double getAccelInstantZ() {
return m_accel_z;
}
/** @return X axis complementary angle */
public synchronized double getXComplementaryAngle() {
return m_compAngleX;
}
/** @return Y axis complementary angle */
public synchronized double getYComplementaryAngle() {
return m_compAngleY;
}
/** @return X axis filtered acceleration angle */
public synchronized double getXFilteredAccelAngle() {
return m_accelAngleX;
}
/** @return Y axis filtered acceleration angle */
public synchronized double getYFilteredAccelAngle() {
return m_accelAngleY;
}
@Override
public void initSendable(NTSendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}