From ec2a455bc78aac70d0392a76e555abe3dafc07de Mon Sep 17 00:00:00 2001 From: thomasclark Date: Mon, 21 Jul 2014 08:31:16 -0400 Subject: [PATCH] Added HAL methods for using the built-in accelerometer Change-Id: I5372f5df9b29c546dab3913fcf983a7a9a5427dc --- hal/include/HAL/Accelerometer.hpp | 15 ++ hal/include/HAL/HAL.hpp | 1 + hal/lib/Athena/Accelerometer.cpp | 233 ++++++++++++++++++++++++++++++ 3 files changed, 249 insertions(+) create mode 100644 hal/include/HAL/Accelerometer.hpp create mode 100644 hal/lib/Athena/Accelerometer.cpp diff --git a/hal/include/HAL/Accelerometer.hpp b/hal/include/HAL/Accelerometer.hpp new file mode 100644 index 0000000000..103fb2a318 --- /dev/null +++ b/hal/include/HAL/Accelerometer.hpp @@ -0,0 +1,15 @@ +#pragma once + +enum AccelerometerRange { + kRange_2G = 0, + kRange_4G = 1, + kRange_8G = 2, +}; + +extern "C" { + void setAccelerometerActive(bool); + void setAccelerometerRange(AccelerometerRange); + double getAccelerometerX(); + double getAccelerometerY(); + double getAccelerometerZ(); +} diff --git a/hal/include/HAL/HAL.hpp b/hal/include/HAL/HAL.hpp index c88122f2d2..57063b6be1 100644 --- a/hal/include/HAL/HAL.hpp +++ b/hal/include/HAL/HAL.hpp @@ -12,6 +12,7 @@ #endif #include +#include "Accelerometer.hpp" #include "Analog.hpp" #include "CAN.hpp" #include "Compressor.hpp" diff --git a/hal/lib/Athena/Accelerometer.cpp b/hal/lib/Athena/Accelerometer.cpp new file mode 100644 index 0000000000..28bf2a55ff --- /dev/null +++ b/hal/lib/Athena/Accelerometer.cpp @@ -0,0 +1,233 @@ +#include "HAL/Accelerometer.hpp" + +#include "ChipObject.h" +#include +#include +#include + +// The 7-bit I2C address with a 0 "send" bit +static const uint8_t kSendAddress = (0x1c << 1) | 0; + +// The 7-bit I2C address with a 1 "receive" bit +static const uint8_t kReceiveAddress = (0x1c << 1) | 1; + +static const uint8_t kControlTxRx = 1; +static const uint8_t kControlStart = 2; +static const uint8_t kControlStop = 4; + +static tAccel *accel = 0; +static AccelerometerRange accelerometerRange; + +// Register addresses +enum Register { + kReg_Status = 0x00, + kReg_OutXMSB = 0x01, + kReg_OutXLSB = 0x02, + kReg_OutYMSB = 0x03, + kReg_OutYLSB = 0x04, + kReg_OutZMSB = 0x05, + kReg_OutZLSB = 0x06, + kReg_Sysmod = 0x0B, + kReg_IntSource = 0x0C, + kReg_WhoAmI = 0x0D, + kReg_XYZDataCfg = 0x0E, + kReg_HPFilterCutoff = 0x0F, + kReg_PLStatus = 0x10, + kReg_PLCfg = 0x11, + kReg_PLCount = 0x12, + kReg_PLBfZcomp = 0x13, + kReg_PLThsReg = 0x14, + kReg_FFMtCfg = 0x15, + kReg_FFMtSrc = 0x16, + kReg_FFMtThs = 0x17, + kReg_FFMtCount = 0x18, + kReg_TransientCfg = 0x1D, + kReg_TransientSrc = 0x1E, + kReg_TransientThs = 0x1F, + kReg_TransientCount = 0x20, + kReg_PulseCfg = 0x21, + kReg_PulseSrc = 0x22, + kReg_PulseThsx = 0x23, + kReg_PulseThsy = 0x24, + kReg_PulseThsz = 0x25, + kReg_PulseTmlt = 0x26, + kReg_PulseLtcy = 0x27, + kReg_PulseWind = 0x28, + kReg_ASlpCount = 0x29, + kReg_CtrlReg1 = 0x2A, + kReg_CtrlReg2 = 0x2B, + kReg_CtrlReg3 = 0x2C, + kReg_CtrlReg4 = 0x2D, + kReg_CtrlReg5 = 0x2E, + kReg_OffX = 0x2F, + kReg_OffY = 0x30, + kReg_OffZ = 0x31 +}; + +extern "C" uint32_t getFPGATime(int32_t *status); + +static void writeRegister(Register reg, uint8_t data); +static uint8_t readRegister(Register reg); + +/** + * Initialize the accelerometer. + */ +static void initializeAccelerometer() { + int32_t status; + + if(!accel) { + accel = tAccel::create(&status); + + // Enable I2C + accel->writeCNFG(1, &status); + + // Set the counter to 100 kbps + accel->writeCNTR(213, &status); + + // The device identification number should be 0x2a + assert(readRegister(kReg_WhoAmI) == 0x2a); + } +} + +static void writeRegister(Register reg, uint8_t data) { + int32_t status = 0; + uint32_t initialTime; + + accel->writeADDR(kSendAddress, &status); + + // Send a start transmit/receive message with the register address + accel->writeCNTL(kControlStart | kControlTxRx, &status); + accel->writeDATO(reg, &status); + accel->strobeGO(&status); + + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while(accel->readSTAT(&status) & 1) { + if(getFPGATime(&status) > initialTime + 1000) break; + } + + // Send a stop transmit/receive message with the data + accel->writeCNTL(kControlStop | kControlTxRx, &status); + accel->writeDATO(data, &status); + accel->strobeGO(&status); + + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while(accel->readSTAT(&status) & 1) { + if(getFPGATime(&status) > initialTime + 1000) break; + } + + fflush(stdout); +} + +static uint8_t readRegister(Register reg) { + int32_t status = 0; + uint32_t initialTime; + + // Send a start transmit/receive message with the register address + accel->writeADDR(kSendAddress, &status); + accel->writeCNTL(kControlStart | kControlTxRx, &status); + accel->writeDATO(reg, &status); + accel->strobeGO(&status); + + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while(accel->readSTAT(&status) & 1) { + if(getFPGATime(&status) > initialTime + 1000) break; + } + + // Receive a message with the data and stop + accel->writeADDR(kReceiveAddress, &status); + accel->writeCNTL(kControlStart | kControlStop | kControlTxRx, &status); + accel->strobeGO(&status); + + // Execute and wait until it's done (up to a millisecond) + initialTime = getFPGATime(&status); + while(accel->readSTAT(&status) & 1) { + if(getFPGATime(&status) > initialTime + 1000) break; + } + + fflush(stdout); + + return accel->readDATI(&status); +} + +/** + * Convert a 12-bit raw acceleration value into a scaled double in units of + * 1 g-force, taking into account the accelerometer range. + */ +static double unpackAxis(int16_t raw) { + // The raw value is actually 12 bits, not 16, so we need to propogate the + // 2's complement sign bit to the unused 4 bits for this to work with + // negative numbers. + raw <<= 4; + raw >>= 4; + + switch(accelerometerRange) { + case kRange_2G: return raw / 1024.0; + case kRange_4G: return raw / 512.0; + case kRange_8G: return raw / 256.0; + } +} + +/** + * Set the accelerometer to active or standby mode. It must be in standby + * mode to change any configuration. + */ +void setAccelerometerActive(bool active) { + initializeAccelerometer(); + + uint8_t ctrlReg1 = readRegister(kReg_CtrlReg1); + ctrlReg1 &= ~1; // Clear the existing active bit + writeRegister(kReg_CtrlReg1, ctrlReg1 | (active? 1 : 0)); +} + +/** + * Set the range of values that can be measured (either 2, 4, or 8 g-forces). + * The accelerometer should be in standby mode when this is called. + */ +void setAccelerometerRange(AccelerometerRange range) { + initializeAccelerometer(); + + accelerometerRange = range; + + uint8_t xyzDataCfg = readRegister(kReg_XYZDataCfg); + xyzDataCfg &= ~3; // Clear the existing two range bits + writeRegister(kReg_XYZDataCfg, xyzDataCfg | range); +} + +/** + * Get the x-axis acceleration + * + * This is a floating point value in units of 1 g-force + */ +double getAccelerometerX() { + initializeAccelerometer(); + + int raw = (readRegister(kReg_OutXMSB) << 4) | (readRegister(kReg_OutXLSB) >> 4); + return unpackAxis(raw); +} + +/** + * Get the y-axis acceleration + * + * This is a floating point value in units of 1 g-force + */ +double getAccelerometerY() { + initializeAccelerometer(); + + int raw = (readRegister(kReg_OutYMSB) << 4) | (readRegister(kReg_OutYLSB) >> 4); + return unpackAxis(raw); +} + +/** + * Get the z-axis acceleration + * + * This is a floating point value in units of 1 g-force + */ +double getAccelerometerZ() { + initializeAccelerometer(); + + int raw = (readRegister(kReg_OutZMSB) << 4) | (readRegister(kReg_OutZLSB) >> 4); + return unpackAxis(raw); +}