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

[wpilib] Make ADIS IMU classes unit-safe (#3860)

Browse Source 
The gyro rate getters were removed since that data isn't available.
This commit is contained in:
Tyler Veness
2022-01-03 20:00:53 -08:00
committed by GitHub
parent 947f589916
commit a2510aaa0e
12 changed files with 381 additions and 666 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -66,10 +66,11 @@ inline void ADISReportError(int32_t status, const char* file, int line,
#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()
: ADIS16448_IMU(kZ, SPI::Port::kMXP, CalibrationTime::_512ms) {}
ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
uint16_t cal_time)
CalibrationTime cal_time)
: m_yaw_axis(yaw_axis),
m_spi_port(port),
m_simDevice("Gyro:ADIS16448", port) {
@@ -80,12 +81,6 @@ ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
m_simGyroAngleZ =
m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
m_simGyroRateX =
m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
m_simGyroRateY =
m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
m_simGyroRateZ =
m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
m_simAccelX =
m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
m_simAccelY =
@@ -311,12 +306,12 @@ bool ADIS16448_IMU::SwitchToAutoSPI() {
/**
*
**/
int ADIS16448_IMU::ConfigCalTime(int new_cal_time) {
int ADIS16448_IMU::ConfigCalTime(CalibrationTime 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;
m_calibration_time = new_cal_time;
m_avg_size = static_cast<uint16_t>(m_calibration_time) * 819;
InitOffsetBuffer(m_avg_size);
return 0;
}
@@ -348,16 +343,6 @@ void ADIS16448_IMU::Calibrate() {
// 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
@@ -744,12 +729,7 @@ int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
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 {
units::degree_t ADIS16448_IMU::GetAngle() const {
switch (m_yaw_axis) {
case kX:
return GetGyroAngleX();
@@ -758,142 +738,115 @@ double ADIS16448_IMU::GetAngle() const {
case kZ:
return GetGyroAngleZ();
default:
return 0.0;
return 0_deg;
}
}
double ADIS16448_IMU::GetRate() const {
switch (m_yaw_axis) {
case kX:
return GetGyroRateX();
case kY:
return GetGyroRateY();
case kZ:
return GetGyroRateZ();
default:
return 0.0;
units::degree_t ADIS16448_IMU::GetGyroAngleX() const {
if (m_simGyroAngleX) {
return units::degree_t{m_simGyroAngleX.Get()};
}
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_gyro_x};
}
units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
if (m_simGyroAngleY) {
return units::degree_t{m_simGyroAngleY.Get()};
}
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_gyro_y};
}
units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
if (m_simGyroAngleZ) {
return units::degree_t{m_simGyroAngleZ.Get()};
}
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_gyro_z};
}
units::meters_per_second_squared_t ADIS16448_IMU::GetAccelX() const {
if (m_simAccelX) {
return units::meters_per_second_squared_t{m_simAccelX.Get()};
}
std::scoped_lock sync(m_mutex);
return m_accel_x * 9.81_mps_sq;
}
units::meters_per_second_squared_t ADIS16448_IMU::GetAccelY() const {
if (m_simAccelY) {
return units::meters_per_second_squared_t{m_simAccelY.Get()};
}
std::scoped_lock sync(m_mutex);
return m_accel_y * 9.81_mps_sq;
}
units::meters_per_second_squared_t ADIS16448_IMU::GetAccelZ() const {
if (m_simAccelZ) {
return units::meters_per_second_squared_t{m_simAccelZ.Get()};
}
std::scoped_lock sync(m_mutex);
return m_accel_z * 9.81_mps_sq;
}
units::tesla_t ADIS16448_IMU::GetMagneticFieldX() const {
std::scoped_lock sync(m_mutex);
return units::gauss_t{m_mag_x * 1e-3};
}
units::tesla_t ADIS16448_IMU::GetMagneticFieldY() const {
std::scoped_lock sync(m_mutex);
return units::gauss_t{m_mag_y * 1e-3};
}
units::tesla_t ADIS16448_IMU::GetMagneticFieldZ() const {
std::scoped_lock sync(m_mutex);
return units::gauss_t{m_mag_z * 1e-3};
}
units::degree_t ADIS16448_IMU::GetXComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_compAngleX};
}
units::degree_t ADIS16448_IMU::GetYComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_compAngleY};
}
units::degree_t ADIS16448_IMU::GetXFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_accelAngleX};
}
units::degree_t ADIS16448_IMU::GetYFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_accelAngleY};
}
units::pounds_per_square_inch_t ADIS16448_IMU::GetBarometricPressure() const {
std::scoped_lock sync(m_mutex);
return units::mbar_t{m_baro};
}
units::celsius_t ADIS16448_IMU::GetTemperature() const {
std::scoped_lock sync(m_mutex);
return units::celsius_t{m_temp};
}
ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
return m_yaw_axis;
}
double ADIS16448_IMU::GetGyroAngleX() const {
if (m_simGyroAngleX) {
return m_simGyroAngleX.Get();
int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
if (m_yaw_axis == yaw_axis) {
return 1;
} else {
m_yaw_axis = yaw_axis;
Reset();
return 0;
}
std::scoped_lock sync(m_mutex);
return m_integ_gyro_x;
}
double ADIS16448_IMU::GetGyroAngleY() const {
if (m_simGyroAngleY) {
return m_simGyroAngleY.Get();
}
std::scoped_lock sync(m_mutex);
return m_integ_gyro_y;
}
double ADIS16448_IMU::GetGyroAngleZ() const {
if (m_simGyroAngleZ) {
return m_simGyroAngleZ.Get();
}
std::scoped_lock sync(m_mutex);
return m_integ_gyro_z;
}
double ADIS16448_IMU::GetGyroRateX() const {
if (m_simGyroRateX) {
return m_simGyroRateX.Get();
}
std::scoped_lock sync(m_mutex);
return m_gyro_x;
}
double ADIS16448_IMU::GetGyroRateY() const {
if (m_simGyroRateY) {
return m_simGyroRateY.Get();
}
std::scoped_lock sync(m_mutex);
return m_gyro_y;
}
double ADIS16448_IMU::GetGyroRateZ() const {
if (m_simGyroRateZ) {
return m_simGyroRateZ.Get();
}
std::scoped_lock sync(m_mutex);
return m_gyro_z;
}
double ADIS16448_IMU::GetAccelX() const {
if (m_simAccelX) {
return m_simAccelX.Get();
}
std::scoped_lock sync(m_mutex);
return m_accel_x;
}
double ADIS16448_IMU::GetAccelY() const {
if (m_simAccelY) {
return m_simAccelY.Get();
}
std::scoped_lock sync(m_mutex);
return m_accel_y;
}
double ADIS16448_IMU::GetAccelZ() const {
if (m_simAccelZ) {
return m_simAccelZ.Get();
}
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;
}
int ADIS16448_IMU::GetPort() const {
@@ -909,6 +862,7 @@ int ADIS16448_IMU::GetPort() const {
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()); });
builder.SetUpdateTable([=]() {
nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
});
}

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

@@ -79,12 +79,6 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
m_simGyroAngleZ =
m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
m_simGyroRateX =
m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
m_simGyroRateY =
m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
m_simGyroRateZ =
m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
m_simAccelX =
m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
m_simAccelY =
@@ -385,22 +379,6 @@ void ADIS16470_IMU::Calibrate() {
}
}
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.
*
@@ -733,112 +711,90 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
*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 {
units::degree_t ADIS16470_IMU::GetAngle() const {
switch (m_yaw_axis) {
case kX:
if (m_simGyroAngleX) {
return m_simGyroAngleX.Get();
return units::degree_t{m_simGyroAngleX.Get()};
}
break;
case kY:
if (m_simGyroAngleY) {
return m_simGyroAngleY.Get();
return units::degree_t{m_simGyroAngleY.Get()};
}
break;
case kZ:
if (m_simGyroAngleZ) {
return m_simGyroAngleZ.Get();
return units::degree_t{m_simGyroAngleZ.Get()};
}
break;
}
std::scoped_lock sync(m_mutex);
return m_integ_angle;
return units::degree_t{m_integ_angle};
}
double ADIS16470_IMU::GetRate() const {
std::scoped_lock sync(m_mutex);
switch (m_yaw_axis) {
case kX:
return GetGyroRateX();
case kY:
return GetGyroRateY();
case kZ:
return GetGyroRateZ();
default:
return 0.0;
units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
if (m_simAccelX) {
return units::meters_per_second_squared_t{m_simAccelX.Get()};
}
std::scoped_lock sync(m_mutex);
return units::meters_per_second_squared_t{m_accel_x};
}
units::meters_per_second_squared_t ADIS16470_IMU::GetAccelY() const {
if (m_simAccelY) {
return units::meters_per_second_squared_t{m_simAccelY.Get()};
}
std::scoped_lock sync(m_mutex);
return units::meters_per_second_squared_t{m_accel_y};
}
units::meters_per_second_squared_t ADIS16470_IMU::GetAccelZ() const {
if (m_simAccelZ) {
return units::meters_per_second_squared_t{m_simAccelZ.Get()};
}
std::scoped_lock sync(m_mutex);
return units::meters_per_second_squared_t{m_accel_z};
}
units::degree_t ADIS16470_IMU::GetXComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_compAngleX};
}
units::degree_t ADIS16470_IMU::GetYComplementaryAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_compAngleY};
}
units::degree_t ADIS16470_IMU::GetXFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_accelAngleX};
}
units::degree_t ADIS16470_IMU::GetYFilteredAccelAngle() const {
std::scoped_lock sync(m_mutex);
return units::degree_t{m_accelAngleY};
}
ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
return m_yaw_axis;
}
double ADIS16470_IMU::GetGyroRateX() const {
if (m_simGyroRateX) {
return m_simGyroRateX.Get();
int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
if (m_yaw_axis == yaw_axis) {
return 1;
}
std::scoped_lock sync(m_mutex);
return m_gyro_x;
}
double ADIS16470_IMU::GetGyroRateY() const {
if (m_simGyroRateY) {
return m_simGyroRateY.Get();
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
std::scoped_lock sync(m_mutex);
return m_gyro_y;
}
double ADIS16470_IMU::GetGyroRateZ() const {
if (m_simGyroRateZ) {
return m_simGyroRateZ.Get();
m_yaw_axis = yaw_axis;
if (!SwitchToAutoSPI()) {
REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
return 2;
}
std::scoped_lock sync(m_mutex);
return m_gyro_z;
}
double ADIS16470_IMU::GetAccelX() const {
if (m_simAccelX) {
return m_simAccelX.Get();
}
std::scoped_lock sync(m_mutex);
return m_accel_x;
}
double ADIS16470_IMU::GetAccelY() const {
if (m_simAccelY) {
return m_simAccelY.Get();
}
std::scoped_lock sync(m_mutex);
return m_accel_y;
}
double ADIS16470_IMU::GetAccelZ() const {
if (m_simAccelZ) {
return m_simAccelZ.Get();
}
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;
return 0;
}
int ADIS16470_IMU::GetPort() const {
@@ -854,6 +810,7 @@ int ADIS16470_IMU::GetPort() const {
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()); });
builder.SetUpdateTable([=]() {
nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
});
}

21
wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
View File

@@ -14,9 +14,6 @@ ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) {
m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
m_simAccelX = deviceSim.GetDouble("accel_x");
m_simAccelY = deviceSim.GetDouble("accel_y");
m_simAccelZ = deviceSim.GetDouble("accel_z");
@@ -34,26 +31,14 @@ void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) {
m_simGyroAngleZ.Set(angle.value());
}
void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) {
m_simGyroRateX.Set(rate.value());
}
void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) {
m_simGyroRateY.Set(rate.value());
}
void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) {
m_simGyroRateZ.Set(rate.value());
}
void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
m_simAccelX.Set(accel.value() / 9.81);
m_simAccelX.Set(accel.value());
}
void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
m_simAccelY.Set(accel.value() / 9.81);
m_simAccelY.Set(accel.value());
}
void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
m_simAccelZ.Set(accel.value() / 9.81);
m_simAccelZ.Set(accel.value());
}

21
wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
View File

@@ -14,9 +14,6 @@ ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) {
m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
m_simAccelX = deviceSim.GetDouble("accel_x");
m_simAccelY = deviceSim.GetDouble("accel_y");
m_simAccelZ = deviceSim.GetDouble("accel_z");
@@ -34,26 +31,14 @@ void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) {
m_simGyroAngleZ.Set(angle.value());
}
void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) {
m_simGyroRateX.Set(rate.value());
}
void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) {
m_simGyroRateY.Set(rate.value());
}
void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) {
m_simGyroRateZ.Set(rate.value());
}
void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
m_simAccelX.Set(accel.value() / 9.81);
m_simAccelX.Set(accel.value());
}
void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
m_simAccelY.Set(accel.value() / 9.81);
m_simAccelY.Set(accel.value());
}
void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
m_simAccelZ.Set(accel.value() / 9.81);
m_simAccelZ.Set(accel.value());
}

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

@@ -13,10 +13,6 @@
#pragma once
#include <frc/DigitalInput.h>
#include <frc/DigitalOutput.h>
#include <frc/DigitalSource.h>
#include <frc/SPI.h>
#include <stdint.h>
#include <atomic>
@@ -25,10 +21,21 @@
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/magnetic_field_strength.h>
#include <units/pressure.h>
#include <units/temperature.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/DigitalInput.h"
#include "frc/DigitalOutput.h"
#include "frc/DigitalSource.h"
#include "frc/SPI.h"
namespace frc {
/**
* Use DMA SPI to read rate, acceleration, and magnetometer data from the
@@ -49,6 +56,22 @@ namespace frc {
class ADIS16448_IMU : public nt::NTSendable,
public wpi::SendableHelper<ADIS16448_IMU> {
public:
/* ADIS16448 Calibration Time Enum Class */
enum class CalibrationTime {
_32ms = 0,
_64ms = 1,
_128ms = 2,
_256ms = 3,
_512ms = 4,
_1s = 5,
_2s = 6,
_4s = 7,
_8s = 8,
_16s = 9,
_32s = 10,
_64s = 11
};
enum IMUAxis { kX, kY, kZ };
/**
@@ -65,7 +88,8 @@ class ADIS16448_IMU : public nt::NTSendable,
* @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);
explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
CalibrationTime cal_time);
~ADIS16448_IMU() override;
@@ -93,7 +117,7 @@ class ADIS16448_IMU : public nt::NTSendable,
*
* @param new_cal_time The calibration time that should be used
*/
int ConfigCalTime(int new_cal_time);
int ConfigCalTime(CalibrationTime new_cal_time);
/**
* Reset the gyro.
@@ -105,64 +129,57 @@ class ADIS16448_IMU : public nt::NTSendable,
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.
* Returns the yaw axis angle in degrees (CCW positive).
*/
double GetAngle() const;
units::degree_t 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
* Returns the accumulated gyro angle in the X axis.
*/
double GetRate() const;
units::degree_t GetGyroAngleX() const;
double GetGyroAngleX() const;
/**
* Returns the accumulated gyro angle in the Y axis.
*/
units::degree_t GetGyroAngleY() const;
double GetGyroAngleY() const;
/**
* Returns the accumulated gyro angle in the Z axis.
*/
units::degree_t GetGyroAngleZ() const;
double GetGyroAngleZ() const;
/**
* Returns the acceleration in the X axis.
*/
units::meters_per_second_squared_t GetAccelX() const;
double GetGyroRateX() const;
/**
* Returns the acceleration in the Y axis.
*/
units::meters_per_second_squared_t GetAccelY() const;
double GetGyroRateY() const;
/**
* Returns the acceleration in the Z axis.
*/
units::meters_per_second_squared_t GetAccelZ() const;
double GetGyroRateZ() const;
units::degree_t GetXComplementaryAngle() const;
double GetAccelX() const;
units::degree_t GetYComplementaryAngle() const;
double GetAccelY() const;
units::degree_t GetXFilteredAccelAngle() const;
double GetAccelZ() const;
units::degree_t GetYFilteredAccelAngle() const;
double GetXComplementaryAngle() const;
units::tesla_t GetMagneticFieldX() const;
double GetYComplementaryAngle() const;
units::tesla_t GetMagneticFieldY() const;
double GetXFilteredAccelAngle() const;
units::tesla_t GetMagneticFieldZ() const;
double GetYFilteredAccelAngle() const;
units::pounds_per_square_inch_t GetBarometricPressure() const;
double GetMagInstantX() const;
double GetMagInstantY() const;
double GetMagInstantZ() const;
double GetBarometricPressure() const;
double GetTemperature() const;
units::celsius_t GetTemperature() const;
IMUAxis GetYawAxis() const;
@@ -317,7 +334,7 @@ class ADIS16448_IMU : public nt::NTSendable,
bool m_auto_configured = false;
SPI::Port m_spi_port;
uint16_t m_calibration_time;
CalibrationTime m_calibration_time;
SPI* m_spi = nullptr;
DigitalInput* m_auto_interrupt = nullptr;
@@ -327,9 +344,6 @@ class ADIS16448_IMU : public nt::NTSendable,
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;

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

@@ -13,10 +13,6 @@
#pragma once
#include <frc/DigitalInput.h>
#include <frc/DigitalOutput.h>
#include <frc/DigitalSource.h>
#include <frc/SPI.h>
#include <stdint.h>
#include <atomic>
@@ -25,10 +21,18 @@
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/DigitalInput.h"
#include "frc/DigitalOutput.h"
#include "frc/DigitalSource.h"
#include "frc/SPI.h"
namespace frc {
/**
* Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
@@ -119,41 +123,32 @@ class ADIS16470_IMU : public nt::NTSendable,
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.
* Returns the yaw axis angle in degrees (CCW positive).
*/
double GetAngle() const;
units::degree_t GetAngle() const;
double GetRate() const;
/**
* Returns the acceleration in the X axis.
*/
units::meters_per_second_squared_t GetAccelX() const;
double GetGyroRateX() const;
/**
* Returns the acceleration in the Y axis.
*/
units::meters_per_second_squared_t GetAccelY() const;
double GetGyroRateY() const;
/**
* Returns the acceleration in the Z axis.
*/
units::meters_per_second_squared_t GetAccelZ() const;
double GetGyroRateZ() const;
units::degree_t GetXComplementaryAngle() const;
double GetAccelX() const;
units::degree_t GetYComplementaryAngle() const;
double GetAccelY() const;
units::degree_t GetXFilteredAccelAngle() const;
double GetAccelZ() const;
double GetXComplementaryAngle() const;
double GetYComplementaryAngle() const;
double GetXFilteredAccelAngle() const;
double GetYFilteredAccelAngle() const;
units::degree_t GetYFilteredAccelAngle() const;
IMUAxis GetYawAxis() const;
@@ -380,9 +375,6 @@ class ADIS16470_IMU : public nt::NTSendable,
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;

24
wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
View File

@@ -48,27 +48,6 @@ class ADIS16448_IMUSim {
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t rate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t rate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t rate);
/**
* Sets the X axis acceleration.
*
@@ -94,9 +73,6 @@ class ADIS16448_IMUSim {
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;

24
wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
View File

@@ -48,27 +48,6 @@ class ADIS16470_IMUSim {
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t rate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t rate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t rate);
/**
* Sets the X axis acceleration.
*
@@ -94,9 +73,6 @@ class ADIS16470_IMUSim {
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;

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

@@ -19,7 +19,6 @@ import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.wpilibj.interfaces.Gyro;
// CHECKSTYLE.OFF: TypeName
// CHECKSTYLE.OFF: MemberName
@@ -50,7 +49,7 @@ import edu.wpi.first.wpilibj.interfaces.Gyro;
"PMD.EmptyIfStmt",
"PMD.EmptyStatementNotInLoop"
})
public class ADIS16448_IMU implements Gyro, NTSendable {
public class ADIS16448_IMU implements AutoCloseable, NTSendable {
/** ADIS16448 Register Map Declaration */
private static final int FLASH_CNT = 0x00; // Flash memory write count
@@ -94,6 +93,27 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
private static final int PROD_ID = 0x56; // Product identifier
private static final int SERIAL_NUM = 0x58; // Lot-specific serial number
public enum CalibrationTime {
_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 CalibrationTime(int value) {
this.value = value;
}
}
public enum IMUAxis {
kX,
kY,
@@ -149,7 +169,7 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
/* State variables */
private volatile boolean m_thread_active = false;
private int m_calibration_time = 0;
private CalibrationTime m_calibration_time = CalibrationTime._512ms;
private volatile boolean m_first_run = true;
private volatile boolean m_thread_idle = false;
private boolean m_auto_configured = false;
@@ -168,9 +188,6 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
private SimDouble m_simGyroAngleX;
private SimDouble m_simGyroAngleY;
private SimDouble m_simGyroAngleZ;
private SimDouble m_simGyroRateX;
private SimDouble m_simGyroRateY;
private SimDouble m_simGyroRateZ;
private SimDouble m_simAccelX;
private SimDouble m_simAccelY;
private SimDouble m_simAccelZ;
@@ -226,7 +243,7 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
}
public ADIS16448_IMU() {
this(IMUAxis.kZ, SPI.Port.kMXP, 4);
this(IMUAxis.kZ, SPI.Port.kMXP, CalibrationTime._512ms);
}
/**
@@ -234,7 +251,7 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
* @param port The SPI Port the gyro is plugged into
* @param cal_time Calibration time
*/
public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, int cal_time) {
public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
m_yaw_axis = yaw_axis;
m_spi_port = port;
@@ -245,9 +262,6 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0);
m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0);
m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0);
m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
@@ -287,7 +301,7 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
"ADIS16448 IMU Detected. Starting initial calibration delay.", false);
// Wait for whatever time the user set as the start-up delay
try {
Thread.sleep((long) (m_calibration_time * 1.2 * 1000));
Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000));
} catch (InterruptedException e) {
}
// Execute calibration routine
@@ -498,12 +512,12 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
* @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(int new_cal_time) {
public int configCalTime(CalibrationTime new_cal_time) {
if (m_calibration_time == new_cal_time) {
return 1;
} else {
m_calibration_time = new_cal_time;
m_avg_size = m_calibration_time * 819;
m_avg_size = m_calibration_time.value * 819;
initOffsetBuffer(m_avg_size);
return 0;
}
@@ -526,8 +540,11 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
}
}
/** {@inheritDoc} */
@Override
/**
* Calibrate the gyro. It's important to make sure that the robot is not moving while the
* calibration is in progress, this is typically done when the robot is first turned on while it's
* sitting at rest before the match starts.
*/
public void calibrate() {
synchronized (this) {
int gyroAverageSize = Math.min(m_accum_count, m_avg_size);
@@ -919,7 +936,7 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
return compAngle;
}
/** */
/** @return Yaw axis angle in degrees (CCW positive) */
public synchronized double getAngle() {
switch (m_yaw_axis) {
case kX:
@@ -933,26 +950,12 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
}
}
/** */
public synchronized double getRate() {
switch (m_yaw_axis) {
case kX:
return getGyroRateX();
case kY:
return getGyroRateY();
case kZ:
return getGyroRateZ();
default:
return 0.0;
}
}
/** @return Yaw Axis */
public IMUAxis getYawAxis() {
return m_yaw_axis;
}
/** @return accumulated gyro angle in the X axis */
/** @return accumulated gyro angle in the X axis in degrees */
public synchronized double getGyroAngleX() {
if (m_simGyroAngleX != null) {
return m_simGyroAngleX.get();
@@ -960,7 +963,7 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
return m_integ_gyro_x;
}
/** @return accumulated gyro angle in the Y axis */
/** @return accumulated gyro angle in the Y axis in degrees */
public synchronized double getGyroAngleY() {
if (m_simGyroAngleY != null) {
return m_simGyroAngleY.get();
@@ -968,7 +971,7 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
return m_integ_gyro_y;
}
/** @return accumulated gyro angle in the Z axis */
/** @return accumulated gyro angle in the Z axis in degrees */
public synchronized double getGyroAngleZ() {
if (m_simGyroAngleZ != null) {
return m_simGyroAngleZ.get();
@@ -976,95 +979,75 @@ public class ADIS16448_IMU implements Gyro, NTSendable {
return m_integ_gyro_z;
}
/** @return current gyro angle in the X axis */
public synchronized double getGyroRateX() {
if (m_simGyroRateX != null) {
return m_simGyroRateX.get();
}
return m_gyro_x;
}
/** @return current gyro angle in the Y axis */
public synchronized double getGyroRateY() {
if (m_simGyroRateY != null) {
return m_simGyroRateY.get();
}
return m_gyro_y;
}
/** @return current gyro angle in the Z axis */
public synchronized double getGyroRateZ() {
if (m_simGyroRateZ != null) {
return m_simGyroRateZ.get();
}
return m_gyro_z;
}
/** @return urrent acceleration in the X axis */
/** @return urrent acceleration in the X axis in meters per second squared */
public synchronized double getAccelX() {
if (m_simAccelX != null) {
return m_simAccelX.get();
}
return m_accel_x;
return m_accel_x * 9.81;
}
/** @return current acceleration in the Y axis */
/** @return current acceleration in the Y axis in meters per second squared */
public synchronized double getAccelY() {
if (m_simAccelY != null) {
return m_simAccelY.get();
}
return m_accel_y;
return m_accel_y * 9.81;
}
/** @return current acceleration in the Z axis */
/** @return current acceleration in the Z axis in meters per second squared */
public synchronized double getAccelZ() {
if (m_simAccelZ != null) {
return m_simAccelZ.get();
}
return m_accel_z;
return m_accel_z * 9.81;
}
/** @return Mag instant X */
public synchronized double getMagInstantX() {
return m_mag_x;
/** @return Magnetic field strength in the X axis in Tesla */
public synchronized double getMagneticFieldX() {
// mG to T
return m_mag_x * 1e-7;
}
/** @return Mag instant Y */
public synchronized double getMagInstantY() {
return m_mag_y;
/** @return Magnetic field strength in the Y axis in Tesla */
public synchronized double getMagneticFieldY() {
// mG to T
return m_mag_y * 1e-7;
}
/** @return Mag instant Z */
public synchronized double getMagInstantZ() {
return m_mag_z;
/** @return Magnetic field strength in the Z axis in Tesla */
public synchronized double getMagneticFieldZ() {
// mG to T
return m_mag_z * 1e-7;
}
/** @return X axis complementary angle */
/** @return X axis complementary angle in degrees */
public synchronized double getXComplementaryAngle() {
return m_compAngleX;
}
/** @return Y axis complementary angle */
/** @return Y axis complementary angle in degrees */
public synchronized double getYComplementaryAngle() {
return m_compAngleY;
}
/** @return X axis filtered acceleration angle */
/** @return X axis filtered acceleration angle in degrees */
public synchronized double getXFilteredAccelAngle() {
return m_accelAngleX;
}
/** @return Y axis filtered acceleration angle */
/** @return Y axis filtered acceleration angle in degrees */
public synchronized double getYFilteredAccelAngle() {
return m_accelAngleY;
}
/** @return Barometric Pressure */
/** @return Barometric Pressure in PSI */
public synchronized double getBarometricPressure() {
return m_baro;
// mbar to PSI
return m_baro * 0.0145;
}
/** @return Temperature */
/** @return Temperature in degrees Celsius */
public synchronized double getTemperature() {
return m_temp;
}

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

@@ -18,7 +18,6 @@ import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
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;
@@ -51,7 +50,7 @@ import java.nio.ByteOrder;
"PMD.EmptyIfStmt",
"PMD.EmptyStatementNotInLoop"
})
public class ADIS16470_IMU implements Gyro, NTSendable {
public class ADIS16470_IMU implements AutoCloseable, 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
@@ -179,13 +178,7 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
FLASH_CNT
};
public enum IMUAxis {
kX,
kY,
kZ
}
public enum ADIS16470CalibrationTime {
public enum CalibrationTime {
_32ms(0),
_64ms(1),
_128ms(2),
@@ -201,11 +194,17 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
private int value;
private ADIS16470CalibrationTime(int value) {
private CalibrationTime(int value) {
this.value = value;
}
}
public enum IMUAxis {
kX,
kY,
kZ
}
// 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;
@@ -256,9 +255,6 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
private SimDouble m_simGyroAngleX;
private SimDouble m_simGyroAngleY;
private SimDouble m_simGyroAngleZ;
private SimDouble m_simGyroRateX;
private SimDouble m_simGyroRateY;
private SimDouble m_simGyroRateZ;
private SimDouble m_simAccelX;
private SimDouble m_simAccelY;
private SimDouble m_simAccelZ;
@@ -277,7 +273,7 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
}
public ADIS16470_IMU() {
this(IMUAxis.kZ, SPI.Port.kOnboardCS0, ADIS16470CalibrationTime._4s);
this(IMUAxis.kZ, SPI.Port.kOnboardCS0, CalibrationTime._4s);
}
/**
@@ -285,7 +281,7 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
* @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) {
public ADIS16470_IMU(IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) {
m_yaw_axis = yaw_axis;
m_calibration_time = cal_time.value;
m_spi_port = port;
@@ -297,9 +293,6 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0);
m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0);
m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0);
m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0);
m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0);
m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0);
m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0);
m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0);
m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0);
@@ -533,7 +526,7 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
* @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) {
public int configCalTime(CalibrationTime new_cal_time) {
if (m_calibration_time == new_cal_time.value) {
return 1;
}
@@ -570,8 +563,11 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
return 0;
}
/** {@inheritDoc} */
@Override
/**
* Calibrate the gyro. It's important to make sure that the robot is not moving while the
* calibration is in progress, this is typically done when the robot is first turned on while it's
* sitting at rest before the match starts.
*/
public void calibrate() {
if (!switchToStandardSPI()) {
DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false);
@@ -916,7 +912,7 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
return compAngle;
}
/** {@inheritDoc} */
/** @return Yaw axis angle in degrees (CCW positive) */
public synchronized double getAngle() {
switch (m_yaw_axis) {
case kX:
@@ -938,61 +934,24 @@ public class ADIS16470_IMU implements Gyro, NTSendable {
return m_integ_angle;
}
/** {@inheritDoc} */
public synchronized double getRate() {
switch (m_yaw_axis) {
case kX:
return getGyroRateX();
case kY:
return getGyroRateY();
case kZ:
return getGyroRateZ();
}
return 0.0;
}
/** @return Yaw Axis */
public IMUAxis getYawAxis() {
return m_yaw_axis;
}
/** @return current gyro angle in the X direction */
public synchronized double getGyroRateX() {
if (m_simGyroRateX != null) {
return m_simGyroRateX.get();
}
return m_gyro_x;
}
/** @return current gyro angle in the Y axis */
public synchronized double getGyroRateY() {
if (m_simGyroRateY != null) {
return m_simGyroRateY.get();
}
return m_gyro_y;
}
/** @return current gyro angle in the Z axis */
public synchronized double getGyroRateZ() {
if (m_simGyroRateZ != null) {
return m_simGyroRateZ.get();
}
return m_gyro_z;
}
/** @return current acceleration in the X axis */
public synchronized double getAccelInstantX() {
return m_accel_x;
public synchronized double getAccelX() {
return m_accel_x * 9.81;
}
/** @return current acceleration in the Y axis */
public synchronized double getAccelInstantY() {
return m_accel_y;
public synchronized double getAccelY() {
return m_accel_y * 9.81;
}
/** @return current acceleration in the Z axis */
public synchronized double getAccelInstantZ() {
return m_accel_z;
public synchronized double getAccelZ() {
return m_accel_z * 9.81;
}
/** @return X axis complementary angle */

63
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java
View File

@@ -13,9 +13,6 @@ public class ADIS16448_IMUSim {
private final SimDouble m_simGyroAngleX;
private final SimDouble m_simGyroAngleY;
private final SimDouble m_simGyroAngleZ;
private final SimDouble m_simGyroRateX;
private final SimDouble m_simGyroRateY;
private final SimDouble m_simGyroRateZ;
private final SimDouble m_simAccelX;
private final SimDouble m_simAccelY;
private final SimDouble m_simAccelZ;
@@ -30,16 +27,13 @@ public class ADIS16448_IMUSim {
m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
m_simAccelX = wrappedSimDevice.getDouble("accel_x");
m_simAccelY = wrappedSimDevice.getDouble("accel_y");
m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
}
/**
* Sets the X axis angle (CCW positive).
* Sets the X axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
@@ -48,7 +42,7 @@ public class ADIS16448_IMUSim {
}
/**
* Sets the Y axis angle (CCW positive).
* Sets the Y axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
@@ -57,7 +51,7 @@ public class ADIS16448_IMUSim {
}
/**
* Sets the Z axis angle (CCW positive).
* Sets the Z axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
@@ -66,56 +60,29 @@ public class ADIS16448_IMUSim {
}
/**
* Sets the X axis angular rate (CCW positive).
* Sets the X axis acceleration in meters per second squared.
*
* @param rateDegreesPerSecond The angular rate.
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setGyroRateX(double rateDegreesPerSecond) {
m_simGyroRateX.set(rateDegreesPerSecond);
public void setAccelX(double accelMetersPerSecondSquared) {
m_simAccelX.set(accelMetersPerSecondSquared);
}
/**
* Sets the Y axis angular rate (CCW positive).
* Sets the Y axis acceleration in meters per second squared.
*
* @param rateDegreesPerSecond The angular rate.
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setGyroRateY(double rateDegreesPerSecond) {
m_simGyroRateY.set(rateDegreesPerSecond);
public void setAccelY(double accelMetersPerSecondSquared) {
m_simAccelY.set(accelMetersPerSecondSquared);
}
/**
* Sets the Z axis angular rate (CCW positive).
* Sets the Z axis acceleration in meters per second squared.
*
* @param rateDegreesPerSecond The angular rate.
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setGyroRateZ(double rateDegreesPerSecond) {
m_simGyroRateZ.set(rateDegreesPerSecond);
}
/**
* Sets the X axis acceleration.
*
* @param accelMetersPerSecond The acceleration.
*/
public void setAccelX(double accelMetersPerSecond) {
m_simAccelX.set(accelMetersPerSecond / 9.81);
}
/**
* Sets the Y axis acceleration.
*
* @param accelMetersPerSecond The acceleration.
*/
public void setAccelY(double accelMetersPerSecond) {
m_simAccelY.set(accelMetersPerSecond / 9.81);
}
/**
* Sets the Z axis acceleration.
*
* @param accelMetersPerSecond The acceleration.
*/
public void setAccelZ(double accelMetersPerSecond) {
m_simAccelZ.set(accelMetersPerSecond / 9.81);
public void setAccelZ(double accelMetersPerSecondSquared) {
m_simAccelZ.set(accelMetersPerSecondSquared);
}
}

63
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java
View File

@@ -13,9 +13,6 @@ public class ADIS16470_IMUSim {
private final SimDouble m_simGyroAngleX;
private final SimDouble m_simGyroAngleY;
private final SimDouble m_simGyroAngleZ;
private final SimDouble m_simGyroRateX;
private final SimDouble m_simGyroRateY;
private final SimDouble m_simGyroRateZ;
private final SimDouble m_simAccelX;
private final SimDouble m_simAccelY;
private final SimDouble m_simAccelZ;
@@ -30,16 +27,13 @@ public class ADIS16470_IMUSim {
m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
m_simAccelX = wrappedSimDevice.getDouble("accel_x");
m_simAccelY = wrappedSimDevice.getDouble("accel_y");
m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
}
/**
* Sets the X axis angle (CCW positive).
* Sets the X axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
@@ -48,7 +42,7 @@ public class ADIS16470_IMUSim {
}
/**
* Sets the Y axis angle (CCW positive).
* Sets the Y axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
@@ -57,7 +51,7 @@ public class ADIS16470_IMUSim {
}
/**
* Sets the Z axis angle (CCW positive).
* Sets the Z axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
@@ -66,56 +60,29 @@ public class ADIS16470_IMUSim {
}
/**
* Sets the X axis angular rate (CCW positive).
* Sets the X axis acceleration in meters per second squared.
*
* @param rateDegreesPerSecond The angular rate.
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setGyroRateX(double rateDegreesPerSecond) {
m_simGyroRateX.set(rateDegreesPerSecond);
public void setAccelX(double accelMetersPerSecondSquared) {
m_simAccelX.set(accelMetersPerSecondSquared);
}
/**
* Sets the Y axis angular rate (CCW positive).
* Sets the Y axis acceleration in meters per second squared.
*
* @param rateDegreesPerSecond The angular rate.
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setGyroRateY(double rateDegreesPerSecond) {
m_simGyroRateY.set(rateDegreesPerSecond);
public void setAccelY(double accelMetersPerSecondSquared) {
m_simAccelY.set(accelMetersPerSecondSquared);
}
/**
* Sets the Z axis angular rate (CCW positive).
* Sets the Z axis acceleration in meters per second squared.
*
* @param rateDegreesPerSecond The angular rate.
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setGyroRateZ(double rateDegreesPerSecond) {
m_simGyroRateZ.set(rateDegreesPerSecond);
}
/**
* Sets the X axis acceleration.
*
* @param accelMetersPerSecond The acceleration.
*/
public void setAccelX(double accelMetersPerSecond) {
m_simAccelX.set(accelMetersPerSecond / 9.81);
}
/**
* Sets the Y axis acceleration.
*
* @param accelMetersPerSecond The acceleration.
*/
public void setAccelY(double accelMetersPerSecond) {
m_simAccelY.set(accelMetersPerSecond / 9.81);
}
/**
* Sets the Z axis acceleration.
*
* @param accelMetersPerSecond The acceleration.
*/
public void setAccelZ(double accelMetersPerSecond) {
m_simAccelZ.set(accelMetersPerSecond / 9.81);
public void setAccelZ(double accelMetersPerSecondSquared) {
m_simAccelZ.set(accelMetersPerSecondSquared);
}
}