mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-03 03:01:44 +00:00
[wpilibc] Move ADIS IMU constants to inside class (#3852)
This commit is contained in:
Matteo Kimura
@@ -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();
|
||||
|
||||
@@ -29,143 +29,6 @@
|
||||
#include <wpi/sendable/SendableHelper.h>
|
||||
|
||||
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<ADIS16470_IMU> {
|
||||
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.
|
||||
|
||||
Reference in New Issue
Block a user