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

[wpilib] Refactor and clean up ADIS IMU classes (#6719)

Browse Source
This commit is contained in:
Gold856
2024-07-19 00:09:11 -04:00
committed by GitHub
parent 45823abe86
commit 289d45b081
6 changed files with 495 additions and 1281 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -281,7 +281,7 @@ class ADIS16448_IMU : public wpi::Sendable,
void InitSendable(wpi::SendableBuilder& builder) override;
private:
/** @brief ADIS16448 Register Map Declaration */
// ADIS16448 Register Map Declaration
static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
static constexpr uint8_t XGYRO_OUT = 0x04; // X-axis gyroscope output
static constexpr uint8_t YGYRO_OUT = 0x06; // Y-axis gyroscope output
@@ -338,9 +338,9 @@ class ADIS16448_IMU : public wpi::Sendable,
static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number
/** @brief ADIS16448 Static Constants */
static constexpr double rad_to_deg = 57.2957795;
static constexpr double deg_to_rad = 0.0174532;
static constexpr double grav = 9.81;
static constexpr double kRadToDeg = 57.2957795;
static constexpr double kDegToRad = 0.0174532;
static constexpr double kGrav = 9.81;
/** @brief struct to store offset data */
struct OffsetData {
@@ -382,8 +382,8 @@ class ADIS16448_IMU : public wpi::Sendable,
double m_temp = 0.0;
// Complementary filter variables
double m_tau = 0.5;
double m_dt, m_alpha = 0.0;
static constexpr double kTau = 0.5;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// vector for storing most recent imu values
@@ -408,8 +408,6 @@ class ADIS16448_IMU : public wpi::Sendable,
// Complementary filter functions
double FormatFastConverge(double compAngle, double accAngle);
double FormatRange0to2PI(double compAngle);
double FormatAccelRange(double accelAngle, double accelZ);
double CompFilterProcess(double compAngle, double accelAngle, double omega);
@@ -463,7 +461,7 @@ class ADIS16448_IMU : public wpi::Sendable,
mutable NonMovableMutexWrapper m_mutex;
// CRC-16 Look-Up Table
static constexpr uint16_t adiscrc[256] = {
static constexpr uint16_t m_adiscrc[256] = {
0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,

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

@@ -162,8 +162,10 @@ class ADIS16470_IMU : public wpi::Sendable,
void Calibrate();
/**
* @brief Switches the active SPI port to standard SPI mode, writes a new
* value to the NULL_CNFG register in the IMU, and re-enables auto SPI.
* Configures calibration time.
*
* @param new_cal_time New calibration time
* @return 0 if success, 1 if no change, 2 if error.
*/
int ConfigCalTime(CalibrationTime new_cal_time);
@@ -418,11 +420,11 @@ class ADIS16470_IMU : public wpi::Sendable,
Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
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;
static constexpr double kRadToDeg = 57.2957795;
static constexpr double kDegToRad = 0.0174532;
static constexpr double kGrav = 9.81;
/** @brief Resources **/
// Resources
DigitalInput* m_reset_in = nullptr;
DigitalOutput* m_status_led = nullptr;
@@ -487,15 +489,13 @@ class ADIS16470_IMU : public wpi::Sendable,
double m_accel_z = 0.0;
// Complementary filter variables
double m_tau = 1.0;
double m_dt, m_alpha = 0.0;
static constexpr double kTau = 1.0;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// Complementary filter functions
double FormatFastConverge(double compAngle, double accAngle);
double FormatRange0to2PI(double compAngle);
double FormatAccelRange(double accelAngle, double accelZ);
double CompFilterProcess(double compAngle, double accelAngle, double omega);