File content as of revision 0:dfd48724e473:

/* 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);
}