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

[wpilib] ADIS16470: Add access to all 3 axes (#6074)

Browse Source
This commit is contained in:
Bryce Roethel
2023-12-26 16:39:37 -05:00
committed by GitHub
parent 795d4be9fd
commit ab78b930e9
3 changed files with 624 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -63,14 +63,35 @@ inline void ADISReportError(int32_t status, const char* file, int line,
* Constructor.
*/
ADIS16470_IMU::ADIS16470_IMU()
: ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
: ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
IMUAxis roll_axis)
: ADIS16470_IMU(yaw_axis, pitch_axis, roll_axis, SPI::Port::kOnboardCS0,
CalibrationTime::_4s) {}
ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
IMUAxis roll_axis, SPI::Port port,
CalibrationTime cal_time)
: m_yaw_axis(yaw_axis),
m_pitch_axis(pitch_axis),
m_roll_axis(roll_axis),
m_spi_port(port),
m_calibration_time(static_cast<uint16_t>(cal_time)),
m_simDevice("Gyro:ADIS16470", port) {
if (yaw_axis == kYaw || yaw_axis == kPitch || yaw_axis == kRoll ||
pitch_axis == kYaw || pitch_axis == kPitch || pitch_axis == kRoll ||
roll_axis == kYaw || roll_axis == kPitch || roll_axis == kRoll) {
REPORT_ERROR(
"ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or "
"IMUAxis.kZ as arguments.");
REPORT_ERROR(
"Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)");
yaw_axis = kZ;
pitch_axis = kY;
roll_axis = kX;
}
if (m_simDevice) {
m_connected =
m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
@@ -266,17 +287,8 @@ bool ADIS16470_IMU::SwitchToAutoSPI() {
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;
}
m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2);
// Configure auto stall time
m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
// Kick off DMA SPI (Note: Device configuration impossible after SPI DMA is
@@ -445,7 +457,9 @@ void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
**/
void ADIS16470_IMU::Reset() {
std::scoped_lock sync(m_mutex);
m_integ_angle = 0.0;
m_integ_angle_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
}
void ADIS16470_IMU::Close() {
@@ -502,7 +516,7 @@ ADIS16470_IMU::~ADIS16470_IMU() {
**/
void ADIS16470_IMU::Acquire() {
// Set data packet length
const int dataset_len = 19; // 18 data points + timestamp
const int dataset_len = 27; // 26 data points + timestamp
/* Fixed buffer size */
const int BUFFER_SIZE = 4000;
@@ -513,7 +527,9 @@ void ADIS16470_IMU::Acquire() {
int data_remainder = 0;
int data_to_read = 0;
uint32_t previous_timestamp = 0;
double delta_angle = 0.0;
double delta_angle_x = 0.0;
double delta_angle_y = 0.0;
double delta_angle_z = 0.0;
double gyro_rate_x = 0.0;
double gyro_rate_y = 0.0;
double gyro_rate_z = 0.0;
@@ -562,14 +578,22 @@ void ADIS16470_IMU::Acquire() {
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_rate_x = (BuffToShort(&buffer[i + 7]) / 10.0);
gyro_rate_y = (BuffToShort(&buffer[i + 9]) / 10.0);
gyro_rate_z = (BuffToShort(&buffer[i + 11]) / 10.0);
accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
delta_angle_x =
(ToInt(&buffer[i + 3]) * delta_angle_sf) /
(m_scaled_sample_rate / (buffer[i] - previous_timestamp));
delta_angle_y =
(ToInt(&buffer[i + 7]) * delta_angle_sf) /
(m_scaled_sample_rate / (buffer[i] - previous_timestamp));
delta_angle_z =
(ToInt(&buffer[i + 11]) * delta_angle_sf) /
(m_scaled_sample_rate / (buffer[i] - previous_timestamp));
gyro_rate_x = (BuffToShort(&buffer[i + 15]) / 10.0);
gyro_rate_y = (BuffToShort(&buffer[i + 17]) / 10.0);
gyro_rate_z = (BuffToShort(&buffer[i + 19]) / 10.0);
accel_x = (BuffToShort(&buffer[i + 21]) / 800.0);
accel_y = (BuffToShort(&buffer[i + 23]) / 800.0);
accel_z = (BuffToShort(&buffer[i + 25]) / 800.0);
// Convert scaled sensor data to SI units
gyro_rate_x_si = gyro_rate_x * deg_to_rad;
@@ -611,9 +635,13 @@ void ADIS16470_IMU::Acquire() {
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;
m_integ_angle_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
} else {
m_integ_angle += delta_angle;
m_integ_angle_x += delta_angle_x;
m_integ_angle_y += delta_angle_y;
m_integ_angle_z += delta_angle_z;
}
m_gyro_rate_x = gyro_rate_x;
m_gyro_rate_y = gyro_rate_y;
@@ -634,7 +662,9 @@ void ADIS16470_IMU::Acquire() {
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
delta_angle = 0.0;
delta_angle_x = 0.0;
delta_angle_y = 0.0;
delta_angle_z = 0.0;
gyro_rate_x = 0.0;
gyro_rate_y = 0.0;
gyro_rate_z = 0.0;
@@ -696,50 +726,143 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
return compAngle;
}
units::degree_t ADIS16470_IMU::GetAngle() const {
switch (m_yaw_axis) {
void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
break;
}
switch (axis) {
case kX:
SetGyroAngleX(angle);
break;
case kY:
SetGyroAngleY(angle);
break;
case kZ:
SetGyroAngleZ(angle);
break;
default:
break;
}
}
void ADIS16470_IMU::SetGyroAngleX(units::degree_t angle) {
std::scoped_lock sync(m_mutex);
m_integ_angle_x = angle.value();
}
void ADIS16470_IMU::SetGyroAngleY(units::degree_t angle) {
std::scoped_lock sync(m_mutex);
m_integ_angle_y = angle.value();
}
void ADIS16470_IMU::SetGyroAngleZ(units::degree_t angle) {
std::scoped_lock sync(m_mutex);
m_integ_angle_z = angle.value();
}
units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
break;
}
switch (axis) {
case kX:
if (m_simGyroAngleX) {
return units::degree_t{m_simGyroAngleX.Get()};
}
break;
{
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_angle_x};
}
case kY:
if (m_simGyroAngleY) {
return units::degree_t{m_simGyroAngleY.Get()};
}
break;
{
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_angle_y};
}
case kZ:
if (m_simGyroAngleZ) {
return units::degree_t{m_simGyroAngleZ.Get()};
}
{
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_angle_z};
}
default:
break;
}
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_angle};
return units::degree_t{0.0};
}
units::degrees_per_second_t ADIS16470_IMU::GetRate() const {
if (m_yaw_axis == kX) {
if (m_simGyroRateX) {
return units::degrees_per_second_t{m_simGyroRateX.Get()};
}
std::scoped_lock sync(m_mutex);
return units::degrees_per_second_t{m_gyro_rate_x};
} else if (m_yaw_axis == kY) {
if (m_simGyroRateY) {
return units::degrees_per_second_t{m_simGyroRateY.Get()};
}
std::scoped_lock sync(m_mutex);
return units::degrees_per_second_t{m_gyro_rate_y};
} else if (m_yaw_axis == kZ) {
if (m_simGyroRateZ) {
return units::degrees_per_second_t{m_simGyroRateZ.Get()};
}
std::scoped_lock sync(m_mutex);
return units::degrees_per_second_t{m_gyro_rate_z};
} else {
return 0_deg_per_s;
units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
break;
}
switch (axis) {
case kX:
if (m_simGyroRateX) {
return units::degrees_per_second_t{m_simGyroRateX.Get()};
}
{
std::scoped_lock sync(m_mutex);
return units::degrees_per_second_t{m_gyro_rate_x};
}
case kY:
if (m_simGyroRateY) {
return units::degrees_per_second_t{m_simGyroRateY.Get()};
}
{
std::scoped_lock sync(m_mutex);
return units::degrees_per_second_t{m_gyro_rate_y};
}
case kZ:
if (m_simGyroRateZ) {
return units::degrees_per_second_t{m_simGyroRateZ.Get()};
}
{
std::scoped_lock sync(m_mutex);
return units::degrees_per_second_t{m_gyro_rate_z};
}
default:
break;
}
return 0_deg_per_s;
}
units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
@@ -790,20 +913,12 @@ ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
return m_yaw_axis;
}
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;
ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetPitchAxis() const {
return m_pitch_axis;
}
ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetRollAxis() const {
return m_roll_axis;
}
int ADIS16470_IMU::GetPort() const {
@@ -819,5 +934,5 @@ int ADIS16470_IMU::GetPort() const {
void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16470 IMU");
builder.AddDoubleProperty(
"Yaw Angle", [=, this] { return GetAngle().value(); }, nullptr);
"Yaw Angle", [=, this] { return GetAngle(kYaw).value(); }, nullptr);
}

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

@@ -69,38 +69,23 @@ class ADIS16470_IMU : public wpi::Sendable,
_64s = 11
};
enum IMUAxis { kX, kY, kZ };
enum IMUAxis { kX, kY, kZ, kYaw, kPitch, kRoll };
/**
* @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();
ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis);
/**
* @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,
explicit ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
IMUAxis roll_axis, frc::SPI::Port port,
CalibrationTime cal_time);
/**
* @brief Destructor. Kills the acquisition loop and closes the SPI
* peripheral.
*/
~ADIS16470_IMU() override;
ADIS16470_IMU(ADIS16470_IMU&&) = default;
ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
/**
* @brief Configures the decimation rate of the IMU.
*/
int ConfigDecRate(uint16_t reg);
/**
@@ -116,22 +101,59 @@ class ADIS16470_IMU : public wpi::Sendable,
int ConfigCalTime(CalibrationTime 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.
* @brief 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();
/**
* Returns the yaw axis angle in degrees (CCW positive).
* Allow the designated gyro angle to be set to a given value. This may happen
* with unread values in the buffer, it is suggested that the IMU is not
* moving when this method is run.
*
* @param axis IMUAxis that will be changed
* @param angle The new angle (CCW positive)
*/
units::degree_t GetAngle() const;
void SetGyroAngle(IMUAxis axis, units::degree_t angle);
/**
* Returns the yaw axis angular rate in degrees per second (CCW positive).
* Allow the gyro angle X to be set to a given value. This may happen with
* unread values in the buffer, it is suggested that the IMU is not moving
* when this method is run.
*
* @param angle The new angle (CCW positive)
*/
units::degrees_per_second_t GetRate() const;
void SetGyroAngleX(units::degree_t angle);
/**
* Allow the gyro angle Y to be set to a given value. This may happen with
* unread values in the buffer, it is suggested that the IMU is not moving
* when this method is run.
*
* @param angle The new angle (CCW positive)
*/
void SetGyroAngleY(units::degree_t angle);
/**
* Allow the gyro angle Z to be set to a given value. This may happen with
* unread values in the buffer, it is suggested that the IMU is not moving
* when this method is run.
*
* @param angle The new angle (CCW positive)
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* @param axis The IMUAxis whose angle to return
* @return The axis angle (CCW positive)
*/
units::degree_t GetAngle(IMUAxis axis) const;
/**
* @param axis The IMUAxis whose rate to return
* @return Axis angular rate (CCW positive)
*/
units::degrees_per_second_t GetRate(IMUAxis axis) const;
/**
* Returns the acceleration in the X axis.
@@ -148,25 +170,60 @@ class ADIS16470_IMU : public wpi::Sendable,
*/
units::meters_per_second_squared_t GetAccelZ() const;
/**
* Returns the X-axis complementary angle.
*/
units::degree_t GetXComplementaryAngle() const;
/**
* Returns the Y-axis complementary angle.
*/
units::degree_t GetYComplementaryAngle() const;
/**
* Returns the X-axis filtered acceleration angle.
*/
units::degree_t GetXFilteredAccelAngle() const;
/**
* Returns the Y-axis filtered acceleration angle.
*/
units::degree_t GetYFilteredAccelAngle() const;
/**
* Returns which axis, kX, kY, or kZ, is set to the yaw axis.
*
* @return IMUAxis Yaw Axis
*/
IMUAxis GetYawAxis() const;
int SetYawAxis(IMUAxis yaw_axis);
bool IsConnected() const;
// IMU yaw axis
IMUAxis m_yaw_axis;
/**
* Returns which axis, kX, kY, or kZ, is set to the pitch axis.
*
* @return IMUAxis Pitch Axis
*/
IMUAxis GetPitchAxis() const;
/**
* Get the SPI port number.
* Returns which axis, kX, kY, or kZ, is set to the roll axis.
*
* @return IMUAxis Roll Axis
*/
IMUAxis GetRollAxis() const;
/**
* Checks the connection status of the IMU.
*
* @return True if the IMU is connected, false otherwise.
*/
bool IsConnected() const;
IMUAxis m_yaw_axis;
IMUAxis m_pitch_axis;
IMUAxis m_roll_axis;
/**
* Gets the SPI port number.
*
* @return The SPI port number.
*/
@@ -175,7 +232,7 @@ class ADIS16470_IMU : public wpi::Sendable,
void InitSendable(wpi::SendableBuilder& builder) override;
private:
/* ADIS16470 Register Map Declaration */
// 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
@@ -276,25 +333,15 @@ class ADIS16470_IMU : public wpi::Sendable,
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};
// Auto SPI Data Packet to read all thrre gyro axes.
static constexpr uint8_t m_autospi_allangle_packet[24] = {
X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, Y_DELTANG_OUT,
FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, 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};
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 */
static constexpr double delta_angle_sf =
2160.0 / 2147483648.0; /* 2160 / (2^31) */
static constexpr double delta_angle_sf = 2160.0 / 2147483648.0;
static constexpr double rad_to_deg = 57.2957795;
static constexpr double deg_to_rad = 0.0174532;
static constexpr double grav = 9.81;
@@ -350,8 +397,10 @@ class ADIS16470_IMU : public wpi::Sendable,
void Close();
// Integrated gyro value
double m_integ_angle = 0.0;
// Integrated gyro angles.
double m_integ_angle_x = 0.0;
double m_integ_angle_y = 0.0;
double m_integ_angle_z = 0.0;
// Instant raw outputs
double m_gyro_rate_x = 0.0;

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

@@ -122,45 +122,21 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
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 = {
// Weight between the previous and current gyro angles represents 1 second for the timestamp,
// this is the point at which we ignore the previous angle because it is too old to be of use.
// The IMU timestamp conversion is 1 = 49.02us, the value 1_000_000 is the number of microseconds
// we average over.
private static final double AVERAGE_RATE_SCALING_FACTOR = 49.02 / 1_000_000;
private static final byte[] m_autospi_allAngle_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,
@@ -179,18 +155,31 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
FLASH_CNT
};
/** Calibration times for the ADIS16470. */
public enum CalibrationTime {
/** 32ms calibration */
_32ms(0),
/** 64ms calibration */
_64ms(1),
/** 128ms calibration */
_128ms(2),
/** 256ms calibration */
_256ms(3),
/** 512ms calibration */
_512ms(4),
/** 1 second calibration */
_1s(5),
/** 2 second calibration */
_2s(6),
/** 4 second calibration */
_4s(7),
/** 8 second calibration */
_8s(8),
/** 16 second calibration */
_16s(9),
/** 32 second calibration */
_32s(10),
/** 64 second calibration */
_64s(11);
private final int value;
@@ -200,10 +189,25 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
}
/**
* IMU axes.
*
* <p>kX, kY, and kZ refer to the IMU's X, Y, and Z axes respectively. kYaw, kPitch, and kRoll are
* configured by the user to refer to an X, Y, or Z axis.
*/
public enum IMUAxis {
/** The IMU's X axis */
kX,
/** The IMU's Y axis */
kY,
kZ
/** The IMU's Z axis */
kZ,
/** The user configured yaw axis */
kYaw,
/** The user configured pitch axis */
kPitch,
/** The user configured roll axis */
kRoll,
}
// Static Constants
@@ -212,19 +216,26 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
private static final double deg_to_rad = 0.0174532;
private static final double grav = 9.81;
// User-specified yaw axis
// User-specified axes
private IMUAxis m_yaw_axis;
private IMUAxis m_pitch_axis;
private IMUAxis m_roll_axis;
// Instant raw outputs
private double m_gyro_rate_x = 0.0;
private double m_gyro_rate_y = 0.0;
private double m_gyro_rate_z = 0.0;
private double m_average_gyro_rate_x = 0.0;
private double m_average_gyro_rate_y = 0.0;
private double m_average_gyro_rate_z = 0.0;
private double m_accel_x = 0.0;
private double m_accel_y = 0.0;
private double m_accel_z = 0.0;
// Integrated gyro angle
private double m_integ_angle = 0.0;
private double m_integ_angle_x = 0.0;
private double m_integ_angle_y = 0.0;
private double m_integ_angle_z = 0.0;
// Complementary filter variables
private double m_dt = 0.0;
@@ -278,18 +289,71 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
}
/**
* Creates a new ADIS16740 IMU object. The default setup is the onboard SPI port with a
* calibration time of 4 seconds. Yaw, pitch, and roll are kZ, kX, and kY respectively.
*/
public ADIS16470_IMU() {
this(IMUAxis.kZ, SPI.Port.kOnboardCS0, CalibrationTime._4s);
this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._4s);
}
/**
* Creates a new ADIS16740 IMU object. The default setup is the onboard SPI port with a
* calibration time of 4 seconds.
*
* <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in
* an error. </i></b>
*
* @param yaw_axis The axis that measures the yaw
* @param pitch_axis The axis that measures the pitch
* @param roll_axis The axis that measures the roll
*/
public ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) {
this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._4s);
}
/**
* Creates a new ADIS16740 IMU object.
*
* <p><b><i> Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in
* an error. </i></b>
*
* @param yaw_axis The axis that measures the yaw
* @param pitch_axis The axis that measures the pitch
* @param roll_axis The axis that measures the roll
* @param port The SPI Port the gyro is plugged into
* @param cal_time Calibration time
*/
@SuppressWarnings("this-escape")
public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
public ADIS16470_IMU(
IMUAxis yaw_axis,
IMUAxis pitch_axis,
IMUAxis roll_axis,
SPI.Port port,
CalibrationTime cal_time) {
if (yaw_axis == IMUAxis.kYaw
|| yaw_axis == IMUAxis.kPitch
|| yaw_axis == IMUAxis.kRoll
|| pitch_axis == IMUAxis.kYaw
|| pitch_axis == IMUAxis.kPitch
|| pitch_axis == IMUAxis.kRoll
|| roll_axis == IMUAxis.kYaw
|| roll_axis == IMUAxis.kPitch
|| roll_axis == IMUAxis.kRoll) {
DriverStation.reportError(
"ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or IMUAxis.kZ as arguments.",
false);
DriverStation.reportError(
"Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)", false);
yaw_axis = IMUAxis.kZ;
pitch_axis = IMUAxis.kY;
roll_axis = IMUAxis.kX;
}
m_yaw_axis = yaw_axis;
m_pitch_axis = pitch_axis;
m_roll_axis = roll_axis;
m_calibration_time = cal_time.value;
m_spi_port = port;
@@ -369,6 +433,11 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
m_connected = true;
}
/**
* Checks the connection status of the IMU.
*
* @return True if the IMU is connected, false otherwise.
*/
public boolean isConnected() {
if (m_simConnected != null) {
return m_simConnected.get();
@@ -411,7 +480,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
/**
* Switch to standard SPI mode.
*
* @return
* @return True if successful, false otherwise.
*/
private boolean switchToStandardSPI() {
// Check to see whether the acquire thread is active. If so, wait for it to stop
@@ -477,7 +546,9 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
/**
* @return
* Switch to auto SPI mode.
*
* @return True if successful, false otherwise.
*/
boolean switchToAutoSPI() {
// No SPI port has been set up. Go set one up first.
@@ -499,17 +570,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
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;
}
m_spi.setAutoTransmitData(m_autospi_allAngle_packet, 2);
// Configure auto stall time
m_spi.configureAutoStall(5, 1000, 1);
// Kick off auto SPI (Note: Device configuration impossible after auto SPI is
@@ -561,6 +622,12 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
return 0;
}
/**
* Configures the decimation rate of the IMU.
*
* @param reg The new decimation value.
* @return 0 if OK, 2 if error
*/
public int configDecRate(int reg) {
int m_reg = reg;
if (!switchToStandardSPI()) {
@@ -568,7 +635,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
return 2;
}
if (m_reg > 1999) {
DriverStation.reportError("Attempted to write an invalid deimation value.", false);
DriverStation.reportError("Attempted to write an invalid decimation value.", false);
m_reg = 1999;
}
m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
@@ -596,28 +663,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
}
/**
* 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
@@ -650,12 +695,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
m_spi.write(buf, 2);
}
public void reset() {
synchronized (this) {
m_integ_angle = 0.0;
}
}
/** Delete (free) the spi port used for the IMU. */
@Override
public void close() {
@@ -691,7 +730,7 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
/** */
private void acquire() {
// Set data packet length
final int dataset_len = 19; // 18 data points + timestamp
final int dataset_len = 27; // 26 data points + timestamp
final int BUFFER_SIZE = 4000;
// Set up buffers and variables
@@ -700,7 +739,9 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
int data_remainder = 0;
int data_to_read = 0;
double previous_timestamp = 0.0;
double delta_angle = 0.0;
double delta_angle_x = 0.0;
double delta_angle_y = 0.0;
double delta_angle_z = 0.0;
double gyro_rate_x = 0.0;
double gyro_rate_y = 0.0;
double gyro_rate_z = 0.0;
@@ -730,8 +771,8 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
data_count =
m_spi.readAutoReceivedData(
buffer, 0, 0); // Read number of bytes currently stored in the
// buffer
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
@@ -772,18 +813,27 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
*/
/*
* Get delta angle value for selected yaw axis and scale by the elapsed time
* Get delta angle value for all 3 axes and scale by the elapsed time
* (based on timestamp)
*/
delta_angle =
delta_angle_x =
(toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf)
/ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
gyro_rate_x = (toShort(buffer[i + 7], buffer[i + 8]) / 10.0);
gyro_rate_y = (toShort(buffer[i + 9], buffer[i + 10]) / 10.0);
gyro_rate_z = (toShort(buffer[i + 11], buffer[i + 12]) / 10.0);
accel_x = (toShort(buffer[i + 13], buffer[i + 14]) / 800.0);
accel_y = (toShort(buffer[i + 15], buffer[i + 16]) / 800.0);
accel_z = (toShort(buffer[i + 17], buffer[i + 18]) / 800.0);
delta_angle_y =
(toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf)
/ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
delta_angle_z =
(toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14])
* delta_angle_sf)
/ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0);
gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0);
gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0);
accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0);
accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0);
accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0);
// Convert scaled sensor data to SI units (for tilt calculations)
// TODO: Should the unit outputs be selectable?
@@ -809,6 +859,10 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si)));
compAngleX = accelAngleX;
compAngleY = accelAngleY;
m_average_gyro_rate_x = 0.0;
m_average_gyro_rate_y = 0.0;
m_average_gyro_rate_z = 0.0;
} else {
// Run inclinometer calculations
accelAngleX =
@@ -830,9 +884,13 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
* Don't accumulate first run. previous_timestamp will be "very" old and the
* integration will end up way off
*/
m_integ_angle = 0.0;
m_integ_angle_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
} else {
m_integ_angle += delta_angle;
m_integ_angle_x += delta_angle_x;
m_integ_angle_y += delta_angle_y;
m_integ_angle_z += delta_angle_z;
}
m_gyro_rate_x = gyro_rate_x;
m_gyro_rate_y = gyro_rate_y;
@@ -844,16 +902,28 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
m_compAngleY = compAngleY * rad_to_deg;
m_accelAngleX = accelAngleX * rad_to_deg;
m_accelAngleY = accelAngleY * rad_to_deg;
m_average_gyro_rate_x += gyro_rate_x;
m_average_gyro_rate_y += gyro_rate_y;
m_average_gyro_rate_z += gyro_rate_z;
}
m_first_run = false;
}
// The inverse of data to read divided by dataset length, his is the number of iterations
// of the for loop inverted (so multiplication can be used instead of division)
double invTotalIterations = (double) dataset_len / data_to_read;
m_average_gyro_rate_x *= invTotalIterations;
m_average_gyro_rate_y *= invTotalIterations;
m_average_gyro_rate_z *= invTotalIterations;
} else {
m_thread_idle = true;
data_count = 0;
data_remainder = 0;
data_to_read = 0;
previous_timestamp = 0.0;
delta_angle = 0.0;
delta_angle_x = 0.0;
delta_angle_y = 0.0;
delta_angle_z = 0.0;
gyro_rate_x = 0.0;
gyro_rate_y = 0.0;
gyro_rate_z = 0.0;
@@ -933,111 +1003,245 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
}
/**
* @return Yaw axis angle in degrees (CCW positive)
* 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.
*/
public synchronized double getAngle() {
switch (m_yaw_axis) {
public void reset() {
synchronized (this) {
m_integ_angle_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
}
}
/**
* Allow the designated gyro angle to be set to a given value. This may happen with unread values
* in the buffer, it is suggested that the IMU is not moving when this method is run.
*
* @param axis IMUAxis that will be changed
* @param angle A double in degrees (CCW positive)
*/
public void setGyroAngle(IMUAxis axis, double angle) {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
}
switch (axis) {
case kX:
this.setGyroAngleX(angle);
break;
case kY:
this.setGyroAngleY(angle);
break;
case kZ:
this.setGyroAngleZ(angle);
break;
default:
}
}
/**
* Allow the gyro angle X to be set to a given value. This may happen with unread values in the
* buffer, it is suggested that the IMU is not moving when this method is run.
*
* @param angle A double in degrees (CCW positive)
*/
public void setGyroAngleX(double angle) {
synchronized (this) {
m_integ_angle_x = angle;
}
}
/**
* Allow the gyro angle Y to be set to a given value. This may happen with unread values in the
* buffer, it is suggested that the IMU is not moving when this method is run.
*
* @param angle A double in degrees (CCW positive)
*/
public void setGyroAngleY(double angle) {
synchronized (this) {
m_integ_angle_y = angle;
}
}
/**
* Allow the gyro angle Z to be set to a given value. This may happen with unread values in the
* buffer, it is suggested that the IMU is not moving when this method is run.
*
* @param angle A double in degrees (CCW positive)
*/
public void setGyroAngleZ(double angle) {
synchronized (this) {
m_integ_angle_z = angle;
}
}
/**
* @param axis The IMUAxis whose angle to return
* @return The axis angle in degrees (CCW positive)
*/
public synchronized double getAngle(IMUAxis axis) {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
}
switch (axis) {
case kX:
if (m_simGyroAngleX != null) {
return m_simGyroAngleX.get();
}
break;
return m_integ_angle_x;
case kY:
if (m_simGyroAngleY != null) {
return m_simGyroAngleY.get();
}
break;
return m_integ_angle_y;
case kZ:
if (m_simGyroAngleZ != null) {
return m_simGyroAngleZ.get();
}
break;
return m_integ_angle_z;
default:
}
return m_integ_angle;
return 0.0;
}
/**
* @return Yaw axis angular rate in degrees per second (CCW positive)
* @param axis The IMUAxis whose rate to return
* @return Axis angular rate in degrees per second (CCW positive)
*/
public synchronized double getRate() {
if (m_yaw_axis == IMUAxis.kX) {
if (m_simGyroRateX != null) {
return m_simGyroRateX.get();
}
return m_gyro_rate_x;
} else if (m_yaw_axis == IMUAxis.kY) {
if (m_simGyroRateY != null) {
return m_simGyroRateY.get();
}
return m_gyro_rate_y;
} else if (m_yaw_axis == IMUAxis.kZ) {
if (m_simGyroRateZ != null) {
return m_simGyroRateZ.get();
}
return m_gyro_rate_z;
} else {
return 0.0;
public synchronized double getRate(IMUAxis axis) {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
}
switch (axis) {
case kX:
if (m_simGyroRateX != null) {
return m_simGyroRateX.get();
}
return m_gyro_rate_x;
case kY:
if (m_simGyroRateY != null) {
return m_simGyroRateY.get();
}
return m_gyro_rate_y;
case kZ:
if (m_simGyroRateZ != null) {
return m_simGyroRateZ.get();
}
return m_gyro_rate_z;
default:
}
return 0.0;
}
/**
* @return Yaw Axis
* Returns which axis, kX, kY, or kZ, is set to the yaw axis.
*
* @return IMUAxis Yaw Axis
*/
public IMUAxis getYawAxis() {
return m_yaw_axis;
}
/**
* @return current acceleration in the X axis
* Returns which axis, kX, kY, or kZ, is set to the pitch axis.
*
* @return IMUAxis Pitch Axis
*/
public IMUAxis getPitchAxis() {
return m_pitch_axis;
}
/**
* Returns which axis, kX, kY, or kZ, is set to the roll axis.
*
* @return IMUAxis Roll Axis
*/
public IMUAxis getRollAxis() {
return m_roll_axis;
}
/**
* @return The acceleration in the X axis
*/
public synchronized double getAccelX() {
return m_accel_x * 9.81;
}
/**
* @return current acceleration in the Y axis
* @return The acceleration in the Y axis
*/
public synchronized double getAccelY() {
return m_accel_y * 9.81;
}
/**
* @return current acceleration in the Z axis
* @return The acceleration in the Z axis
*/
public synchronized double getAccelZ() {
return m_accel_z * 9.81;
}
/**
* @return X-axis complementary angle
* @return The X-axis complementary angle
*/
public synchronized double getXComplementaryAngle() {
return m_compAngleX;
}
/**
* @return Y-axis complementary angle
* @return The Y-axis complementary angle
*/
public synchronized double getYComplementaryAngle() {
return m_compAngleY;
}
/**
* @return X-axis filtered acceleration angle
* @return The X-axis filtered acceleration angle
*/
public synchronized double getXFilteredAccelAngle() {
return m_accelAngleX;
}
/**
* @return Y-axis filtered acceleration angle
* @return The Y-axis filtered acceleration angle
*/
public synchronized double getYFilteredAccelAngle() {
return m_accelAngleY;
}
/**
* Get the SPI port number.
* Gets the SPI port number.
*
* @return The SPI port number.
*/
@@ -1048,6 +1252,6 @@ public class ADIS16470_IMU implements AutoCloseable, Sendable {
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
builder.addDoubleProperty("Value", () -> getAngle(m_yaw_axis), null);
}
}