diff --git a/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp index 47cc8d819e..94eed095a9 100644 --- a/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp +++ b/wpilibc/src/main/native/cpp/ADIS_16470_IMU.cpp @@ -63,11 +63,10 @@ inline void ADISReportError(int32_t status, const char* file, int line, * Constructor. */ ADIS16470_IMU::ADIS16470_IMU() - : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, ADIS16470CalibrationTime::_4s) { -} + : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {} ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, - ADIS16470CalibrationTime cal_time) + CalibrationTime cal_time) : m_yaw_axis(yaw_axis), m_spi_port(port), m_calibration_time(static_cast(cal_time)) { @@ -292,7 +291,7 @@ bool ADIS16470_IMU::SwitchToAutoSPI() { * themselves. After waiting the configured calibration time, the Calibrate() *function should be called to activate the new offset calibration. **/ -int ADIS16470_IMU::ConfigCalTime(ADIS16470CalibrationTime new_cal_time) { +int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) { if (m_calibration_time == static_cast(new_cal_time)) { return 1; } diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h index 64c11f3b90..faa4199e0a 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h @@ -34,75 +34,6 @@ #endif namespace frc { - -/** @brief 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 -static constexpr uint8_t ZGYRO_OUT = 0x08; // Z-axis gyroscope output -static constexpr uint8_t XACCL_OUT = 0x0A; // X-axis accelerometer output -static constexpr uint8_t YACCL_OUT = 0x0C; // Y-axis accelerometer output -static constexpr uint8_t ZACCL_OUT = 0x0E; // Z-axis accelerometer output -static constexpr uint8_t XMAGN_OUT = 0x10; // X-axis magnetometer output -static constexpr uint8_t YMAGN_OUT = 0x12; // Y-axis magnetometer output -static constexpr uint8_t ZMAGN_OUT = 0x14; // Z-axis magnetometer output -static constexpr uint8_t BARO_OUT = - 0x16; // Barometer pressure measurement, high word -static constexpr uint8_t TEMP_OUT = 0x18; // Temperature output -static constexpr uint8_t XGYRO_OFF = - 0x1A; // X-axis gyroscope bias offset factor -static constexpr uint8_t YGYRO_OFF = - 0x1C; // Y-axis gyroscope bias offset factor -static constexpr uint8_t ZGYRO_OFF = - 0x1E; // Z-axis gyroscope bias offset factor -static constexpr uint8_t XACCL_OFF = - 0x20; // X-axis acceleration bias offset factor -static constexpr uint8_t YACCL_OFF = - 0x22; // Y-axis acceleration bias offset factor -static constexpr uint8_t ZACCL_OFF = - 0x24; // Z-axis acceleration bias offset factor -static constexpr uint8_t XMAGN_HIC = - 0x26; // X-axis magnetometer, hard iron factor -static constexpr uint8_t YMAGN_HIC = - 0x28; // Y-axis magnetometer, hard iron factor -static constexpr uint8_t ZMAGN_HIC = - 0x2A; // Z-axis magnetometer, hard iron factor -static constexpr uint8_t XMAGN_SIC = - 0x2C; // X-axis magnetometer, soft iron factor -static constexpr uint8_t YMAGN_SIC = - 0x2E; // Y-axis magnetometer, soft iron factor -static constexpr uint8_t ZMAGN_SIC = - 0x30; // Z-axis magnetometer, soft iron factor -static constexpr uint8_t GPIO_CTRL = 0x32; // GPIO control -static constexpr uint8_t MSC_CTRL = 0x34; // MISC control -static constexpr uint8_t SMPL_PRD = - 0x36; // Sample clock/Decimation filter control -static constexpr uint8_t SENS_AVG = 0x38; // Digital filter control -static constexpr uint8_t SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter -static constexpr uint8_t DIAG_STAT = 0x3C; // System status -static constexpr uint8_t GLOB_CMD = 0x3E; // System command -static constexpr uint8_t ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold -static constexpr uint8_t ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold -static constexpr uint8_t ALM_SMPL1 = 0x44; // Alarm 1 sample size -static constexpr uint8_t ALM_SMPL2 = 0x46; // Alarm 2 sample size -static constexpr uint8_t ALM_CTRL = 0x48; // Alarm control -static constexpr uint8_t LOT_ID1 = 0x52; // Lot identification number -static constexpr uint8_t LOT_ID2 = 0x54; // Lot identification number -static constexpr uint8_t PROD_ID = 0x56; // Product identifier -static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number - -/** @brief ADIS16448 Static Constants */ -const double rad_to_deg = 57.2957795; -const double deg_to_rad = 0.0174532; -const double grav = 9.81; - -/** @brief struct to store offset data */ -struct offset_data { - double m_accum_gyro_x = 0.0; - double m_accum_gyro_y = 0.0; - double m_accum_gyro_z = 0.0; -}; - /** * Use DMA SPI to read rate, acceleration, and magnetometer data from the * ADIS16448 IMU and return the robots heading relative to a starting position, @@ -246,6 +177,74 @@ class ADIS16448_IMU : public nt::NTSendable, void InitSendable(nt::NTSendableBuilder& builder) override; private: + /** @brief 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 + static constexpr uint8_t ZGYRO_OUT = 0x08; // Z-axis gyroscope output + static constexpr uint8_t XACCL_OUT = 0x0A; // X-axis accelerometer output + static constexpr uint8_t YACCL_OUT = 0x0C; // Y-axis accelerometer output + static constexpr uint8_t ZACCL_OUT = 0x0E; // Z-axis accelerometer output + static constexpr uint8_t XMAGN_OUT = 0x10; // X-axis magnetometer output + static constexpr uint8_t YMAGN_OUT = 0x12; // Y-axis magnetometer output + static constexpr uint8_t ZMAGN_OUT = 0x14; // Z-axis magnetometer output + static constexpr uint8_t BARO_OUT = + 0x16; // Barometer pressure measurement, high word + static constexpr uint8_t TEMP_OUT = 0x18; // Temperature output + static constexpr uint8_t XGYRO_OFF = + 0x1A; // X-axis gyroscope bias offset factor + static constexpr uint8_t YGYRO_OFF = + 0x1C; // Y-axis gyroscope bias offset factor + static constexpr uint8_t ZGYRO_OFF = + 0x1E; // Z-axis gyroscope bias offset factor + static constexpr uint8_t XACCL_OFF = + 0x20; // X-axis acceleration bias offset factor + static constexpr uint8_t YACCL_OFF = + 0x22; // Y-axis acceleration bias offset factor + static constexpr uint8_t ZACCL_OFF = + 0x24; // Z-axis acceleration bias offset factor + static constexpr uint8_t XMAGN_HIC = + 0x26; // X-axis magnetometer, hard iron factor + static constexpr uint8_t YMAGN_HIC = + 0x28; // Y-axis magnetometer, hard iron factor + static constexpr uint8_t ZMAGN_HIC = + 0x2A; // Z-axis magnetometer, hard iron factor + static constexpr uint8_t XMAGN_SIC = + 0x2C; // X-axis magnetometer, soft iron factor + static constexpr uint8_t YMAGN_SIC = + 0x2E; // Y-axis magnetometer, soft iron factor + static constexpr uint8_t ZMAGN_SIC = + 0x30; // Z-axis magnetometer, soft iron factor + static constexpr uint8_t GPIO_CTRL = 0x32; // GPIO control + static constexpr uint8_t MSC_CTRL = 0x34; // MISC control + static constexpr uint8_t SMPL_PRD = + 0x36; // Sample clock/Decimation filter control + static constexpr uint8_t SENS_AVG = 0x38; // Digital filter control + static constexpr uint8_t SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter + static constexpr uint8_t DIAG_STAT = 0x3C; // System status + static constexpr uint8_t GLOB_CMD = 0x3E; // System command + static constexpr uint8_t ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold + static constexpr uint8_t ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold + static constexpr uint8_t ALM_SMPL1 = 0x44; // Alarm 1 sample size + static constexpr uint8_t ALM_SMPL2 = 0x46; // Alarm 2 sample size + static constexpr uint8_t ALM_CTRL = 0x48; // Alarm control + static constexpr uint8_t LOT_ID1 = 0x52; // Lot identification number + static constexpr uint8_t LOT_ID2 = 0x54; // Lot identification number + static constexpr uint8_t PROD_ID = 0x56; // Product identifier + 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; + + /** @brief struct to store offset data */ + struct offset_data { + double m_accum_gyro_x = 0.0; + double m_accum_gyro_y = 0.0; + double m_accum_gyro_z = 0.0; + }; + bool SwitchToStandardSPI(); bool SwitchToAutoSPI(); diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h index d515f14166..9feff5fdd1 100644 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h @@ -29,143 +29,6 @@ #include namespace frc { - -/* ADIS16470 Calibration Time Enum Class */ -enum class ADIS16470CalibrationTime { - _32ms = 0, - _64ms = 1, - _128ms = 2, - _256ms = 3, - _512ms = 4, - _1s = 5, - _2s = 6, - _4s = 7, - _8s = 8, - _16s = 9, - _32s = 10, - _64s = 11 -}; - -/* ADIS16470 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 -static constexpr uint8_t X_GYRO_LOW = - 0x04; // X-axis gyroscope output, lower word -static constexpr uint8_t X_GYRO_OUT = - 0x06; // X-axis gyroscope output, upper word -static constexpr uint8_t Y_GYRO_LOW = - 0x08; // Y-axis gyroscope output, lower word -static constexpr uint8_t Y_GYRO_OUT = - 0x0A; // Y-axis gyroscope output, upper word -static constexpr uint8_t Z_GYRO_LOW = - 0x0C; // Z-axis gyroscope output, lower word -static constexpr uint8_t Z_GYRO_OUT = - 0x0E; // Z-axis gyroscope output, upper word -static constexpr uint8_t X_ACCL_LOW = - 0x10; // X-axis accelerometer output, lower word -static constexpr uint8_t X_ACCL_OUT = - 0x12; // X-axis accelerometer output, upper word -static constexpr uint8_t Y_ACCL_LOW = - 0x14; // Y-axis accelerometer output, lower word -static constexpr uint8_t Y_ACCL_OUT = - 0x16; // Y-axis accelerometer output, upper word -static constexpr uint8_t Z_ACCL_LOW = - 0x18; // Z-axis accelerometer output, lower word -static constexpr uint8_t Z_ACCL_OUT = - 0x1A; // Z-axis accelerometer output, upper word -static constexpr uint8_t TEMP_OUT = - 0x1C; // Temperature output (internal, not calibrated) -static constexpr uint8_t TIME_STAMP = 0x1E; // PPS mode time stamp -static constexpr uint8_t X_DELTANG_LOW = - 0x24; // X-axis delta angle output, lower word -static constexpr uint8_t X_DELTANG_OUT = - 0x26; // X-axis delta angle output, upper word -static constexpr uint8_t Y_DELTANG_LOW = - 0x28; // Y-axis delta angle output, lower word -static constexpr uint8_t Y_DELTANG_OUT = - 0x2A; // Y-axis delta angle output, upper word -static constexpr uint8_t Z_DELTANG_LOW = - 0x2C; // Z-axis delta angle output, lower word -static constexpr uint8_t Z_DELTANG_OUT = - 0x2E; // Z-axis delta angle output, upper word -static constexpr uint8_t X_DELTVEL_LOW = - 0x30; // X-axis delta velocity output, lower word -static constexpr uint8_t X_DELTVEL_OUT = - 0x32; // X-axis delta velocity output, upper word -static constexpr uint8_t Y_DELTVEL_LOW = - 0x34; // Y-axis delta velocity output, lower word -static constexpr uint8_t Y_DELTVEL_OUT = - 0x36; // Y-axis delta velocity output, upper word -static constexpr uint8_t Z_DELTVEL_LOW = - 0x38; // Z-axis delta velocity output, lower word -static constexpr uint8_t Z_DELTVEL_OUT = - 0x3A; // Z-axis delta velocity output, upper word -static constexpr uint8_t XG_BIAS_LOW = - 0x40; // X-axis gyroscope bias offset correction, lower word -static constexpr uint8_t XG_BIAS_HIGH = - 0x42; // X-axis gyroscope bias offset correction, upper word -static constexpr uint8_t YG_BIAS_LOW = - 0x44; // Y-axis gyroscope bias offset correction, lower word -static constexpr uint8_t YG_BIAS_HIGH = - 0x46; // Y-axis gyroscope bias offset correction, upper word -static constexpr uint8_t ZG_BIAS_LOW = - 0x48; // Z-axis gyroscope bias offset correction, lower word -static constexpr uint8_t ZG_BIAS_HIGH = - 0x4A; // Z-axis gyroscope bias offset correction, upper word -static constexpr uint8_t XA_BIAS_LOW = - 0x4C; // X-axis accelerometer bias offset correction, lower word -static constexpr uint8_t XA_BIAS_HIGH = - 0x4E; // X-axis accelerometer bias offset correction, upper word -static constexpr uint8_t YA_BIAS_LOW = - 0x50; // Y-axis accelerometer bias offset correction, lower word -static constexpr uint8_t YA_BIAS_HIGH = - 0x52; // Y-axis accelerometer bias offset correction, upper word -static constexpr uint8_t ZA_BIAS_LOW = - 0x54; // Z-axis accelerometer bias offset correction, lower word -static constexpr uint8_t ZA_BIAS_HIGH = - 0x56; // Z-axis accelerometer bias offset correction, upper word -static constexpr uint8_t FILT_CTRL = 0x5C; // Filter control -static constexpr uint8_t MSC_CTRL = 0x60; // Miscellaneous control -static constexpr uint8_t UP_SCALE = 0x62; // Clock scale factor, PPS mode -static constexpr uint8_t DEC_RATE = - 0x64; // Decimation rate control (output data rate) -static constexpr uint8_t NULL_CNFG = 0x66; // Auto-null configuration control -static constexpr uint8_t GLOB_CMD = 0x68; // Global commands -static constexpr uint8_t FIRM_REV = 0x6C; // Firmware revision -static constexpr uint8_t FIRM_DM = - 0x6E; // Firmware revision date, month and day -static constexpr uint8_t FIRM_Y = 0x70; // Firmware revision date, year -static constexpr uint8_t PROD_ID = 0x72; // Product identification -static constexpr uint8_t SERIAL_NUM = - 0x74; // Serial number (relative to assembly lot) -static constexpr uint8_t USER_SCR1 = 0x76; // User scratch register 1 -static constexpr uint8_t USER_SCR2 = 0x78; // User scratch register 2 -static constexpr uint8_t USER_SCR3 = 0x7A; // User scratch register 3 -static constexpr uint8_t FLSHCNT_LOW = 0x7C; // Flash update count, lower word -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}; - -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 */ -const double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */ -const double rad_to_deg = 57.2957795; -const double deg_to_rad = 0.0174532; -const double grav = 9.81; - /** * Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and * return the robot's heading relative to a starting position and instant @@ -185,6 +48,22 @@ const double grav = 9.81; class ADIS16470_IMU : public nt::NTSendable, public wpi::SendableHelper { public: + /* ADIS16470 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 }; /** @@ -206,7 +85,7 @@ class ADIS16470_IMU : public nt::NTSendable, * @param cal_time The calibration time that should be used on start-up. */ explicit ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port, - ADIS16470CalibrationTime cal_time); + CalibrationTime cal_time); /** * @brief Destructor. Kills the acquisiton loop and closes the SPI peripheral. @@ -228,7 +107,7 @@ class ADIS16470_IMU : public nt::NTSendable, * @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. */ - int ConfigCalTime(ADIS16470CalibrationTime new_cal_time); + int ConfigCalTime(CalibrationTime new_cal_time); /** * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations. @@ -285,6 +164,130 @@ class ADIS16470_IMU : public nt::NTSendable, void InitSendable(nt::NTSendableBuilder& builder) override; private: + /* ADIS16470 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 + static constexpr uint8_t X_GYRO_LOW = + 0x04; // X-axis gyroscope output, lower word + static constexpr uint8_t X_GYRO_OUT = + 0x06; // X-axis gyroscope output, upper word + static constexpr uint8_t Y_GYRO_LOW = + 0x08; // Y-axis gyroscope output, lower word + static constexpr uint8_t Y_GYRO_OUT = + 0x0A; // Y-axis gyroscope output, upper word + static constexpr uint8_t Z_GYRO_LOW = + 0x0C; // Z-axis gyroscope output, lower word + static constexpr uint8_t Z_GYRO_OUT = + 0x0E; // Z-axis gyroscope output, upper word + static constexpr uint8_t X_ACCL_LOW = + 0x10; // X-axis accelerometer output, lower word + static constexpr uint8_t X_ACCL_OUT = + 0x12; // X-axis accelerometer output, upper word + static constexpr uint8_t Y_ACCL_LOW = + 0x14; // Y-axis accelerometer output, lower word + static constexpr uint8_t Y_ACCL_OUT = + 0x16; // Y-axis accelerometer output, upper word + static constexpr uint8_t Z_ACCL_LOW = + 0x18; // Z-axis accelerometer output, lower word + static constexpr uint8_t Z_ACCL_OUT = + 0x1A; // Z-axis accelerometer output, upper word + static constexpr uint8_t TEMP_OUT = + 0x1C; // Temperature output (internal, not calibrated) + static constexpr uint8_t TIME_STAMP = 0x1E; // PPS mode time stamp + static constexpr uint8_t X_DELTANG_LOW = + 0x24; // X-axis delta angle output, lower word + static constexpr uint8_t X_DELTANG_OUT = + 0x26; // X-axis delta angle output, upper word + static constexpr uint8_t Y_DELTANG_LOW = + 0x28; // Y-axis delta angle output, lower word + static constexpr uint8_t Y_DELTANG_OUT = + 0x2A; // Y-axis delta angle output, upper word + static constexpr uint8_t Z_DELTANG_LOW = + 0x2C; // Z-axis delta angle output, lower word + static constexpr uint8_t Z_DELTANG_OUT = + 0x2E; // Z-axis delta angle output, upper word + static constexpr uint8_t X_DELTVEL_LOW = + 0x30; // X-axis delta velocity output, lower word + static constexpr uint8_t X_DELTVEL_OUT = + 0x32; // X-axis delta velocity output, upper word + static constexpr uint8_t Y_DELTVEL_LOW = + 0x34; // Y-axis delta velocity output, lower word + static constexpr uint8_t Y_DELTVEL_OUT = + 0x36; // Y-axis delta velocity output, upper word + static constexpr uint8_t Z_DELTVEL_LOW = + 0x38; // Z-axis delta velocity output, lower word + static constexpr uint8_t Z_DELTVEL_OUT = + 0x3A; // Z-axis delta velocity output, upper word + static constexpr uint8_t XG_BIAS_LOW = + 0x40; // X-axis gyroscope bias offset correction, lower word + static constexpr uint8_t XG_BIAS_HIGH = + 0x42; // X-axis gyroscope bias offset correction, upper word + static constexpr uint8_t YG_BIAS_LOW = + 0x44; // Y-axis gyroscope bias offset correction, lower word + static constexpr uint8_t YG_BIAS_HIGH = + 0x46; // Y-axis gyroscope bias offset correction, upper word + static constexpr uint8_t ZG_BIAS_LOW = + 0x48; // Z-axis gyroscope bias offset correction, lower word + static constexpr uint8_t ZG_BIAS_HIGH = + 0x4A; // Z-axis gyroscope bias offset correction, upper word + static constexpr uint8_t XA_BIAS_LOW = + 0x4C; // X-axis accelerometer bias offset correction, lower word + static constexpr uint8_t XA_BIAS_HIGH = + 0x4E; // X-axis accelerometer bias offset correction, upper word + static constexpr uint8_t YA_BIAS_LOW = + 0x50; // Y-axis accelerometer bias offset correction, lower word + static constexpr uint8_t YA_BIAS_HIGH = + 0x52; // Y-axis accelerometer bias offset correction, upper word + static constexpr uint8_t ZA_BIAS_LOW = + 0x54; // Z-axis accelerometer bias offset correction, lower word + static constexpr uint8_t ZA_BIAS_HIGH = + 0x56; // Z-axis accelerometer bias offset correction, upper word + static constexpr uint8_t FILT_CTRL = 0x5C; // Filter control + static constexpr uint8_t MSC_CTRL = 0x60; // Miscellaneous control + static constexpr uint8_t UP_SCALE = 0x62; // Clock scale factor, PPS mode + static constexpr uint8_t DEC_RATE = + 0x64; // Decimation rate control (output data rate) + static constexpr uint8_t NULL_CNFG = 0x66; // Auto-null configuration control + static constexpr uint8_t GLOB_CMD = 0x68; // Global commands + static constexpr uint8_t FIRM_REV = 0x6C; // Firmware revision + static constexpr uint8_t FIRM_DM = + 0x6E; // Firmware revision date, month and day + static constexpr uint8_t FIRM_Y = 0x70; // Firmware revision date, year + static constexpr uint8_t PROD_ID = 0x72; // Product identification + static constexpr uint8_t SERIAL_NUM = + 0x74; // Serial number (relative to assembly lot) + static constexpr uint8_t USER_SCR1 = 0x76; // User scratch register 1 + static constexpr uint8_t USER_SCR2 = 0x78; // User scratch register 2 + static constexpr uint8_t USER_SCR3 = 0x7A; // User scratch register 3 + static constexpr uint8_t FLSHCNT_LOW = + 0x7C; // Flash update count, lower word + 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}; + + 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 rad_to_deg = 57.2957795; + static constexpr double deg_to_rad = 0.0174532; + static constexpr double grav = 9.81; + /** * @brief Switches to standard SPI operation. Primarily used when exiting auto * SPI mode.