Diff: KX134.cpp

Revision:

0:01d5616ba355

Child:

1:c6e2a348da09

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KX134.cpp	Tue Sep 22 19:36:06 2020 -0400
@@ -0,0 +1,252 @@
+#include "KX134.h"
+#include "math.h"
+
+#define SPI_FREQ 100000
+
+char defaultBuffer[2] = {0}; // allows calling writeRegisterOneByte
+                             // without buf argument
+
+/* Writes one byte to a register
+ */
+void KX134::writeRegisterOneByte(Register addr, uint8_t data,
+                                 char *buf = defaultBuffer)
+{
+    uint8_t _data[1] = {data};
+    writeRegister(addr, _data, buf);
+}
+
+KX134::KX134(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName int1,
+             PinName int2, PinName rst)
+    : _spi(mosi, miso, sclk), _int1(int1), _int2(int2), _cs(cs), _rst(rst)
+{
+    // set default values for settings variables
+    resStatus = 1;   // high performance mode
+    drdyeStatus = 0; // Data Ready Engine disabled
+    gsel1Status = 0; // +-8g bit 1
+    gsel0Status = 0; // +-8g bit 0
+    tdteStatus = 0;  // Tap/Double-Tap engine disabled
+    tpeStatus = 0;   // Tilt Position Engine disabled
+
+    iirBypass = 0; // IIR filter is not bypassed, i.e. filtering is applied
+    lpro = 0;      // IIR filter corner frequency set to ODR/9
+    fstup = 0;     // Fast Start is disabled
+    osa3 = 0;      // Output Data Rate bits
+    osa2 = 1;      // default is 50hz
+    osa1 = 1;
+    osa0 = 0;
+
+    registerWritingEnabled = 0;
+
+    deselect();
+}
+
+bool KX134::init()
+{
+    deselect();
+
+    _spi.frequency(SPI_FREQ);
+    _spi.format(8, 1);
+
+    return reset();
+}
+
+bool KX134::reset()
+{
+    // write registers to start reset
+    writeRegisterOneByte(Register::INTERNAL_0X7F, 0x00);
+    writeRegisterOneByte(Register::CNTL2, 0x00);
+    writeRegisterOneByte(Register::CNTL2, 0x80);
+
+    // software reset time
+    wait_us(2000);
+
+    // check existence
+    return checkExistence();
+}
+
+/* Helper Functions
+ * ====================================================
+ */
+
+void KX134::deselect()
+{
+    _cs.write(1);
+}
+
+void KX134::select()
+{
+    _cs.write(0);
+}
+
+void KX134::readRegister(Register addr, char *rx_buf, int size)
+{
+    select();
+
+    rx_buf[0] = _spi.write((uint8_t)addr | (1 << 7));
+
+    for(int i = 1; i < size; ++i)
+    {
+        rx_buf[i] = _spi.write(0x00);
+    }
+
+    deselect();
+}
+
+void KX134::writeRegister(Register addr, uint8_t *data, char *rx_buf, int size)
+{
+    select();
+
+    _spi.write((uint8_t)addr); // select register
+    for(int i = 0; i < size; ++i)
+    {
+        rx_buf[i] = _spi.write(data[i]);
+    }
+
+    deselect();
+}
+
+/* Returns a 16 bit signed integer representation of a 2 address read
+ * Assumes 2s Complement
+ */
+int16_t KX134::read16BitValue(Register lowAddr, Register highAddr)
+{
+    // get contents of register
+    char lowWord[2], highWord[2];
+    readRegister(lowAddr, lowWord);
+    readRegister(highAddr, highWord);
+
+    // combine low & high words
+    uint16_t val2sComplement =
+        (static_cast<uint16_t>(highWord[1] << 8)) | lowWord[1];
+    int16_t value = static_cast<int16_t>(val2sComplement);
+
+    return value;
+}
+
+float KX134::convertRawToGravs(int16_t lsbValue)
+{
+    if(gsel1Status && gsel0Status)
+    {
+        // +-64g
+        return (float)lsbValue * 0.00195f;
+    }
+    else if(gsel1Status && !gsel0Status)
+    {
+        // +-32g
+        return (float)lsbValue * 0.00098f;
+    }
+    else if(!gsel1Status && gsel0Status)
+    {
+        // +-16g
+        return (float)lsbValue * 0.00049f;
+    }
+    else if(!gsel1Status && !gsel0Status)
+    {
+        // +-8g
+        return (float)lsbValue * 0.00024f;
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+void KX134::getAccelerations(int16_t *output)
+{
+    // read X, Y, and Z
+    output[0] = read16BitValue(Register::XOUT_L, Register::XOUT_H);
+    output[1] = read16BitValue(Register::YOUT_L, Register::YOUT_H);
+    output[2] = read16BitValue(Register::ZOUT_L, Register::ZOUT_H);
+}
+
+bool KX134::checkExistence()
+{
+    // verify WHO_I_AM
+    char whoami[5];
+    readRegister(Register::WHO_AM_I, whoami);
+
+    if(whoami[1] != 0x46)
+    {
+        return false; // WHO_AM_I is incorrect
+    }
+
+    // verify COTR
+    char cotr[2];
+    readRegister(Register::COTR, cotr);
+    if(cotr[1] != 0x55)
+    {
+        return false; // COTR is incorrect
+    }
+
+    return true;
+}
+
+void KX134::setAccelRange(Range range)
+{
+    gsel0Status = (uint8_t)range & 0b01;
+    gsel1Status = (uint8_t)range & 0b10;
+
+    uint8_t writeByte = (1 << 7) | (resStatus << 6) | (drdyeStatus << 5) |
+                        (gsel1Status << 4) | (gsel0Status << 3) |
+                        (tdteStatus << 2) | (tpeStatus);
+    // reserved bit 1, PC1 bit must be enabled
+
+    writeRegisterOneByte(Register::CNTL1, writeByte);
+
+    registerWritingEnabled = 0;
+}
+
+void KX134::setOutputDataRateBytes(uint8_t byteHz)
+{
+    if(!registerWritingEnabled)
+    {
+        return;
+    }
+
+    osa0 = byteHz & 0b0001;
+    osa1 = byteHz & 0b0010;
+    osa2 = byteHz & 0b0100;
+    osa3 = byteHz & 0b1000;
+
+    uint8_t writeByte = (iirBypass << 7) | (lpro << 6) | (fstup << 5) |
+                        (osa3 << 3) | (osa2 << 2) | (osa1 << 1) | osa0;
+    // reserved bit 4
+
+    writeRegisterOneByte(Register::ODCNTL, writeByte);
+}
+
+void KX134::setOutputDataRateHz(uint32_t hz)
+{
+    // calculate byte representation from new polling rate
+    // bytes = log2(32*rate/25)
+
+    double new_rate = (double)hz;
+
+    double bytes_double = log2((32.f / 25.f) * new_rate);
+    uint8_t bytes_int = (uint8_t)ceil(bytes_double);
+
+    setOutputDataRateBytes(bytes_int);
+}
+
+void KX134::enableRegisterWriting()
+{
+    writeRegisterOneByte(Register::CNTL1, 0x00);
+    registerWritingEnabled = 1;
+}
+
+void KX134::disableRegisterWriting()
+{
+    if(!registerWritingEnabled)
+    {
+        return;
+    }
+
+    uint8_t writeByte = (0 << 7) | (resStatus << 6) | (drdyeStatus << 5) |
+                        (gsel1Status << 4) | (gsel0Status << 3) |
+                        (tdteStatus << 2) | (tpeStatus);
+    // reserved bit 1, PC1 bit must be enabled
+
+    writeRegisterOneByte(Register::CNTL1, writeByte);
+
+    registerWritingEnabled = 0;
+}