Squashed 'third_party/allwpilib_2016/' content from commit 7f61816
Change-Id: If9d9245880859cdf580f5d7f77045135d0521ce7
git-subtree-dir: third_party/allwpilib_2016
git-subtree-split: 7f618166ed253a24629934fcf89c3decb0528a3b
diff --git a/hal/lib/Athena/Accelerometer.cpp b/hal/lib/Athena/Accelerometer.cpp
new file mode 100644
index 0000000..0895c9c
--- /dev/null
+++ b/hal/lib/Athena/Accelerometer.cpp
@@ -0,0 +1,234 @@
+#include "HAL/Accelerometer.hpp"
+
+#include "ChipObject.h"
+#include <stdint.h>
+#include <stdio.h>
+#include <assert.h>
+
+// 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;
+ default: return 0.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);
+}