Diff: K64_MagAcc.cpp

Revision:

0:dfd48724e473

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/K64_MagAcc.cpp	Wed Jun 18 17:12:40 2014 +0000
@@ -0,0 +1,123 @@
+/* Copyright (c) 2014 J Kernthaler MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+
+#include "K64MagAcc.h"
+
+// FXOS8700CQ internal register addresses
+#define FXOS8700Q_STATUS 0x00
+#define FXOS8700Q_OUT_X_MSB 0x01
+#define FXOS8700Q_OUT_Y_MSB 0x03
+#define FXOS8700Q_OUT_Z_MSB 0x05
+#define FXOS8700Q_M_OUT_X_MSB 0x33
+#define FXOS8700Q_M_OUT_Y_MSB 0x35
+#define FXOS8700Q_M_OUT_Z_MSB 0x37
+#define FXOS8700Q_WHOAMI 0x0D
+#define FXOS8700Q_XYZ_DATA_CFG 0x0E
+#define FXOS8700Q_CTRL_REG1 0x2A
+#define FXOS8700Q_M_CTRL_REG1 0x5B
+#define FXOS8700Q_M_CTRL_REG2 0x5C
+#define FXOS8700Q_WHOAMI_VAL 0xC7
+
+/*
+Mag data is from 16 bit ADC, +/- 1200 uTesla full scale, so 1 lsb is 1200/32768
+Acc data is from 14 bit ADC, and conversion factor is for +/- 2g full scale
+*/
+const float MagScale = 0.0366;
+const float AccScale = 0.244e-3;
+
+/** K64MagAcc class
+ *  Used for setting up and reading data from the FXOS8700Q device on the
+ *  K64F board.
+ *  Usage
+ *      Call the Update method to read data from the device
+ *      The data is made available through two float arrays Mag and Acc
+ *  @param sda Pin name of the I2C sda connection
+ *  @param sdl Pin name of the sdl connection
+ *  @param addr Address of the device on the I2C bus
+ *  @note On the K64F board these parameters are hard wired, so you can leave them out to use the default settings
+ */
+K64MagAcc::K64MagAcc(PinName sda, PinName sdl, int addr):i2c(sda, sdl)
+{
+    i2addr = addr;
+    i2c.frequency(400000);
+    writeControl(FXOS8700Q_CTRL_REG1, 0x00); // put device in standby
+    writeControl(FXOS8700Q_M_CTRL_REG1, 0x1F);
+    writeControl(FXOS8700Q_M_CTRL_REG2, 0x20);
+    writeControl(FXOS8700Q_XYZ_DATA_CFG, 0x00); // sets accelerometer to  +/- 2g range or 0.244 mg per lsb
+    writeControl(FXOS8700Q_CTRL_REG1, 0x1D); // make device active
+}
+    
+void K64MagAcc::Update(void)
+{
+    // read status, magnetometer and accel data
+    readRegs(FXOS8700Q_STATUS, SensorData, 13);
+    Status = SensorData[0];
+    AccData[0] = Unpick14(1);
+    AccData[1] = Unpick14(3);
+    AccData[2] = Unpick14(5);
+    MagData[0] = Unpick(7);
+    MagData[1] = Unpick(9);
+    MagData[2] = Unpick(11);
+    Scale(MagData, Mag, MagScale);
+    Scale(AccData, Acc, AccScale);
+}
+
+uint8_t K64MagAcc::WhoAmI(void)
+{
+    uint8_t data;
+    readRegs(FXOS8700Q_WHOAMI, &data, 1);
+    return data;
+}
+
+int16_t K64MagAcc::Unpick(int n)
+{
+    return (SensorData[n] << 8) | SensorData[n+1];
+}
+
+int16_t K64MagAcc::Unpick14(int n)
+{
+    // pulls 14 bit data from SensorData and returns it as a signed integer
+    return (int16_t)(((SensorData[n] << 8) | SensorData[n+1]))>>2;
+}
+
+void K64MagAcc::Scale(int16_t * RawData, float * ScaledData, float Factor)
+{
+    for(int i = 0; i < 3; i++)
+    *ScaledData++ = *RawData++ * Factor;
+} 
+
+void K64MagAcc::readRegs(int RegAddr, uint8_t * data, int len) 
+{
+    char t[1] = {RegAddr};
+    i2c.write(i2addr, t, 1, true);
+    i2c.read(i2addr, (char *)data, len);
+}
+
+void K64MagAcc::writeRegs(uint8_t * data, int len) 
+{
+    i2c.write(i2addr, (char *)data, len);
+}
+
+void K64MagAcc::writeControl(uint8_t reg, uint8_t data)
+{
+uint8_t buffer[2];
+    buffer[0] = reg;
+    buffer[1] = data;
+    i2c.write(i2addr, (char*) buffer, 2);
+}
\ No newline at end of file