Reimplemented FXAS21000 library for Freescale Gyro
Fork of FXAS21000 by
FXAS21000.cpp
- Committer:
- Mashu
- Date:
- 2015-07-08
- Revision:
- 4:41bd7f360406
- Parent:
- 2:cd21ef326977
File content as of revision 4:41bd7f360406:
#include "FXAS21000.h" const uint16_t uint14_max = 0x3FFF; void static inline normalize_14bits(int16_t &x) { x = ((x) > (uint14_max/2)) ? (x - uint14_max) : (x); } static int16_t dummy_int16_t = 0; static float dummy_float = 0.0f; FXAS21000::FXAS21000(I2C &i2c, uint8_t addr) { _i2c = &i2c; _addr = addr; // activate the peripheral uint8_t data[2]; _i2c->frequency(400000); /* * | ZR_COND | RST | ST | DR[2:0] | Active | Ready | * | 0 | 0 | 0 | 010 | 0 | 0 | * ZR_COND - Offset compensation calibration * RST - Reset of the device * DR - Sampling rate 50Hz (010) * Active - Device is measuring * Ready - Device is ready to measure but not taking samples */ data[0] = FXAS21000_CTRL_REG1; data[1] = 0x08; // 00001000 writeRegs(data, 2); /* * CTRL_REG0 can be only modified in Standby or Read modes * | 0 | 0 | SPIW | SEL[1:0] | HPF_EN | FS[1:0] | * | 0 | 0 | 0 | 11 | 0 | 11 | * SPIW - SPI bus mode, zero for four wires * SEL - High pass filter frequency cut-off * HPF_EN - Enable high-pass filter * FS - Full scale ranges +-1600 dps when 00 */ data[0] = FXAS21000_CTRL_REG0; data[1] = 0x1B; writeRegs(data, 2); /* * Same a first write but Active bit gets enabled */ data[0] = FXAS21000_CTRL_REG1; data[1] = 0x0A; writeRegs(data, 2); } FXAS21000::~FXAS21000() { _i2c = 0; _addr = 0; } void FXAS21000::readRegs(uint8_t addr, uint8_t *data, uint32_t len) const { uint8_t t[1] = {addr}; _i2c->write(_addr, (char *)t, sizeof(t), true); _i2c->read(_addr, (char *)data, len); } uint8_t FXAS21000::whoAmI() const { uint8_t who_am_i = 0; readRegs(FXAS21000_WHOAMI, &who_am_i, sizeof(who_am_i)); return who_am_i; } void FXAS21000::writeRegs(uint8_t * data, uint32_t len) const { _i2c->write(_addr, (char *)data, len); } int16_t FXAS21000::getSensorAxis(uint8_t addr) const { uint8_t res[2]; readRegs(addr, res, sizeof(res)); return static_cast<int16_t>((res[0] << 8) | res[1]); } void FXAS21000::enable(void) const { uint8_t data[2]; readRegs(FXAS21000_CTRL_REG1, &data[1], 1); data[1] |= 0x0A; // 00000010 to turn on Active bit data[0] = FXAS21000_CTRL_REG1; writeRegs(data, sizeof(data)); } void FXAS21000::disable(void) const { uint8_t data[2]; readRegs(FXAS21000_CTRL_REG1, &data[1], 1); data[1] &= 0xFD; // 11111101 to turn off Active bit data[0] = FXAS21000_CTRL_REG1; writeRegs(data, sizeof(data)); } uint32_t FXAS21000::dataReady(void) const { uint8_t stat = 0; readRegs(FXAS21000_STATUS, &stat, 1); stat &= 0x88; // 10001000 to zero out other bits if(stat == 0x88) { // If 7th and 4th bit are positive then return 1 stat = 1; } else { stat = 0; // Otherwise no data ready, return 0 } return (uint32_t)stat; } // OK uint32_t FXAS21000::sampleRate(uint32_t frequency) const { return(50); // for now sample rate is fixed at 50Hz } int16_t FXAS21000Gyroscpe::getX(int16_t &x = dummy_int16_t) const { x = getSensorAxis(FXAS21000_OUT_X_MSB) >> 2; normalize_14bits(x); return x; } int16_t FXAS21000Gyroscpe::getY(int16_t &y = dummy_int16_t) const { y = getSensorAxis(FXAS21000_OUT_Y_MSB) >> 2; normalize_14bits(y); return y; } int16_t FXAS21000Gyroscpe::getZ(int16_t &z = dummy_int16_t) const { z = getSensorAxis(FXAS21000_OUT_Z_MSB) >> 2; normalize_14bits(z); return z; } float FXAS21000Gyroscpe::getX(float &x = dummy_float) const { int16_t val = getSensorAxis(FXAS21000_OUT_X_MSB) >> 2; normalize_14bits(val); x = val / 4096.0f; return x; } float FXAS21000Gyroscpe::getY(float &y = dummy_float) const { int16_t val = getSensorAxis(FXAS21000_OUT_Y_MSB) >> 2; normalize_14bits(val); y = val / 4096.0f; return y; } float FXAS21000Gyroscpe::getZ(float &z = dummy_float) const { int16_t val = getSensorAxis(FXAS21000_OUT_Z_MSB) >> 2; normalize_14bits(val); z = val / 4096.0f; return z; } void FXAS21000Gyroscpe::getAxis(motion_data_counts_t &xyz) const { xyz.x = getSensorAxis(FXAS21000_OUT_X_MSB) >> 2; xyz.y = getSensorAxis(FXAS21000_OUT_Y_MSB) >> 2; xyz.z = getSensorAxis(FXAS21000_OUT_Z_MSB) >> 2; normalize_14bits(xyz.x); normalize_14bits(xyz.y); normalize_14bits(xyz.z); } void FXAS21000Gyroscpe::getAxis(motion_data_units_t &xyz) const { motion_data_counts_t _xyz; FXAS21000Gyroscpe::getAxis(_xyz); xyz.x = _xyz.x / 4096.0f; xyz.y = _xyz.y / 4096.0f; xyz.z = _xyz.z / 4096.0f; }