Revision 0:73d3025fc18b, committed 2020-02-21

Comitter:

Rita Yang

Date:

Fri Feb 21 00:18:37 2020 -0800

Commit message:

Adding source library files.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADIS16467.cpp	Fri Feb 21 00:18:37 2020 -0800
@@ -0,0 +1,812 @@
+/* * @author Rita Yang
+     * @filename ADIS16467.cpp
+     * * @section DESCRIPTION
+     * * Software Driver for ADIS16467-3 Precision MEMS IMU Module
+     * * Datasheet:
+     * * https://www.analog.com/media/en/technical-documentation/data-sheets/adis16467.pdf
+ */
+
+//Output Register Memory Map (Table 8)
+//==================================
+#define DIAG_STAT                    0x02 //R, output, system error flag
+#define X_GYRO_LOW                   0x04 //R, output, x-axis gyro, low word
+#define X_GYRO_OUT                   0x06 //R, output, x-axis gyro, high word
+#define Y_GYRO_LOW                   0x08 //R, output, y-axis gyro, low word
+#define Y_GYRO_OUT                   0x0A //R, output, y-axis gyro, high word
+#define Z_GYRO_LOW                   0x0C //R, output, z-axis gyro, low word
+#define Z_GYRO_OUT                   0x0E //R, output, z-axis gyro, high word
+#define X_ACCL_LOW                   0x10 //R, output, x-axis accel, low word
+#define X_ACCL_OUT                   0x12 //R, output, x-axis accel, high word
+#define Y_ACCL_LOW                   0x14 //R, output, y-axis accel, low word
+#define Y_ACCL_OUT                   0x16 //R, output, y-axis accel, high word
+#define Z_ACCL_LOW                   0x18 //R, output, z-axis accel, low word
+#define Z_ACCL_OUT                   0x1A //R, output, z-axis accel, high word
+#define TEMP_OUT                     0x1C //R, output, temperature
+#define TIME_STAMP                   0x1E //R, output, time stamp
+#define DATA_CNTR                    0x22 //R, output, new data counter
+#define X_DELTANG_LOW                0x24 //R, output, x-axis delta angle, low word
+#define X_DELTANG_OUT                0x26 //R, output, x-axis delta angle, high word
+#define Y_DELTANG_LOW                0x28 //R, output, y-axis delta angle, low word
+#define Y_DELTANG_OUT                0x2A //R, output, y-axis delta angle, high word
+#define Z_DELTANG_LOW                0x2C //R, output, z-axis delta angle, low word
+#define Z_DELTANG_OUT                0x2E //R, output, z-axis delta angle, high word
+#define X_DELTVEL_LOW                0x30 //R, output, x-axis delta velocity, low word
+#define X_DELTVEL_OUT                0x32 //R, output, x-axis delta velocity, high word
+#define Y_DELTVEL_LOW                0x34 //R, output, y-axis delta velocity, low word
+#define Y_DELTVEL_OUT                0x36 //R, output, y-axis delta velocity, high word
+#define Z_DELTVEL_LOW                0x38 //R, output, z-axis delta velocity, low word
+#define Z_DELTVEL_OUT                0x3A //R, output, z-axis delta velocity, high word
+#define FLSHCNT_LOW                  0x7C //R, output, flash memory write cycle counter, lower word
+#define FLSHCNT_HIGH                 0x7E //R, output, flash memory write cycle counter, upper word
+
+//Calibration Registers (Table 8)
+//==================================
+#define XG_BIAS_LOW_1                0x40 //R/W, calib, offset, gyro, x-axis, low word, lower byte
+#define XG_BIAS_LOW_2				 0x41 // higher byte
+#define XG_BIAS_HIGH_1               0x42 //R/W, calib, offset, gyro, x-axis, high word, lower byte
+#define XG_BIAS_HIGH_2				 0x43 // higher byte
+#define YG_BIAS_LOW_1                0x44 //R/W, calib, offset, gyro, y-axis, low word, lower byte
+#define YG_BIAS_LOW_2				 0x45 // higher byte
+#define YG_BIAS_HIGH_1               0x46 //R/W, calib, offset, gyro, y-axis, high word, lower byte
+#define YG_BIAS_HIGH_2				 0x47 // higher byte
+#define ZG_BIAS_LOW_1                0x48 //R/W, calib, offset, gyro, z-axis, low word, lower byte
+#define ZG_BIAS_LOW_2				 0x49 // higher byte
+#define ZG_BIAS_HIGH_1               0x4A //R/W, calib, offset, gyro, z-axis, high word, lower byte
+#define ZG_BIAS_HIGH_2				 0x4B // higher byte
+#define XA_BIAS_LOW_1                0x4C //R/W, calib, offset, accel, x-axis, low word, lower byte
+#define XA_BIAS_LOW_2				 0x4D // higher byte
+#define XA_BIAS_HIGH_1               0x4E //R/W, calib, offset, accel, x-axis, high word, lower byte
+#define XA_BIAS_HIGH_2				 0x4F // higher byte
+#define YA_BIAS_LOW_1                0x50 //R/W, calib, offset, accel, y-axis, low word, lower byte
+#define YA_BIAS_LOW_2				 0x51 // higher byte
+#define YA_BIAS_HIGH_1               0x52 //R/W, calib, offset, accel, y-axis, high word, lower byte
+#define YA_BIAS_HIGH_2				 0x53 // higher byte
+#define ZA_BIAS_LOW_1                0x54 //R/W, calib, offset, accel, z-axis, low word, lower byte
+#define ZA_BIAS_LOW_2				 0x55 // higher byte
+#define ZA_BIAS_HIGH_1               0x56 //R/W, calib, offset, accel, z-axis, high word, lower byte
+#define ZA_BIAS_HIGH_2				 0x57 // higher byte
+
+//Control Registers
+//==================================
+#define FILT_CTRL_1                  0x5C //R/W, control, Bartlett window FIR filter, lower byte
+#define FILT_CTRL_2					 0x5D // higher byte
+#define RANG_MDL                     0x5E //R, measurement range (model specific) identifier
+#define MSC_CTRL_1                   0x60 //R/W, control, input/output & other, lower byte
+#define MSC_CTRL_2					 0x61 // higher byte
+#define UP_SCALE_1                   0x62 //R/W, control, scale factor for input clock, PPS mode, lower byte
+#define UP_SCALE_2					 0x63 // higher byte
+#define DEC_RATE_1                   0x64 //R/W, control, decimation filter (output data rate), lower byte
+#define DEC_RATE_2					 0x65 // higher byte
+#define NULL_CNFG_1                  0x66 //R/W, control, bias estimation period, lower byte
+#define NULL_CNFG_2					 0x67 // higher byte
+#define GLOB_CMD_1                   0x68 //W, control, global commands, lower byte
+#define GLOB_CMD_2					 0x69 // higher byte
+
+//Identification
+//==================================
+#define FIRM_REV                     0x6C //R, id, firmware revision
+#define FIRM_DM                      0x6E //R, id, date code, day and month
+#define FIRM_Y                       0x70 //R, id, date code, year
+#define PROD_ID                      0x72 //R, id, device number
+#define SERIAL_NUM                   0x74 //R, id, serial number
+#define USER_SCR_1_LOW               0x76 //R/W, user scratch register 1, lower byte
+#define USER_SCR_1_HIGH				 0x77 // higher byte
+#define USER_SCR_2_LOW               0x78 //R/W, user scratch register 2, lower byte
+#define USER_SCR_2_HIGH				 0x79 // higher byte
+#define USER_SCR_3_LOW               0x7A //R/W, user scratch register 3, lower byte
+#define USER_SCR_3_HIGH				 0x7B // higher byte
+
+#define SPI_FREQ 1000000 //supports burst read (<= 1 MHz)
+#define BURST_READ_COMMAND 0x6800
+
+#include <inttypes.h>
+
+#include "ADIS16467.h"
+
+namespace ADIS16467 {
+	/* Constructor
+	 * Set all error flag variables to false
+	 * *    @param        DIN         | SPI data input, clocks on SCLK rising edge (MOSI pin)
+	 * @param        DOUT        | SPI data output, clocks on SCLK falling edge (MISO pin)
+	 * @param        SCLK        | Clock pin
+	 * @param        CS             | Chip Select
+	 */
+	ADIS16467::ADIS16467(Serial *debug, PinName DIN, PinName DOUT, PinName SCLK, PinName CS, PinName RST):
+		debugPort(debug), spi(DIN, DOUT, SCLK), cs(CS), rst(RST) {
+	}
+
+	/* ADIS16467::initADIS
+	 * Initialize ADIS16467. NEED to be called after each reset 
+	 * of the chip.
+	 */
+	void ADIS16467::initADIS() {
+		//reset ADIS
+		rst.write(0);
+		wait_us(10);
+
+		resetTimer.reset();
+		rst.write(1);
+		resetTimer.start(); //start the reset timer
+		spi.frequency(SPI_FREQ);
+		spi.format(16, 3); //transmits 16 bits at a time, mode 3, master
+
+		begin();
+	}
+
+	/* ADIS16467::readResetTimer()
+	 * Return the time passed in mili-seconds. Should be called before
+	 * calling the initADIS().
+	 */
+	uint32_t ADIS16467::readResetTimer() {
+		return resetTimer.read_ms();
+	}
+
+	/* ADIS16467::readStatDiag
+	 * Reading the DIAG_STAT Register and returns its value.
+	 * Reading this register will cause all of its bits to
+	 * reset to 0.
+	 */
+	uint16_t ADIS16467::readStatDiag() {
+		return readRegister(DIAG_STAT);
+	}
+
+	/* ADIS16467::setErrorFlags
+	 * Taking in a given reading from the DIAG_STAT Register
+	 * (ex. from readStatDiag function or the burstRead Function)
+	 * and set error flags respectively. The error flags will hold
+	 * their value, until cleared, so that if an error occurs, it
+	 * will not be overridden until it has been read.
+	 * Refer to Table 10 in datasheet
+	 */
+	void ADIS16467::setErrorFlags(uint16_t statusDiagVal) {
+		clockError |= static_cast<bool>(statusDiagVal & (1 << 7));
+		memoryFailure |= static_cast<bool>(statusDiagVal & (1 << 6));
+		sensorFailure |= static_cast<bool>(statusDiagVal & (1 << 5));
+		standbyMode |= static_cast<bool>(statusDiagVal & (1 << 4));
+		comSPIError |= static_cast<bool>(statusDiagVal & (1 << 3));
+		flashMemUpdateFailure |= static_cast<bool>(statusDiagVal & (1 << 2));
+		datapathOverrun |= static_cast<bool>(statusDiagVal & (1 << 1));
+	}
+
+	/* ADIS16467::getClockError
+	 * Returns true if the clockError flag has been set
+	 */
+	bool ADIS16467::getClockError() {
+		return clockError;
+	}
+
+	/* ADIS16467::getMemFailure
+	 * Returns true if the memoryFailure flag has been set
+	 */
+	bool ADIS16467::getMemFailure() {
+		return memoryFailure;
+	}
+
+	/* ADIS16467::getSensorFailure
+	 * Returns true if the sensorFailure flag has been set
+	 */
+	bool ADIS16467::getSensorFailure() {
+		return sensorFailure;
+	}
+
+	/* ADIS16467::getStandbyMode
+	 * Returns true if the standbyMode flag has been set
+	 */
+	bool ADIS16467::getStandbyMode() {
+		return standbyMode;
+	}
+
+	/* ADIS16467::getSPIError
+	 * Returns true if the comSPIError flag has been set
+	 */
+	bool ADIS16467::getSPIError() {
+		return comSPIError;
+	}
+
+	/* ADIS16467::getFlashUpFail
+	 * Returns true if the flashMemUpdateFailure flag has been set
+	 */
+	bool ADIS16467::getFlashUpFail() {
+		return flashMemUpdateFailure;
+	}
+
+	/* ADIS16467::getDatapathOverrun
+	 * Returns true if the datapathOverrun flag has been set
+	 */
+	bool ADIS16467::getDatapathOverrun() {
+		return datapathOverrun;
+	}
+
+	/**
+	 * ADIS16467::checkExistence
+	 * Reads the PROD_ID register to check if it is connected
+	 * and working
+	 * Return true if the PROD_ID is 16467
+	 * 16467 is device number from page 28 of the data sheet
+	 */
+	bool ADIS16467::checkExistence()
+	{
+		return (readRegister(PROD_ID)== 16467);
+	}
+
+	/* ADIS16467::resetAllFlags
+	 * Reset all the private variables that
+	 * are used to track the error flags
+	 */
+	void ADIS16467::resetAllFlags() {
+		clockError = false;
+		memoryFailure = false;
+		sensorFailure = false;
+		standbyMode = false;
+		comSPIError = false;
+		flashMemUpdateFailure = false;
+		datapathOverrun = false;
+	}
+
+	/* ADIS16467::readGyroX
+	 * Read both the high and low words of
+	 * the X-Axis Gyroscope, return the data
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::GYRO_CONV
+	 */
+	int32_t ADIS16467::readGyroX() {
+		return read32BitValue(X_GYRO_OUT, X_GYRO_LOW);
+	}
+
+	/* ADIS16467::readGyroY
+	 * Read both the high and low words of
+	 * the Y-Axis Gyroscope, return the data
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::GYRO_CONV
+	 */
+	int32_t ADIS16467::readGyroY() {
+		return read32BitValue(Y_GYRO_OUT, Y_GYRO_LOW);
+	}
+
+	/* ADIS16467::readGyroZ
+	 * Read both the high and low words of
+	 * the Z-Axis Gyroscope, return the data
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::GYRO_CONV
+	 */
+	int32_t ADIS16467::readGyroZ() {
+		return read32BitValue(Z_GYRO_OUT, Z_GYRO_LOW);
+	}
+
+	/* ADIS16467::readAcclX
+	 * Read both the high and low words of
+	 * the X-Axis Accelerometer, return the
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::ACCEL_CONV
+	 */
+	int32_t ADIS16467::readAccelX() {
+		return read32BitValue(X_ACCL_OUT, X_ACCL_LOW);
+	}
+
+	/* ADIS16467::readAcclY
+	 * Read both the high and low words of
+	 * the Y-Axis Accelerometer, return the
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::ACCEL_CONV
+	 */
+	int32_t ADIS16467::readAccelY() {
+		return read32BitValue(Y_ACCL_OUT, Y_ACCL_LOW);
+	}
+
+	/* ADIS16467::readAcclZ
+	 * Read both the high and low words of
+	 * the Z-Axis Accelerometer, return the
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::ACCEL_CONV
+	 */
+	int32_t ADIS16467::readAccelZ() {
+		return read32BitValue(Z_ACCL_OUT, Z_ACCL_LOW);
+	}
+
+	/* ADIS16467::readInternalTemp
+	 * Read the coarse measurement of the temperature
+	 * inside the ADIS16467 from the TEMP_OUT register,
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::TEMP_CONV
+	 */
+	uint16_t ADIS16467::readInternalTemp() {
+		return readRegister(TEMP_OUT);
+	}
+
+	/* ADIS16467::readTimeStamp
+	 * Read the time from the last pulse on the SYNC
+	 * pin from the TIME_STAMP register, which works in
+	 * conjunction with Scaled Sync Mode.
+	 * To get a human readable format, multiply by
+	 * ADIS16467::TIME_CONV
+	 */
+	uint16_t ADIS16467::readTimeStamp() {
+		return readRegister(TIME_STAMP);
+	}
+
+	/* ADIS16467::readDataCounter
+	 * Read the DATA_CNTR register, which increments every
+	 * time new data loads into the output registers; once
+	 * the value reaches 0xFFFF it will wrap back to 0x0000
+	 */
+	uint16_t ADIS16467::readDataCounter() {
+		return readRegister(DATA_CNTR);
+	}
+
+	/* ADIS16467::burstRead
+	 * Utilizing the burst read function to read registers:
+	 * DIAG_STAT, all gyros axes, all accel axes, TEMP_OUT,
+	 * and DATA_CNTR. In this function, only the high word of
+	 * the gyros and accel will be read (16-bit value). A
+	 * 32-bit Checksum value will also be given at the end
+	 * of the read.
+	 * If the chip is not ready, an error message will be printed.
+	 */
+	void ADIS16467::burstRead(BurstReadResult &newResult) {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		spi.write(BURST_READ_COMMAND);
+		uint16_t rxBuf[10];
+
+		for(int i=0; i < 10; i++) {
+			rxBuf[i] = static_cast<uint16_t>(spi.write(0));
+		}
+
+		uint16_t verify = 0;
+		for(int i = 0; i < 9; i++) {
+			uint16_t temp = rxBuf[i];
+			verify += (temp & 0x00FF); //add in [7:0]
+			verify += ((temp >> 8) & 0x00FF); //add in [15:8]
+		}
+		if(verify != rxBuf[9]) {
+			debugPort->printf("Wrong checksum value\r\n");
+			return;
+		}
+
+		//reading the values into each variable respectively
+		newResult.statusDiag = rxBuf[0];
+		newResult.gyroX = static_cast<int32_t>(rxBuf[1]) << 16;
+		newResult.gyroY = static_cast<int32_t>(rxBuf[2]) << 16;
+		newResult.gyroZ = static_cast<int32_t>(rxBuf[3]) << 16;
+		newResult.accelX = static_cast<int32_t>(rxBuf[4]) << 16;
+		newResult.accelY = static_cast<int32_t>(rxBuf[5]) << 16;
+		newResult.accelZ = static_cast<int32_t>(rxBuf[6]) << 16;
+		newResult.internalTemp = rxBuf[7];
+		newResult.dataCounter = rxBuf[8];
+		newResult.checkSum = rxBuf[9];
+	}
+
+	/* ADIS16467::setPollRate
+	 * Takes in the desired polling rate in Hz, calculates the corresponding
+	 * decimation factor and applies the decimation filter; the polling rate
+	 * cannont exceed 2000 or below 1
+	 */
+	void ADIS16467::setPollRate(uint16_t hz) {
+		if(hz > 2000)
+			hz = 2000;
+		if(hz <= 0)
+			hz = 1;
+
+		uint16_t decRateVal = 2000 / hz - 1;
+		curr_DecRate = decRateVal;
+		
+		writeConfigurationRegister(DEC_RATE_1, DEC_RATE_2, decRateVal);
+	}
+
+	/* ADIS16467::sensorSelfTest()
+	 * Write a 1 to GLOB_CMD Register Bit 2. An internal
+	 * test routine is performed by the device. A pass or 
+	 * fail result will be reported to Register DIAG_STAT, 
+	 * Bit 5. The error flags will be automatically checked 
+	 * after the function is called, no need to call 
+	 * setErrorFlags(uint16_t) again, can directly call
+	 * getSensorFailure() to see the result.
+	 */
+	void ADIS16467::sensorSelfTest() {
+		writeConfigurationRegister(GLOB_CMD_1, GLOB_CMD_2, (1 << 2));
+	}
+
+	/* ADIS16467::updateDeltaAngles()
+	 * Call on read functions for all three Delta Angles
+	 * to retrieve new values and to update the three Delta
+	 * Angle Sum variables
+	 *
+	 * @return true if the angles were updated, false if there
+	 *  has not been any new data yet.
+	 */
+	bool ADIS16467::updateDeltaAngles() {
+		int32_t curr_DC = readDataCounter();
+		//check for data counter roll overs
+		if(curr_DC < lastDC_DeltaAngle) {
+			lastDC_DeltaAngle -= 0xFFFF;
+		}
+
+		if((curr_DC - lastDC_DeltaAngle) > 1)
+			debugPort->printf("Delta Angles polling too slow, missed %" PRIi32 " updates\n\r", curr_DC - lastDC_DeltaAngle);
+		else if((curr_DC - lastDC_DeltaAngle) < 1) {
+			// no new data
+			return false;
+		}
+
+		readDeltaAngleX();
+		readDeltaAngleY();
+		readDeltaAngleZ();
+
+		lastDC_DeltaAngle = curr_DC;
+
+		return true;
+	}
+
+	/* ADIS16467::getDeltaAngleXSum()
+	 * Return the cumulative sum value of X-axis Delta Angles
+	 * from all the calls of updateDeltaAngles().
+	 * To get a human readable result, multiply by 
+	 * ADIS16467::DELTA_ANGLE_CONV
+	 * If the chip is not ready yet, 0 will be returned with an 
+	 * error message.
+	 */
+	int64_t ADIS16467::getDeltaAngleXSum() {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		return deltaAngleX_sum;
+	}
+
+	/* ADIS16467::getDeltaAngleYSum()
+	 * Return the cumulative sum value of Y-axis Delta Angles
+	 * from all the calls of updateDeltaAngles().
+	 * To get a human readable result, multiply by 
+	 * ADIS16467::DELTA_ANGLE_CONV
+	 * If the chip is not ready yet, 0 will be returned with an 
+	 * error message.
+	 */
+	int64_t ADIS16467::getDeltaAngleYSum() {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		return deltaAngleY_sum;
+	}
+
+	/* ADIS16467::getDeltaAngleZSum()
+	 * Return the cumulative sum value of Z-axis Delta Angles
+	 * from all the calls of updateDeltaAngles().
+	 * To get a human readable result, multiply by 
+	 * ADIS16467::DELTA_ANGLE_CONV
+	 * If the chip is not ready yet, 0 will be returned with an 
+	 * error message.
+	 */
+	int64_t ADIS16467::getDeltaAngleZSum() {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		return deltaAngleZ_sum;
+	}
+
+	/* ADIS16467::updateDeltaVels()
+	 * Call on read functions for all three Delta Velocity
+	 * to retrieve new values and to update the three Delta
+	 * Velocity Sum variables
+	 */
+	void ADIS16467::updateDeltaVels() {
+		int32_t curr_DC = readDataCounter();
+		//check for data counter roll overs
+		if(curr_DC < lastDC_DeltaVel) {
+			lastDC_DeltaVel -= 0xFFFF;
+		}
+
+		if((curr_DC - lastDC_DeltaVel) > 1)
+			debugPort->printf("Delta Velocity polling too slow\n\r");
+		else if((curr_DC - lastDC_DeltaVel) < 1) {
+			debugPort->printf("Delta Velocity polling too fast\n\r");
+			return;
+		}
+
+		readDeltaVelX();
+		readDeltaVelY();
+		readDeltaVelZ();
+
+		lastDC_DeltaVel = curr_DC;
+	}
+
+	/* ADIS16467::getDeltaVelXSum()
+	 * Return the cumulative sum value of X-axis Delta Velocity
+	 * from all the calls of updateDeltaVels().
+	 * To get a human readable result, multiply by 
+	 * ADIS16467::DELTA_VEL_CONV
+	 * If the chip is not ready yet, 0 will be returned with an 
+	 * error message.
+	 */
+	int64_t ADIS16467::getDelVelXSum() {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		return deltaVelocityX_sum;
+	}
+	
+	/* ADIS16467::getDeltaVelYSum()
+	 * Return the cumulative sum value of Y-axis Delta Velocity
+	 * from all the calls of updateDeltaVels().
+	 * To get a human readable result, multiply by 
+	 * ADIS16467::DELTA_VEL_CONV
+	 * If the chip is not ready yet, 0 will be returned with an 
+	 * error message.
+	 */
+	int64_t ADIS16467::getDelVelYSum() {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		return deltaVelocityY_sum;
+	}
+	
+	/* ADIS16467::getDeltaVelZSum()
+	 * Return the cumulative sum value of Z-axis Delta Velocity
+	 * from all the calls of updateDeltaVels().
+	 * To get a human readable result, multiply by 
+	 * ADIS16467::DELTA_VEL_CONV
+	 * If the chip is not ready yet, 0 will be returned with an 
+	 * error message.
+	 */
+	int64_t ADIS16467::getDelVelZSum() {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		return deltaVelocityZ_sum;
+	}
+
+	/**
+	 * Set the gyro calibration biases.  Arguments are in degrees per second.
+	 * As would be logical, a positive bias value increases the gyro's
+	 * reading in that axis (the register setting is the other way).
+	 *
+	 * @param xBias
+	 * @param yBias
+	 * @param zBias
+	 */
+	void ADIS16467::setGyroBiases(float xBias, float yBias, float zBias)
+	{
+		int32_t xBiasRegister = static_cast<int32_t>((-1 * xBias) / GYRO_CONV);
+		int32_t yBiasRegister = static_cast<int32_t>((-1 * yBias) / GYRO_CONV);
+		int32_t zBiasRegister = static_cast<int32_t>((-1 * zBias) / GYRO_CONV);
+
+		writeConfigurationRegister(XG_BIAS_LOW_1, XG_BIAS_LOW_2, static_cast<int16_t>(xBiasRegister & 0xFFFF));
+		writeConfigurationRegister(XG_BIAS_HIGH_1, XG_BIAS_HIGH_2, static_cast<int16_t>(xBiasRegister >> 16));
+
+		writeConfigurationRegister(YG_BIAS_LOW_1, YG_BIAS_LOW_2, static_cast<int16_t>(yBiasRegister & 0xFFFF));
+		writeConfigurationRegister(YG_BIAS_HIGH_1, YG_BIAS_HIGH_2, static_cast<int16_t>(yBiasRegister >> 16));
+
+		writeConfigurationRegister(ZG_BIAS_LOW_1, ZG_BIAS_LOW_2, static_cast<int16_t>(zBiasRegister & 0xFFFF));
+		writeConfigurationRegister(ZG_BIAS_HIGH_1, ZG_BIAS_HIGH_2, static_cast<int16_t>(zBiasRegister >> 16));
+
+	}
+
+	/* ADIS16467::getFirmwareInformation
+	 * Read the firmware revision, date and month, year, and
+	 * serial number.
+	 * All the values retrieved are in the format where
+	 * each 4 bits represents one number.
+	*/
+	void ADIS16467::getFirmwareInformation(FirmwareInfo &info) {
+		uint16_t firmRev = readRegister(FIRM_REV);
+
+		info.firmRevMajor = static_cast<uint8_t>(firmRev >> 8);
+		info.firmRevMinor = static_cast<uint8_t>(firmRev & 0x00FF);
+
+		uint16_t firmDM = readRegister(FIRM_DM);
+
+		info.firmwareMonth = static_cast<uint8_t>((firmDM >> 8 & 0xF) + (firmDM >> 12 & 0xF) * 10);
+		info.firmwareDay = static_cast<uint8_t>((firmDM & 0xF) + (firmDM >> 4 & 0xF) * 10);
+
+		uint16_t firmY = readRegister(FIRM_Y);
+
+		info.firmwareYear = static_cast<uint16_t>((firmY & 0xF) + (firmY >> 4 & 0xF) * 10 + (firmY >> 8 & 0xF) * 100 + (firmY >> 12 & 0xF) * 1000);
+
+		info.serialNum = readRegister(SERIAL_NUM);
+	}
+
+	/* ADIS16467::resetDeltaAngle
+	 * Update lastDC_DeltaAngle to the current value in the data counter
+	 * register. Reset all Delta Angle sums to 0.
+	 */
+	void ADIS16467::resetDeltaAngle() {
+		lastDC_DeltaAngle = readDataCounter();
+		deltaAngleX_sum = 0;
+		deltaAngleY_sum = 0;
+		deltaAngleZ_sum = 0;
+	}
+
+	/* ADIS16467::resetDeltaVel
+	 * Update lastDC_DeltaVel to the current value in the data counter
+	 * register. Reset all Delta Velocity sums to 0.
+	 */
+	void ADIS16467::resetDeltaVel() {
+		lastDC_DeltaVel = readDataCounter();
+		deltaVelocityX_sum = 0;
+		deltaVelocityY_sum = 0;
+		deltaVelocityZ_sum = 0;
+	}
+
+
+
+//-----------------------private-------------------------------------------
+
+	/* ADIS16467::readConfigurationRegister
+	 * Reads in the lower and upper byte addresses for a control register and 
+	 * the data you want to write to the register; writes the command to the 
+	 * corresponding control register to configure it
+	 */
+	void ADIS16467::writeConfigurationRegister(uint8_t addressLow,
+											   uint8_t addressHigh, int16_t data) {
+		uint16_t command1 = (1 << 15) | (addressLow << 8) | (data & 0xFF);
+		uint16_t command2 = (1 << 15) | (addressHigh << 8) | ((data >> 8) & 0xFF);
+
+		spi.write(command1);
+
+		wait_us(20);
+
+		spi.write(command2);
+
+		setErrorFlags(readStatDiag());
+	}
+
+	/* ADIS16467::readRegister
+	 * Reads the register at the given address.
+	 * If the chip is not ready, 0 will be returned along
+	 * with an error message
+	 */
+	uint16_t ADIS16467::readRegister(uint8_t address) {
+		if(readResetTimer() < 275) {
+			wait_ms(275 - readResetTimer());
+		}
+
+		uint16_t readCommand = static_cast<int>(address) << 8;
+		spi.write(readCommand);
+		
+		wait_us(20);
+
+		uint16_t result = static_cast<uint16_t>(spi.write(0));
+
+		return result;
+	}
+
+	/* ADIS16467::read32BitValue
+	 * Reads the values within two addresses given and combine them to 
+	 * return a 32 bit value
+	 */
+	int32_t ADIS16467::read32BitValue(uint8_t highAddress, uint8_t lowAddress) {
+		uint16_t lowWord, highWord;
+		highWord = readRegister(highAddress);
+		lowWord = readRegister(lowAddress);
+		uint32_t val2Complement = (static_cast<uint32_t>(highWord) << 16) | lowWord;
+		int32_t val = static_cast<int32_t>(val2Complement);
+
+		return val;
+	}
+
+	/* ADIS16467::readDeltaAngleX
+	 * Read both the high and low words of
+	 * the X-Axis DeltaAngle register, return 
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::DELTA_ANGLE_CONV
+	 */
+	int32_t ADIS16467::readDeltaAngleX() {
+		int32_t value = read32BitValue(X_DELTANG_OUT, X_DELTANG_LOW);
+//		value = value / (curr_DecRate + 1) * 2000;
+		deltaAngleX_sum += value;
+
+		return value;
+	}
+
+	/* ADIS16467::readDeltaAngleY
+	 * Read both the high and low words of
+	 * the Y-Axis DeltaAngle register, return 
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::DELTA_ANGLE_CONV
+	 */
+	int32_t ADIS16467::readDeltaAngleY() {
+		int32_t value = read32BitValue(Y_DELTANG_OUT, Y_DELTANG_LOW);
+//		value = value / (curr_DecRate + 1) * 2000;
+		deltaAngleY_sum += value;
+
+		return value;
+	}
+
+	/* ADIS16467::readDeltaAngleZ
+	 * Read both the high and low words of
+	 * the Z-Axis DeltaAngle register, return 
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::DELTA_ANGLE_CONV
+	 */
+	int32_t ADIS16467::readDeltaAngleZ() {
+		int32_t value = read32BitValue(Z_DELTANG_OUT, Z_DELTANG_LOW);
+//		value = value / (curr_DecRate + 1) * 2000;
+		deltaAngleZ_sum += value;
+
+		return value;
+	}
+
+	/* ADIS16467::readDeltaVelX
+	 * Read both the high and low words of the
+	 * X-Axis DeltaVelocity register, return 
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::DELTA_VEL_CONV 
+	 */
+	int32_t ADIS16467::readDeltaVelX() {
+		int32_t value = read32BitValue(X_DELTVEL_OUT, X_DELTVEL_LOW);
+		value = value / (curr_DecRate + 1) * 2000;
+		deltaVelocityX_sum += value;
+
+		return value;
+	}
+	
+	/* ADIS16467::readDeltaVelY
+	 * Read both the high and low words of the
+	 * Y-Axis DeltaVelocity register, return 
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::DELTA_VEL_CONV 
+	 */
+	int32_t ADIS16467::readDeltaVelY() {
+		int32_t value = read32BitValue(Y_DELTVEL_OUT, Y_DELTVEL_LOW);
+		value = value / (curr_DecRate + 1) * 2000;
+		deltaVelocityY_sum += value;
+
+		return value;
+	}
+	
+	/* ADIS16467::readDeltaVelZ
+	 * Read both the high and low words of the
+	 * Z-Axis DeltaVelocity register, return 
+	 * as a 2s complement 32-bit value in LSBs.
+	 * To get a human readable format, multiply
+	 * by ADIS16467::DELTA_VEL_CONV 
+	 */
+	int32_t ADIS16467::readDeltaVelZ() {
+		int32_t value = read32BitValue(Z_DELTVEL_OUT, Z_DELTVEL_LOW);
+		value = value / (curr_DecRate + 1) * 2000;
+		deltaVelocityZ_sum += value;
+
+		return value;
+	}
+
+	/* ADIS16467::begin
+	 * Reset all private variables. Called in initADIS.
+	 */
+	void ADIS16467::begin() {
+		clockError = false;
+		memoryFailure = false;
+		sensorFailure = false;
+		standbyMode = false;
+		comSPIError = false;
+		flashMemUpdateFailure = false;
+		datapathOverrun = false;
+		deltaAngleX_sum = 0;
+		deltaAngleY_sum = 0;
+		deltaAngleZ_sum = 0;
+		deltaVelocityX_sum = 0;
+		deltaVelocityY_sum = 0;
+		deltaVelocityZ_sum = 0;
+		lastDC_DeltaAngle = 0;
+		lastDC_DeltaVel = 0;
+		curr_DecRate = 1999; //factory default, 2000Hz
+	}
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ADIS16467.h	Fri Feb 21 00:18:37 2020 -0800
@@ -0,0 +1,139 @@
+/*
+ * @author Rita Yang
+ * @filename ADIS16467.h
+ *
+ * @section DESCRIPTION
+ *
+ * Software Driver for ADIS16467-3 Precision MEMS IMU Module
+ *
+ * Datasheet:
+ *
+ * https://www.analog.com/media/en/technical-documentation/data-sheets/adis16467.pdf
+ */  
+
+#ifndef ADIS16467_H
+#define ADIS16467_H
+
+#include "mbed.h"
+
+namespace ADIS16467
+{
+	typedef struct BurstReadResult
+	{
+		int32_t gyroX;
+		int32_t gyroY;
+		int32_t gyroZ;
+		int32_t accelX;
+		int32_t accelY;
+		int32_t accelZ;
+		uint16_t statusDiag;
+		uint16_t internalTemp;
+		uint16_t dataCounter;
+		uint16_t checkSum;
+	} BurstReadResult;
+
+	typedef struct FirmwareInfo
+	{
+		uint8_t firmRevMajor;
+		uint8_t firmRevMinor;
+
+		uint8_t firmwareDay;
+		uint8_t firmwareMonth;
+		uint16_t firmwareYear;
+		uint16_t serialNum;
+	} FirmwareInfo;
+
+	class ADIS16467
+	{
+		public:
+			ADIS16467(Serial *debug, PinName DIN, PinName DOUT, PinName SCLK, PinName CS, PinName RST);
+			void initADIS();
+
+			uint32_t readResetTimer();
+			uint16_t readStatDiag();
+			void setErrorFlags(uint16_t statusDiagVal);
+			bool getClockError();
+			bool getMemFailure();
+			bool getSensorFailure();
+			bool getStandbyMode();
+			bool getSPIError();
+			bool getFlashUpFail();
+			bool getDatapathOverrun();
+			bool checkExistence();
+			void resetAllFlags();
+			int32_t readGyroX();
+			int32_t readGyroY();
+			int32_t readGyroZ();
+			int32_t readAccelX();
+			int32_t readAccelY();
+			int32_t readAccelZ();
+			uint16_t readInternalTemp();
+			uint16_t readTimeStamp();
+			uint16_t readDataCounter();
+			void burstRead(BurstReadResult &newResult);
+			void setPollRate(uint16_t hz);
+			void sensorSelfTest();
+			bool updateDeltaAngles();
+			int64_t getDeltaAngleXSum();
+			int64_t getDeltaAngleYSum();
+			int64_t getDeltaAngleZSum();
+			void updateDeltaVels();
+			int64_t getDelVelXSum();
+			int64_t getDelVelYSum();
+			int64_t getDelVelZSum();
+			void setGyroBiases(float xBias, float yBias, float zBias);
+			void getFirmwareInformation(FirmwareInfo &info);
+			void resetDeltaAngle();
+			void resetDeltaVel();
+
+			//conversion factors
+			static constexpr float GYRO_CONV = 0.1f / (1 << 16); //10 LSB/degree/sec
+			static constexpr float ACCEL_CONV = 1.25f / (1 << 16); //1 LSB = 1.25 mg
+			static constexpr float TEMP_CONV = 0.1f; //1 LSB = 0.1 degree Celcius
+			static constexpr float TIME_CONV = 49.02f; //1 LSB = 49.02 us
+			static constexpr float DELTA_ANGLE_CONV =  2160.0f / (1 << 31); //degrees per LSB 	
+			static constexpr float DELTA_VEL_CONV = 400.0f / (1 << 15); //meters/sec per LSB
+
+		private:
+			Serial *debugPort;
+			SPI spi;
+			DigitalOut cs;
+			DigitalOut rst;
+			Timer resetTimer; //keep track of reset time
+			bool clockError;
+			bool memoryFailure;
+			bool sensorFailure;
+			bool standbyMode;
+			bool comSPIError;
+			bool flashMemUpdateFailure;
+			bool datapathOverrun;
+			void writeConfigurationRegister(uint8_t addressLow, uint8_t addressHigh, int16_t data);
+			uint16_t readRegister(uint8_t address);
+			int32_t read32BitValue(uint8_t highAddress, uint8_t lowAddress);
+			void begin();
+			void select();
+			void deselect();
+
+			int64_t deltaAngleX_sum;
+			int64_t deltaAngleY_sum;
+			int64_t deltaAngleZ_sum;
+			int32_t readDeltaAngleX();
+			int32_t readDeltaAngleY();
+			int32_t readDeltaAngleZ();
+			int32_t lastDC_DeltaAngle;
+
+			int64_t deltaVelocityX_sum;
+			int64_t deltaVelocityY_sum;
+			int64_t deltaVelocityZ_sum;
+			int32_t readDeltaVelX();
+			int32_t readDeltaVelY();
+			int32_t readDeltaVelZ();
+			int32_t lastDC_DeltaVel;
+
+			uint16_t curr_DecRate;
+	};
+}
+
+
+
+#endif