Skip to content

Commit

Permalink
[wpilib] ADIS16470: Add access to all 3 axes (#6074)
Browse files Browse the repository at this point in the history
  • Loading branch information
@BR88C
BR88C authored Dec 26, 2023
1 parent 795d4be commit ab78b93
Show file tree
Hide file tree
Showing 3 changed files with 621 additions and 253 deletions.
253 changes: 184 additions & 69 deletions wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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 {
Expand All @@ -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);
}
Loading

0 comments on commit ab78b93

Please sign in to comment.