Created
June 6, 2019 19:35
-
-
Save PCModeActivate/5ea62d85c74c09e4d9b38a8826461e2a to your computer and use it in GitHub Desktop.
Modified for debug output ADIS 16470 library
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/*----------------------------------------------------------------------------*/ | |
/* Copyright (c) FIRST 2016. 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. */ | |
/*----------------------------------------------------------------------------*/ | |
package ca.mcrobotics.subsystems; | |
import java.nio.ByteBuffer; | |
import java.nio.ByteOrder; | |
import java.util.concurrent.atomic.AtomicBoolean; | |
import java.util.concurrent.locks.Condition; | |
import java.util.concurrent.locks.Lock; | |
import java.util.concurrent.locks.ReentrantLock; | |
import edu.wpi.first.hal.HAL; | |
import edu.wpi.first.hal.FRCNetComm.tResourceType; | |
import edu.wpi.first.wpilibj.DigitalInput; | |
import edu.wpi.first.wpilibj.DigitalOutput; | |
import edu.wpi.first.wpilibj.DriverStation; | |
import edu.wpi.first.wpilibj.GyroBase; | |
import edu.wpi.first.wpilibj.PIDSource; | |
import edu.wpi.first.wpilibj.SPI; | |
import edu.wpi.first.wpilibj.Sendable; | |
import edu.wpi.first.wpilibj.Timer; | |
import edu.wpi.first.wpilibj.interfaces.Gyro; | |
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; | |
import edu.wpi.first.networktables.NetworkTable; | |
import edu.wpi.first.networktables.NetworkTableEntry; | |
import edu.wpi.first.networktables.NetworkTableInstance; | |
//This is a slightly modified library of ADIS 16470 (Nothing is changed, System.out output is enabled) | |
/** | |
* This class is for the ADIS16448 IMU that connects to the RoboRIO MXP port. | |
*/ | |
@SuppressWarnings("unused") | |
public class ADIS extends GyroBase implements Gyro, PIDSource, Sendable { | |
public static final double kCalibrationSampleTime = 5.0; // Calibration time in seconds (regardless of sps) | |
public static final double kDegreePerSecondPerLSB = 1.0/10.0; | |
public static final double kGPerLSB = 1.0/800.0; | |
public static final double kDegCPerLSB = 1.0/10.0; | |
public static final double kDegCOffset = 0.0; | |
public static final int kGLOB_CMD = 0x68; | |
public static final int kRegDEC_RATE = 0x64; | |
public static final int kRegFILT_CTRL = 0x5C; | |
public static final int kRegMSC_CTRL = 0x60; | |
public static final int kRegPROD_ID = 0x72; | |
public enum Axis { kX, kY, kZ } | |
// AHRS yaw axis | |
public Axis m_yaw_axis; | |
// gyro offset | |
public double m_gyro_offset_x = 0.0; | |
public double m_gyro_offset_y = 0.0; | |
public double m_gyro_offset_z = 0.0; | |
// last read values (post-scaling) | |
public double m_gyro_x = 0.0; | |
public double m_gyro_y = 0.0; | |
public double m_gyro_z = 0.0; | |
public double m_accel_x = 0.0; | |
public double m_accel_y = 0.0; | |
public double m_accel_z = 0.0; | |
public double m_temp = 0.0; | |
public double m_status = 0.0; | |
public double m_counter = 0.0; | |
public double dt = 0.0; | |
// accumulated gyro values (for offset calculation) | |
public int m_accum_count = 0; | |
public double m_accum_gyro_x = 0.0; | |
public double m_accum_gyro_y = 0.0; | |
public double m_accum_gyro_z = 0.0; | |
// integrated gyro values | |
public double m_integ_gyro_x = 0.0; | |
public double m_integ_gyro_y = 0.0; | |
public double m_integ_gyro_z = 0.0; | |
// last sample time | |
public double m_last_sample_time = 0.0; | |
public AtomicBoolean m_freed = new AtomicBoolean(false); | |
public SPI m_spi; | |
public DigitalInput m_interrupt; | |
// Sample from the IMU | |
public static class Sample { | |
public double gyro_x; | |
public double gyro_y; | |
public double gyro_z; | |
public double accel_x; | |
public double accel_y; | |
public double accel_z; | |
public double mag_x; | |
public double mag_y; | |
public double mag_z; | |
public double baro; | |
public double temp; | |
public double dt; | |
// Swap axis as appropriate for yaw axis selection | |
public void adjustYawAxis(Axis yaw_axis) { | |
switch (yaw_axis) { | |
case kX: { | |
// swap X and Z | |
double tmp; | |
tmp = accel_x; | |
accel_x = accel_z; | |
accel_z = tmp; | |
tmp = mag_x; | |
mag_x = mag_z; | |
mag_z = tmp; | |
tmp = gyro_x; | |
gyro_x = gyro_z; | |
gyro_z = tmp; | |
break; | |
} | |
case kY: { | |
// swap Y and Z | |
double tmp; | |
tmp = accel_y; | |
accel_y = accel_z; | |
accel_z = tmp; | |
tmp = mag_y; | |
mag_y = mag_z; | |
mag_z = tmp; | |
tmp = gyro_y; | |
gyro_y = gyro_z; | |
gyro_z = tmp; | |
break; | |
} | |
case kZ: | |
default: | |
// no swap required | |
break; | |
} | |
} | |
} | |
// Sample FIFO | |
public static final int kSamplesDepth = 10; | |
public final Sample[] m_samples; | |
public final Lock m_samples_mutex; | |
public final Condition m_samples_not_empty; | |
public int m_samples_count = 0; | |
public int m_samples_take_index = 0; | |
public int m_samples_put_index = 0; | |
public boolean m_calculate_started = false; | |
// Previous timestamp | |
long timestamp_old = 0; | |
public static class AcquireTask implements Runnable { | |
public ADIS imu; | |
public AcquireTask(ADIS imu) { | |
this.imu = imu; | |
} | |
@Override | |
public void run() { | |
imu.acquire(); | |
} | |
} | |
public Thread m_acquire_task; | |
public ADIS(){ | |
this(Axis.kZ, SPI.Port.kOnboardCS0); | |
} | |
/** | |
* @param yaw_axis Which axis is Yaw | |
* @param port SPI port to use | |
*/ | |
public ADIS(Axis yaw_axis, SPI.Port port) { | |
m_yaw_axis = yaw_axis; | |
//TODO: DIO based reset | |
Timer.delay(0.5); | |
m_spi = new SPI(port); | |
m_spi.setClockRate(1000000); | |
m_spi.setMSBFirst(); | |
m_spi.setSampleDataOnTrailingEdge(); | |
m_spi.setClockActiveLow(); | |
m_spi.setChipSelectActiveLow(); | |
readRegister(kRegPROD_ID); // dummy read | |
// Validate the product ID | |
if (readRegister(kRegPROD_ID) != 16982&&false) { | |
m_spi.close(); | |
m_spi = null; | |
m_samples = null; | |
m_samples_mutex = null; | |
m_samples_not_empty = null; | |
DriverStation.reportError("could not find ADIS16470", false); | |
return; | |
} | |
// Set IMU internal decimation to 200 SPS | |
writeRegister(kRegDEC_RATE, 0x0009); | |
// Enable Data Ready (LOW = Good Data), gSense Compensation, PoP | |
writeRegister(kRegMSC_CTRL, 0x00C1); | |
// Configure IMU internal Bartlett filter | |
writeRegister(kRegFILT_CTRL, 0x0000); | |
// Create data acq FIFO. We make the FIFO 2 longer than it needs | |
// to be so the input and output never overlap (we hold a reference | |
// to the output while the lock is released). | |
m_samples_mutex = new ReentrantLock(); | |
m_samples_not_empty = m_samples_mutex.newCondition(); | |
m_samples = new Sample[kSamplesDepth + 2]; | |
for (int i=0; i<kSamplesDepth + 2; i++) { | |
m_samples[i] = new Sample(); | |
} | |
// Configure interrupt on SPI CS1 | |
m_interrupt = new DigitalInput(26); | |
// Configure DMA SPI | |
m_spi.initAuto(8200); | |
m_spi.setAutoTransmitData(new byte[]{kGLOB_CMD},21); | |
// Kick off DMA SPI (Note: Device configration impossible after SPI DMA is activated) | |
m_spi.startAutoTrigger(m_interrupt,true,false); | |
m_freed.set(false); | |
m_acquire_task = new Thread(new AcquireTask(this)); | |
m_acquire_task.setDaemon(true); | |
m_acquire_task.start(); | |
// Execute offset calibration on start-up | |
calibrate(); | |
Timer.delay(0.500); | |
// Re-initialize accumulations after calibration | |
reset(); | |
// Let the user know the IMU was initiallized successfully | |
DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); | |
// Report usage and post data to DS | |
HAL.report(tResourceType.kResourceType_ADIS16470, 0); | |
setName("ADIS16470"); | |
} | |
/** | |
* {@inheritDoc} | |
*/ | |
@Override | |
public void calibrate() { | |
if (m_spi == null) return; | |
Timer.delay(0.1); | |
synchronized (this) { | |
m_accum_count = 0; | |
m_accum_gyro_x = 0.0; | |
m_accum_gyro_y = 0.0; | |
m_accum_gyro_z = 0.0; | |
} | |
Timer.delay(kCalibrationSampleTime); | |
synchronized (this) { | |
m_gyro_offset_x = m_accum_gyro_x / m_accum_count; | |
m_gyro_offset_y = m_accum_gyro_y / m_accum_count; | |
m_gyro_offset_z = m_accum_gyro_z / m_accum_count; | |
} | |
//System.out.println("Calibrated now."); | |
} | |
static int ToUShort(ByteBuffer buf) { | |
return (buf.getShort(0)) & 0xFFFF; | |
} | |
static int ToUShort(int... data) { | |
ByteBuffer buf = ByteBuffer.allocateDirect(data.length); | |
for(int d : data) { | |
buf.put((byte)d); | |
} | |
return ToUShort(buf); | |
} | |
public static long ToULong(int sint) { | |
return sint & 0x00000000FFFFFFFFL; | |
} | |
public static int ToShort(int... buf) { | |
return (short)(((short)buf[0]) << 8 | buf[1]); | |
} | |
static int ToShort(ByteBuffer buf) { | |
return ToShort(buf.get(0), buf.get(1)); | |
} | |
public int readRegister(int reg) { | |
ByteBuffer buf = ByteBuffer.allocateDirect(2); | |
buf.order(ByteOrder.BIG_ENDIAN); | |
buf.put(0, (byte) (reg & 0x7f)); | |
buf.put(1, (byte) 0); | |
m_spi.write(buf, 2); | |
m_spi.read(false, buf, 2); | |
return ToUShort(buf); | |
} | |
public void writeRegister(int reg, int val) { | |
ByteBuffer buf = ByteBuffer.allocateDirect(2); | |
// low byte | |
buf.put(0, (byte)((0x80 | reg) | 0x10)); | |
buf.put(1, (byte) (val & 0xff)); | |
m_spi.write(buf, 2); | |
// high byte | |
buf.put(0, (byte) (0x81 | reg)); | |
buf.put(1, (byte) (val >> 8)); | |
m_spi.write(buf, 2); | |
} | |
/** | |
* {@inheritDoc} | |
*/ | |
public void reset() { | |
synchronized (this) { | |
m_integ_gyro_x = 0.0; | |
m_integ_gyro_y = 0.0; | |
m_integ_gyro_z = 0.0; | |
} | |
} | |
/** | |
* Delete (free) the spi port used for the IMU. | |
*/ | |
@Override | |
public void free() { | |
m_freed.set(true); | |
if (m_samples_mutex != null) { | |
m_samples_mutex.lock(); | |
try { | |
m_samples_not_empty.signal(); | |
} finally { | |
m_samples_mutex.unlock(); | |
} | |
} | |
try { | |
if (m_acquire_task != null) { | |
m_acquire_task.join(); | |
} | |
} catch (InterruptedException e) { | |
} | |
if (m_interrupt != null) { | |
m_interrupt.close(); | |
m_interrupt = null; | |
} | |
if (m_spi != null) { | |
m_spi.close(); | |
m_spi = null; | |
} | |
} | |
public void acquire() { | |
ByteBuffer readBuf = ByteBuffer.allocateDirect(64000); | |
readBuf.order(ByteOrder.LITTLE_ENDIAN); | |
double gyro_x, gyro_y, gyro_z, accel_x, accel_y, accel_z, temp, status, counter; | |
int data_count = 0; | |
int array_offset = 0; | |
int imu_checksum = 0; | |
double dt = 0; // This number must be adjusted if decimation setting is changed. Default is 1/102.4 SPS | |
int data_subset[] = new int[23]; | |
long timestamp_new = 0; | |
int data_to_read = 0; | |
while (!m_freed.get()) { | |
// Waiting for the buffer to fill... | |
Timer.delay(.020); // A delay less than 10ms could potentially overflow the local buffer | |
data_count = m_spi.readAutoReceivedData(readBuf,0,0); // Read number of bytes currently stored in the buffer | |
array_offset = data_count % 92; // Look for "extra" data This is 92 not 23 like in C++ b/c everything is 32-bits and takes up 4 bytes in the buffer | |
data_to_read = data_count - array_offset; // Discard "extra" data | |
m_spi.readAutoReceivedData(readBuf,data_to_read,0); // Read data from DMA buffer | |
for(int i = 0; i < data_to_read; i += 92) { // Process each set of 23 bytes (timestamp + 23 data) * 4 (32-bit ints) | |
for(int j = 0; j < 23; j++) { // Split each set of 23 bytes into a sub-array for processing | |
data_subset[j] = readBuf.getInt(4 + (i + j) * 4); // (i + j) * 4 = position in buffer + 4 to skip timestamp | |
} | |
// Calculate checksum | |
int calc_checksum = 0; | |
for(int k = 2; k < 20; k++ ) { // Cycle through STATUS, XYZ GYRO, XYZ ACCEL, TEMP, COUNTER (Ignore DUT Checksum) | |
calc_checksum += data_subset[k]; | |
} | |
//System.out.print("Calc Checksum: " + calc_checksum + "\t"); | |
// This is the data needed for Checksum | |
ByteBuffer bBuf = ByteBuffer.allocateDirect(2); | |
bBuf.put((byte)readBuf.getInt((i + 20) * 4 + 4)); | |
bBuf.put((byte)readBuf.getInt((i + 21) * 4 + 4)); | |
//System.out.println(data_subset[20] + "," + data_subset[21]); | |
imu_checksum = ToUShort(bBuf); | |
//System.out.println("IMU Checksum: " + imu_checksum); | |
// Compare calculated vs read CRC. Don't update outputs if CRC-16 is bad | |
if(true||imu_checksum==calc_checksum) { //Change | |
// Calculate delta-time (dt) using FPGA timestamps | |
timestamp_new = ToULong(readBuf.getInt(i * 4)); | |
dt = (timestamp_new - timestamp_old)/1000000.0; // Calculate dt and convert us to seconds | |
timestamp_old = timestamp_new; // Store new timestamp in old variable for next cycle | |
gyro_x = ToShort(data_subset[4], data_subset[5]) * kDegreePerSecondPerLSB; | |
gyro_y = ToShort(data_subset[6], data_subset[7]) * kDegreePerSecondPerLSB; | |
gyro_z = ToShort(data_subset[8], data_subset[9]) * kDegreePerSecondPerLSB; | |
accel_x = ToShort(data_subset[10], data_subset[11]) * kGPerLSB; | |
accel_y = ToShort(data_subset[12], data_subset[13]) * kGPerLSB; | |
accel_z = ToShort(data_subset[14], data_subset[15]) * kGPerLSB; | |
temp = ToShort(data_subset[16], data_subset[17]) * kDegCPerLSB + kDegCOffset; | |
status = ToUShort(data_subset[2], data_subset[3]); | |
counter = ToUShort(data_subset[18], data_subset[19]); | |
// Update global state | |
synchronized(this){ | |
m_gyro_x = gyro_x; | |
m_gyro_y = gyro_y; | |
m_gyro_z = gyro_z; | |
m_accel_x = accel_x; | |
m_accel_y = accel_y; | |
m_accel_z = accel_z; | |
m_temp = temp; | |
m_status = status; | |
m_counter = counter; | |
this.dt = dt; | |
++m_accum_count; | |
m_accum_gyro_x += gyro_x; | |
m_accum_gyro_y += gyro_y; | |
m_accum_gyro_z += gyro_z; | |
m_integ_gyro_x += (gyro_x - m_gyro_offset_x) * dt; | |
m_integ_gyro_y += (gyro_y - m_gyro_offset_y) * dt; | |
m_integ_gyro_z += (gyro_z - m_gyro_offset_z) * dt; | |
} | |
}else{ | |
System.out.println("Invalid checksum"); | |
} | |
} | |
} | |
} | |
/** | |
* {@inheritDoc} | |
*/ | |
public double getAngle() { | |
switch (m_yaw_axis) { | |
case kX: | |
return getAngleX(); | |
case kY: | |
return getAngleY(); | |
case kZ: | |
return getAngleZ(); | |
} | |
return 0.0; | |
} | |
/** | |
* {@inheritDoc} | |
*/ | |
public double getRate() { | |
switch (m_yaw_axis) { | |
case kX: | |
return getRateX(); | |
case kY: | |
return getRateY(); | |
case kZ: | |
return getRateZ(); | |
} | |
return 0.0; | |
} | |
public synchronized double getAngleX() { | |
return m_integ_gyro_x; | |
} | |
public synchronized double getAngleY() { | |
return m_integ_gyro_y; | |
} | |
public synchronized double getAngleZ() { | |
return m_integ_gyro_z; | |
} | |
public synchronized double getRateX() { | |
return m_gyro_x; | |
} | |
public synchronized double getRateY() { | |
return m_gyro_y; | |
} | |
public synchronized double getRateZ() { | |
return m_gyro_z; | |
} | |
public synchronized double getAccelX() { | |
return m_accel_x; | |
} | |
public synchronized double getAccelY() { | |
return m_accel_y; | |
} | |
public synchronized double getAccelZ() { | |
return m_accel_z; | |
} | |
public synchronized double getTemperature() { | |
return m_temp; | |
} | |
public synchronized double getStatus() { | |
return m_status; | |
} | |
public synchronized double getCounter(){ | |
return m_counter; | |
} | |
public synchronized double getdt(){ | |
return dt; | |
} | |
/** | |
* {@inheritDoc} | |
*/ | |
@Override | |
public void initSendable(SendableBuilder builder) { | |
builder.setSmartDashboardType("ADIS16470 IMU"); | |
NetworkTableInstance tb = builder.getEntry("GyroX").getInstance(); | |
int gyroX = builder.getEntry("GyroX").getHandle(); | |
int gyroY = builder.getEntry("GyroY").getHandle(); | |
int gyroZ = builder.getEntry("GyroZ").getHandle(); | |
int accelX = builder.getEntry("AccelX").getHandle(); | |
int accelY = builder.getEntry("AccelY").getHandle(); | |
int accelZ = builder.getEntry("AccelZ").getHandle(); | |
int angleX = builder.getEntry("AngleX").getHandle(); | |
int angleY = builder.getEntry("AngleY").getHandle(); | |
int angleZ = builder.getEntry("AngleZ").getHandle(); | |
int temp = builder.getEntry("Temperature").getHandle(); | |
int dt = builder.getEntry("dt").getHandle(); | |
int status = builder.getEntry("Status").getHandle(); | |
int counter = builder.getEntry("Counter").getHandle(); | |
builder.setUpdateTable(new Runnable(){ | |
@Override | |
public void run(){ | |
new NetworkTableEntry(tb, gyroX).setDouble(getRateX()); | |
new NetworkTableEntry(tb, gyroY).setDouble(getRateY()); | |
new NetworkTableEntry(tb, gyroZ).setDouble(getRateZ()); | |
new NetworkTableEntry(tb, accelX).setDouble(getAccelX()); | |
new NetworkTableEntry(tb, accelY).setDouble(getAccelY()); | |
new NetworkTableEntry(tb, accelZ).setDouble(getAccelZ()); | |
new NetworkTableEntry(tb, angleX).setDouble(getAngleX()); | |
new NetworkTableEntry(tb, angleY).setDouble(getAngleY()); | |
new NetworkTableEntry(tb, angleZ).setDouble(getAngleZ()); | |
new NetworkTableEntry(tb, temp).setDouble(getTemperature()); | |
new NetworkTableEntry(tb, dt).setDouble(getdt()); | |
new NetworkTableEntry(tb, status).setDouble(getStatus()); | |
new NetworkTableEntry(tb, counter).setDouble(getCounter()); | |
} | |
}); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment