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

[wpilib] 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;