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] Add ADIS IMUs (#3777)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Matteo Kimura <mateus.sakata@gmail.com>
This commit is contained in:
Thad House
2021-12-30 19:43:53 -08:00
committed by GitHub
parent 315be873c4
commit 1f59ff72f9
7 changed files with 4418 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

378
wpilibc/src/main/native/include/frc/ADIS16448_IMU.h Normal file
View File

@@ -0,0 +1,378 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/* */
/* Modified by Juan Chong - frcsupport@analog.com */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/DigitalInput.h>
#include <frc/DigitalOutput.h>
#include <frc/DigitalSource.h>
#include <frc/SPI.h>
#include <stdint.h>
#include <atomic>
#include <memory>
#include <thread>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableHelper.h>
// Not always defined in cmath (not part of standard)
#ifndef M_PI
#define M_PI 3.14159265358979323846
#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,
* AHRS, and instant measurements
*
* The ADIS16448 gyro angle outputs track the robot's heading based on the
* starting position. As the robot rotates the new heading is computed by
* integrating the rate of rotation returned by the IMU. When the class is
* instantiated, a short calibration routine is performed where the IMU samples
* the gyros while at rest to determine the initial offset. This is subtracted
* from each sample to determine the heading.
*
* This class is for the ADIS16448 IMU connected via the SPI port available on
* the RoboRIO MXP port.
*/
class ADIS16448_IMU : public nt::NTSendable,
public wpi::SendableHelper<ADIS16448_IMU> {
public:
enum IMUAxis { kX, kY, kZ };
/**
* IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
* AHRS computation.
*/
ADIS16448_IMU();
/**
* IMU constructor on the specified MXP port and orientation.
*
* @param yaw_axis The axis where gravity is present. Valid options are kX,
* kY, and kZ
* @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);
~ADIS16448_IMU() override;
ADIS16448_IMU(ADIS16448_IMU&&) = default;
ADIS16448_IMU& operator=(ADIS16448_IMU&&) = default;
/**
* Initialize the IMU.
*
* Perform gyro offset calibration by collecting data for a number of seconds
* and computing the center value. The center value is subtracted from
* subsequent measurements.
*
* It's important to make sure that the robot is not moving while the
* centering calculations are in progress, this is typically done when the
* robot is first turned on while it's sitting at rest before the match
* starts.
*
* The calibration routine can be triggered by the user during runtime.
*/
void Calibrate();
/**
* Configures the calibration time used for the next calibrate.
*
* @param new_cal_time The calibration time that should be used
*/
int ConfigCalTime(int new_cal_time);
/**
* Reset the gyro.
*
* Resets the gyro accumulations to a heading of zero. This can be used if
* there is significant drift in the gyro and it needs to be recalibrated
* after running.
*/
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.
*/
double 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
*/
double GetRate() const;
double GetGyroAngleX() const;
double GetGyroAngleY() const;
double GetGyroAngleZ() const;
double GetGyroInstantX() const;
double GetGyroInstantY() const;
double GetGyroInstantZ() const;
double GetAccelInstantX() const;
double GetAccelInstantY() const;
double GetAccelInstantZ() const;
double GetXComplementaryAngle() const;
double GetYComplementaryAngle() const;
double GetXFilteredAccelAngle() const;
double GetYFilteredAccelAngle() const;
double GetMagInstantX() const;
double GetMagInstantY() const;
double GetMagInstantZ() const;
double GetBarometricPressure() const;
double GetTemperature() const;
IMUAxis GetYawAxis() const;
int SetYawAxis(IMUAxis yaw_axis);
int ConfigDecRate(uint16_t DecimationRate);
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
bool SwitchToStandardSPI();
bool SwitchToAutoSPI();
uint16_t ReadRegister(uint8_t reg);
void WriteRegister(uint8_t reg, uint16_t val);
void Acquire();
void Close();
// User-specified yaw axis
IMUAxis m_yaw_axis;
// Last read values (post-scaling)
double m_gyro_x = 0.0;
double m_gyro_y = 0.0;
double m_gyro_z = 0.0;
double m_accel_x = 0.0;
double m_accel_y = 0.0;
double m_accel_z = 0.0;
double m_mag_x = 0.0;
double m_mag_y = 0.0;
double m_mag_z = 0.0;
double m_baro = 0.0;
double m_temp = 0.0;
// Complementary filter variables
double m_tau = 0.5;
double m_dt, m_alpha = 0.0;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// vector for storing most recent imu values
offset_data* m_offset_buffer = nullptr;
double m_gyro_offset_x = 0.0;
double m_gyro_offset_y = 0.0;
double m_gyro_offset_z = 0.0;
// function to re-init offset buffer
void InitOffsetBuffer(int size);
// Accumulated gyro values (for offset calculation)
int m_avg_size = 0;
int m_accum_count = 0;
// Integrated gyro values
double m_integ_gyro_x = 0.0;
double m_integ_gyro_y = 0.0;
double m_integ_gyro_z = 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);
// State and resource variables
volatile bool m_thread_active = false;
volatile bool m_first_run = true;
volatile bool m_thread_idle = false;
volatile bool m_start_up_mode = true;
bool m_auto_configured = false;
SPI::Port m_spi_port;
uint16_t m_calibration_time;
SPI* m_spi = nullptr;
DigitalInput* m_auto_interrupt = nullptr;
std::thread m_acquire_task;
struct NonMovableMutexWrapper {
wpi::mutex mutex;
NonMovableMutexWrapper() = default;
NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
return *this;
}
void lock() { mutex.lock(); }
void unlock() { mutex.unlock(); }
bool try_lock() noexcept { return mutex.try_lock(); }
};
mutable NonMovableMutexWrapper m_mutex;
// CRC-16 Look-Up Table
static constexpr uint16_t 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,
0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 0x1C39, 0x0BF7, 0x13E6, 0x0428,
0x0387, 0x1449, 0x0C58, 0x1B96, 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8,
0x0B76, 0x1367, 0x04A9, 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74,
0x1265, 0x05AB, 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A,
0x1A94, 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 0x060C,
0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 0x1933, 0x0EFD,
0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 0x0408, 0x13C6, 0x0BD7,
0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 0x1B37, 0x0CF9, 0x14E8, 0x0326,
0x0489, 0x1347, 0x0B56, 0x1C98, 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B,
0x1245, 0x0A54, 0x1D9A, 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A,
0x156B, 0x02A5, 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040,
0x178E, 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 0x1123,
0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 0x0C18, 0x1BD6,
0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 0x1327, 0x04E9, 0x1CF8,
0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 0x1225, 0x05EB, 0x1DFA, 0x0A34,
0x0D9B, 0x1A55, 0x0244, 0x158A, 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4,
0x056A, 0x1D7B, 0x0AB5, 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060,
0x1871, 0x0FBF, 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E,
0x1080, 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 0x1429,
0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 0x0B16, 0x1CD8,
0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 0x0A14, 0x1DDA, 0x05CB,
0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 0x152B, 0x02E5, 0x1AF4, 0x0D3A,
0x0A95, 0x1D5B, 0x054A, 0x1284};
};
} // namespace frc

390
wpilibc/src/main/native/include/frc/ADIS16470_IMU.h Normal file
View File

@@ -0,0 +1,390 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/* */
/* Juan Chong - frcsupport@analog.com */
/*----------------------------------------------------------------------------*/
#pragma once
#include <frc/DigitalInput.h>
#include <frc/DigitalOutput.h>
#include <frc/DigitalSource.h>
#include <frc/SPI.h>
#include <stdint.h>
#include <atomic>
#include <memory>
#include <thread>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#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
* measurements
*
* The ADIS16470 gyro angle outputs track the robot's heading based on the
* starting position. As the robot rotates the new heading is computed by
* integrating the rate of rotation returned by the IMU. When the class is
* instantiated, a short calibration routine is performed where the IMU samples
* the gyros while at rest to determine the initial offset. This is subtracted
* from each sample to determine the heading.
*
* This class is for the ADIS16470 IMU connected via the primary SPI port
* available on the RoboRIO.
*/
class ADIS16470_IMU : public nt::NTSendable,
public wpi::SendableHelper<ADIS16470_IMU> {
public:
enum IMUAxis { kX, kY, kZ };
/**
* @brief Default constructor. Uses CS0 on the 10-pin SPI port, the yaw axis
* is set to the IMU Z axis, and calibration time is defaulted to 4 seconds.
*/
ADIS16470_IMU();
/**
* @brief Customizable constructor. Allows the SPI port and CS to be
* customized, the yaw axis used for GetAngle() is adjustable, and initial
* calibration time can be modified.
*
* @param yaw_axis Selects the "default" axis to use for GetAngle() and
* GetRate()
*
* @param port The SPI port and CS where the IMU is connected.
*
* @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);
/**
* @brief Destructor. Kills the acquisiton loop and closes the SPI peripheral.
*/
~ADIS16470_IMU() override;
ADIS16470_IMU(ADIS16470_IMU&&) = default;
ADIS16470_IMU& operator=(ADIS16470_IMU&&) = default;
int ConfigDecRate(uint16_t reg);
/**
* @brief Switches the active SPI port to standard SPI mode, writes the
* command to activate the new null configuration, and re-enables auto SPI.
*/
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.
*/
int ConfigCalTime(ADIS16470CalibrationTime new_cal_time);
/**
* @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
*
* Resets the gyro accumulations to a heading of zero. This can be used if
* the "zero" orientation of the sensor needs to be changed in runtime.
*/
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.
*/
double GetAngle() const;
double GetRate() const;
double GetGyroInstantX() const;
double GetGyroInstantY() const;
double GetGyroInstantZ() const;
double GetAccelInstantX() const;
double GetAccelInstantY() const;
double GetAccelInstantZ() const;
double GetXComplementaryAngle() const;
double GetYComplementaryAngle() const;
double GetXFilteredAccelAngle() const;
double GetYFilteredAccelAngle() const;
IMUAxis GetYawAxis() const;
int SetYawAxis(IMUAxis yaw_axis);
// IMU yaw axis
IMUAxis m_yaw_axis;
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
/**
* @brief Switches to standard SPI operation. Primarily used when exiting auto
* SPI mode.
*
* @return A boolean indicating the success or failure of setting up the SPI
* peripheral in standard SPI mode.
*/
bool SwitchToStandardSPI();
/**
* @brief Switches to auto SPI operation. Primarily used when exiting standard
* SPI mode.
*
* @return A boolean indicating the success or failure of setting up the SPI
* peripheral in auto SPI mode.
*/
bool SwitchToAutoSPI();
/**
* @brief Reads the contents of a specified register location over SPI.
*
* @param reg An unsigned, 8-bit register location.
*
* @return An unsigned, 16-bit number representing the contents of the
* requested register location.
*/
uint16_t ReadRegister(uint8_t reg);
/**
* @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
* locations over SPI.
*
* @param reg An unsigned, 8-bit register location.
*
* @param val An unsigned, 16-bit value to be written to the specified
* register location.
*/
void WriteRegister(uint8_t reg, uint16_t val);
/**
* @brief Main acquisition loop. Typically called asynchronously and
* free-wheels while the robot code is active.
*/
void Acquire();
void Close();
// Integrated gyro value
double m_integ_angle = 0.0;
// Instant raw outputs
double m_gyro_x, m_gyro_y, m_gyro_z, m_accel_x, m_accel_y, m_accel_z = 0.0;
// Complementary filter variables
double m_tau = 1.0;
double m_dt, m_alpha = 0.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);
// State and resource variables
volatile bool m_thread_active = false;
volatile bool m_first_run = true;
volatile bool m_thread_idle = false;
bool m_auto_configured = false;
SPI::Port m_spi_port;
uint16_t m_calibration_time;
SPI* m_spi = nullptr;
DigitalInput* m_auto_interrupt = nullptr;
double m_scaled_sample_rate = 2500.0; // Default sample rate setting
std::thread m_acquire_task;
struct NonMovableMutexWrapper {
wpi::mutex mutex;
NonMovableMutexWrapper() = default;
NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
return *this;
}
void lock() { mutex.lock(); }
void unlock() { mutex.unlock(); }
bool try_lock() noexcept { return mutex.try_lock(); }
};
mutable NonMovableMutexWrapper m_mutex;
};
} // namespace frc