zzz
Dependents: Assignment2_ver3 Assignment2_ver5 Assignment2_ver6
FXOS8700Q.cpp
- Committer:
- weebgrammers
- Date:
- 2016-11-29
- Revision:
- 0:cf7bca71c4e3
File content as of revision 0:cf7bca71c4e3:
#include "FXOS8700Q.h" #define UINT14_MAX 16383 uint8_t SensorBuffer[12]; int MagReadStatus; FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) { // activate the peripheral uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00}; m_i2c.frequency(400000); writeRegs(data, 2); data[0] = FXOS8700Q_M_CTRL_REG1; data[1] = 0x1F; writeRegs(data, 2); data[0] = FXOS8700Q_M_CTRL_REG2; data[1] = 0x20; writeRegs(data, 2); data[0] = FXOS8700Q_XYZ_DATA_CFG; data[1] = 0x11; writeRegs(data, 2); data[0] = FXOS8700Q_CTRL_REG1; data[1] = 0x54; writeRegs(data, 2); // Initialize XYZ Data Configuration data[0] = FXOS8700Q_XYZ_DATA_CFG; data[1] = 0x11; // high pass active, +/- 4g mode writeRegs(data,2); // Initialize Motion Detection Register (A_FFMT_CFG) data[0] = FXOS8700Q_A_FFMT_CFG; data[1] = 0xE0; // latch, motion, z thresh active writeRegs(data, 2); // Initialize Motion Threshold Register (A_FFMT_THS) data[0] = FXOS8700Q_A_FFMT_THS; data[1] = 0x10; writeRegs(data, 2); // Initialize Motion Count (A_FFMT_COUNT) data[0] = FXOS8700Q_A_FFMT_COUNT; data[1] = 0x0A; // 10 time steps x 5ms ODR = 50ms debounce period writeRegs(data, 2); // // Initialize CTRL REG 2 // data[0] = FXOS8700Q_CTRL_REG2; // data[1] = 0x1D; // low power sleep, auto-sleep, low power/noise OSR // writeRegs(data, 2); // Iniitialize CTRL REG 3 data[0] = FXOS8700Q_CTRL_REG3; data[1] = 0x00; // default settings ==> interrupts are active low! writeRegs(data, 2); // Initialize CTRL REG 4 data[0] = FXOS8700Q_CTRL_REG4; data[1] = 0x04; // freefall/motion interrupt enabled // could change: 0x06 to enable ffmt and a_vecm writeRegs(data, 2); // Initialize CTRL REG 5 data[0] = FXOS8700Q_CTRL_REG5; data[1] = 0x04; // ffmt on INT1 pin, a_vecm on INT2 pin writeRegs(data, 2); MagReadStatus = 0; } FXOS8700Q_acc::~FXOS8700Q_acc() { } void FXOS8700Q_acc::enable(void) { uint8_t data[2]; readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); printf("Is this working %04x \r\n", &data[1]); data[1] |= 0x01; data[0] = FXOS8700Q_CTRL_REG1; writeRegs(data, 2); MagReadStatus = 0; } void FXOS8700Q_acc::disable(void) { uint8_t data[2]; readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); data[1] &= 0xFE; data[0] = FXOS8700Q_CTRL_REG1; writeRegs(data, 2); MagReadStatus = 0; } uint32_t FXOS8700Q_acc::whoAmI() { uint8_t who_am_i = 0; readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1); return (uint32_t) who_am_i; } uint32_t FXOS8700Q_acc::dataReady(void) { uint8_t stat = 0; readRegs(FXOS8700Q_STATUS, &stat, 1); return (uint32_t) stat; } uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) { return(50); // for now sample rate is fixed at 50Hz } void FXOS8700Q_acc::getX(float * x) { *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f); } void FXOS8700Q_acc::getY(float * y) { *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f); } void FXOS8700Q_acc::getZ(float * z) { *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f); } void FXOS8700Q_acc::getX(int16_t * d) { *d = getAccAxis(FXOS8700Q_OUT_X_MSB); } void FXOS8700Q_acc::getY(int16_t * d) { *d = getAccAxis(FXOS8700Q_OUT_Y_MSB); } void FXOS8700Q_acc::getZ(int16_t * d) { *d = getAccAxis(FXOS8700Q_OUT_Z_MSB); } void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) { int16_t acc, t[3]; readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12); acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2); if (acc > UINT14_MAX/2) acc -= UINT14_MAX; t[0] = acc; acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2); if (acc > UINT14_MAX/2) acc -= UINT14_MAX; t[1] = acc; acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2); if (acc > UINT14_MAX/2) acc -= UINT14_MAX; t[2] = acc; data.x = ((float) t[0]) / 4096.0f; data.y = ((float) t[1]) / 4096.0f; data.z = ((float) t[2]) / 4096.0f; MagReadStatus = 1; } void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) { int16_t acc; readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12); acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2); if (acc > UINT14_MAX/2) acc -= UINT14_MAX; data.x = acc; acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2); if (acc > UINT14_MAX/2) acc -= UINT14_MAX; data.y = acc; acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2); if (acc > UINT14_MAX/2) acc -= UINT14_MAX; data.z = acc; MagReadStatus = 1; } void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) { char t[1] = {addr}; m_i2c.write(m_addr, t, 1, true); m_i2c.read(m_addr, (char *)data, len); } void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) { m_i2c.write(m_addr, (char *)data, len); } int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) { int16_t acc; uint8_t res[2]; readRegs(addr, res, 2); acc = (res[0] << 6) | (res[1] >> 2); if (acc > UINT14_MAX/2) acc -= UINT14_MAX; return acc; } uint8_t FXOS8700Q_acc::getStatus(void) { uint8_t stat = 0; readRegs( FXOS8700Q_A_FFMT_SRC, &stat, 1); return stat; } uint8_t FXOS8700Q_acc::getIntSource(void) { uint8_t intSrc = 0; readRegs ( 0x0C, &intSrc, 1); return intSrc; } FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) { // activate the peripheral uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00}; m_i2c.frequency(400000); writeRegs(data, 2); data[0] = FXOS8700Q_M_CTRL_REG1; data[1] = 0x1F; writeRegs(data, 2); data[0] = FXOS8700Q_M_CTRL_REG2; data[1] = 0x20; writeRegs(data, 2); data[0] = FXOS8700Q_XYZ_DATA_CFG; data[1] = 0x00; writeRegs(data, 2); data[0] = FXOS8700Q_CTRL_REG1; data[1] = 0x18;//0x1D; writeRegs(data, 2); MagReadStatus = 0; } FXOS8700Q_mag::~FXOS8700Q_mag() { } void FXOS8700Q_mag::enable(void) { uint8_t data[2]; readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); data[1] |= 0x01; data[0] = FXOS8700Q_CTRL_REG1; writeRegs(data, 2); MagReadStatus = 0; } void FXOS8700Q_mag::disable(void) { uint8_t data[2]; readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); data[1] &= 0xFE; data[0] = FXOS8700Q_CTRL_REG1; writeRegs(data, 2); MagReadStatus = 0; } uint32_t FXOS8700Q_mag::whoAmI() { uint8_t who_am_i = 0; readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1); return (uint32_t) who_am_i; } uint32_t FXOS8700Q_mag::dataReady(void) { uint8_t stat = 0; readRegs(FXOS8700Q_STATUS, &stat, 1); return (uint32_t) stat; } uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) { return(50); // for now sample rate is fixed at 50Hz } void FXOS8700Q_mag::getX(float * x) { *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f); } void FXOS8700Q_mag::getY(float * y) { *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f); } void FXOS8700Q_mag::getZ(float * z) { *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f); } void FXOS8700Q_mag::getX(int16_t * d) { *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB); } void FXOS8700Q_mag::getY(int16_t * d) { *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB); } void FXOS8700Q_mag::getZ(int16_t * d) { *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB); } void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) { int16_t t[3]; uint8_t res[6]; if(MagReadStatus) { t[0] = (SensorBuffer[6] << 8) | SensorBuffer[7]; t[1] = (SensorBuffer[8] << 8) | SensorBuffer[9]; t[2] = (SensorBuffer[10] << 8) | SensorBuffer[11]; } else { readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6); t[0] = (res[0] << 8) | res[1]; t[1] = (res[2] << 8) | res[3]; t[2] = (res[4] << 8) | res[5]; } data.x = ((float) t[0]) * 0.1f; data.y = ((float) t[1]) * 0.1f; data.z = ((float) t[2]) * 0.1f; } void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) { uint8_t res[6]; readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6); data.x = (res[0] << 8) | res[1]; data.y = (res[2] << 8) | res[3]; data.z = (res[4] << 8) | res[5]; } void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) { char t[1] = {addr}; m_i2c.write(m_addr, t, 1, true); m_i2c.read(m_addr, (char *)data, len); } void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) { m_i2c.write(m_addr, (char *)data, len); } int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) { int16_t acc; uint8_t res[2]; readRegs(addr, res, 2); acc = (res[0] << 8) | res[1]; return acc; }