Mateusz Kaduk / FXAS21000

Reimplemented FXAS21000 library for Freescale Gyro

Fork of FXAS21000 by Jim Carver

FXAS21000.cpp@4:41bd7f360406, 2015-07-08 (annotated)

Committer:
Mashu
Date:
Wed Jul 08 13:34:46 2015 +0000
Revision:
4:41bd7f360406
Parent:
2:cd21ef326977 
Reimplemented FXAS21000 library based on FXOS8700Q and updated MotionSensor data types.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mashu 4:41bd7f360406 1 #include "FXAS21000.h"
Mashu 4:41bd7f360406 2
Mashu 4:41bd7f360406 3 const uint16_t uint14_max = 0x3FFF;
Mashu 4:41bd7f360406 4 void static inline normalize_14bits(int16_t &x)
JimCarver 0:4cfc774d6d85 5 {
Mashu 4:41bd7f360406 6 x = ((x) > (uint14_max/2)) ? (x - uint14_max) : (x);
JimCarver 0:4cfc774d6d85 7 }
JimCarver 0:4cfc774d6d85 8
Mashu 4:41bd7f360406 9 static int16_t dummy_int16_t = 0;
Mashu 4:41bd7f360406 10 static float dummy_float = 0.0f;
Mashu 4:41bd7f360406 11
Mashu 4:41bd7f360406 12 FXAS21000::FXAS21000(I2C &i2c, uint8_t addr)
JimCarver 0:4cfc774d6d85 13 {
Mashu 4:41bd7f360406 14 _i2c = &i2c;
Mashu 4:41bd7f360406 15 _addr = addr;
Mashu 4:41bd7f360406 16 // activate the peripheral
Mashu 4:41bd7f360406 17 uint8_t data[2];
Mashu 4:41bd7f360406 18 _i2c->frequency(400000);
Mashu 4:41bd7f360406 19
Mashu 4:41bd7f360406 20 /*
Mashu 4:41bd7f360406 21 * | ZR_COND | RST | ST | DR[2:0] | Active | Ready |
Mashu 4:41bd7f360406 22 * | 0 | 0 | 0 | 010 | 0 | 0 |
Mashu 4:41bd7f360406 23 * ZR_COND - Offset compensation calibration
Mashu 4:41bd7f360406 24 * RST - Reset of the device
Mashu 4:41bd7f360406 25 * DR - Sampling rate 50Hz (010)
Mashu 4:41bd7f360406 26 * Active - Device is measuring
Mashu 4:41bd7f360406 27 * Ready - Device is ready to measure but not taking samples
Mashu 4:41bd7f360406 28 */
JimCarver 0:4cfc774d6d85 29 data[0] = FXAS21000_CTRL_REG1;
Mashu 4:41bd7f360406 30 data[1] = 0x08; // 00001000
Mashu 4:41bd7f360406 31 writeRegs(data, 2);
JimCarver 0:4cfc774d6d85 32
Mashu 4:41bd7f360406 33 /*
Mashu 4:41bd7f360406 34 * CTRL_REG0 can be only modified in Standby or Read modes
Mashu 4:41bd7f360406 35 * | 0 | 0 | SPIW | SEL[1:0] | HPF_EN | FS[1:0] |
Mashu 4:41bd7f360406 36 * | 0 | 0 | 0 | 11 | 0 | 11 |
Mashu 4:41bd7f360406 37 * SPIW - SPI bus mode, zero for four wires
Mashu 4:41bd7f360406 38 * SEL - High pass filter frequency cut-off
Mashu 4:41bd7f360406 39 * HPF_EN - Enable high-pass filter
Mashu 4:41bd7f360406 40 * FS - Full scale ranges +-1600 dps when 00
Mashu 4:41bd7f360406 41 */
JimCarver 0:4cfc774d6d85 42 data[0] = FXAS21000_CTRL_REG0;
JimCarver 0:4cfc774d6d85 43 data[1] = 0x1B;
Mashu 4:41bd7f360406 44 writeRegs(data, 2);
Mashu 4:41bd7f360406 45
Mashu 4:41bd7f360406 46 /*
Mashu 4:41bd7f360406 47 * Same a first write but Active bit gets enabled
Mashu 4:41bd7f360406 48 */
JimCarver 0:4cfc774d6d85 49 data[0] = FXAS21000_CTRL_REG1;
JimCarver 0:4cfc774d6d85 50 data[1] = 0x0A;
Mashu 4:41bd7f360406 51 writeRegs(data, 2);
Mashu 4:41bd7f360406 52 }
Mashu 4:41bd7f360406 53
Mashu 4:41bd7f360406 54 FXAS21000::~FXAS21000()
Mashu 4:41bd7f360406 55 {
Mashu 4:41bd7f360406 56 _i2c = 0;
Mashu 4:41bd7f360406 57 _addr = 0;
Mashu 4:41bd7f360406 58 }
Mashu 4:41bd7f360406 59
Mashu 4:41bd7f360406 60 void FXAS21000::readRegs(uint8_t addr, uint8_t *data, uint32_t len) const
Mashu 4:41bd7f360406 61 {
Mashu 4:41bd7f360406 62 uint8_t t[1] = {addr};
Mashu 4:41bd7f360406 63 _i2c->write(_addr, (char *)t, sizeof(t), true);
Mashu 4:41bd7f360406 64 _i2c->read(_addr, (char *)data, len);
Mashu 4:41bd7f360406 65 }
Mashu 4:41bd7f360406 66
Mashu 4:41bd7f360406 67 uint8_t FXAS21000::whoAmI() const
Mashu 4:41bd7f360406 68 {
Mashu 4:41bd7f360406 69 uint8_t who_am_i = 0;
Mashu 4:41bd7f360406 70 readRegs(FXAS21000_WHOAMI, &who_am_i, sizeof(who_am_i));
Mashu 4:41bd7f360406 71 return who_am_i;
Mashu 4:41bd7f360406 72 }
Mashu 4:41bd7f360406 73
Mashu 4:41bd7f360406 74 void FXAS21000::writeRegs(uint8_t * data, uint32_t len) const
Mashu 4:41bd7f360406 75 {
Mashu 4:41bd7f360406 76 _i2c->write(_addr, (char *)data, len);
JimCarver 0:4cfc774d6d85 77 }
JimCarver 0:4cfc774d6d85 78
Mashu 4:41bd7f360406 79 int16_t FXAS21000::getSensorAxis(uint8_t addr) const
Mashu 4:41bd7f360406 80 {
Mashu 4:41bd7f360406 81 uint8_t res[2];
Mashu 4:41bd7f360406 82 readRegs(addr, res, sizeof(res));
Mashu 4:41bd7f360406 83 return static_cast<int16_t>((res[0] << 8) | res[1]);
Mashu 4:41bd7f360406 84 }
Mashu 4:41bd7f360406 85
Mashu 4:41bd7f360406 86 void FXAS21000::enable(void) const
Mashu 4:41bd7f360406 87 {
Mashu 4:41bd7f360406 88 uint8_t data[2];
Mashu 4:41bd7f360406 89 readRegs(FXAS21000_CTRL_REG1, &data[1], 1);
Mashu 4:41bd7f360406 90 data[1] |= 0x0A; // 00000010 to turn on Active bit
Mashu 4:41bd7f360406 91 data[0] = FXAS21000_CTRL_REG1;
Mashu 4:41bd7f360406 92 writeRegs(data, sizeof(data));
Mashu 4:41bd7f360406 93 }
Mashu 4:41bd7f360406 94
Mashu 4:41bd7f360406 95 void FXAS21000::disable(void) const
JimCarver 0:4cfc774d6d85 96 {
Mashu 4:41bd7f360406 97 uint8_t data[2];
Mashu 4:41bd7f360406 98 readRegs(FXAS21000_CTRL_REG1, &data[1], 1);
Mashu 4:41bd7f360406 99 data[1] &= 0xFD; // 11111101 to turn off Active bit
Mashu 4:41bd7f360406 100 data[0] = FXAS21000_CTRL_REG1;
Mashu 4:41bd7f360406 101 writeRegs(data, sizeof(data));
Mashu 4:41bd7f360406 102 }
Mashu 4:41bd7f360406 103
Mashu 4:41bd7f360406 104 uint32_t FXAS21000::dataReady(void) const
Mashu 4:41bd7f360406 105 {
Mashu 4:41bd7f360406 106 uint8_t stat = 0;
Mashu 4:41bd7f360406 107 readRegs(FXAS21000_STATUS, &stat, 1);
Mashu 4:41bd7f360406 108 stat &= 0x88; // 10001000 to zero out other bits
Mashu 4:41bd7f360406 109 if(stat == 0x88) { // If 7th and 4th bit are positive then return 1
Mashu 4:41bd7f360406 110 stat = 1;
Mashu 4:41bd7f360406 111 } else {
Mashu 4:41bd7f360406 112 stat = 0; // Otherwise no data ready, return 0
Mashu 4:41bd7f360406 113 }
Mashu 4:41bd7f360406 114 return (uint32_t)stat;
JimCarver 0:4cfc774d6d85 115 }
JimCarver 0:4cfc774d6d85 116
Mashu 4:41bd7f360406 117 // OK
Mashu 4:41bd7f360406 118 uint32_t FXAS21000::sampleRate(uint32_t frequency) const
JimCarver 0:4cfc774d6d85 119 {
Mashu 4:41bd7f360406 120 return(50); // for now sample rate is fixed at 50Hz
Mashu 4:41bd7f360406 121 }
Mashu 4:41bd7f360406 122
Mashu 4:41bd7f360406 123 int16_t FXAS21000Gyroscpe::getX(int16_t &x = dummy_int16_t) const
Mashu 4:41bd7f360406 124 {
Mashu 4:41bd7f360406 125 x = getSensorAxis(FXAS21000_OUT_X_MSB) >> 2;
Mashu 4:41bd7f360406 126 normalize_14bits(x);
Mashu 4:41bd7f360406 127 return x;
Mashu 4:41bd7f360406 128 }
JimCarver 0:4cfc774d6d85 129
Mashu 4:41bd7f360406 130 int16_t FXAS21000Gyroscpe::getY(int16_t &y = dummy_int16_t) const
Mashu 4:41bd7f360406 131 {
Mashu 4:41bd7f360406 132 y = getSensorAxis(FXAS21000_OUT_Y_MSB) >> 2;
Mashu 4:41bd7f360406 133 normalize_14bits(y);
Mashu 4:41bd7f360406 134 return y;
Mashu 4:41bd7f360406 135 }
JimCarver 0:4cfc774d6d85 136
Mashu 4:41bd7f360406 137 int16_t FXAS21000Gyroscpe::getZ(int16_t &z = dummy_int16_t) const
Mashu 4:41bd7f360406 138 {
Mashu 4:41bd7f360406 139 z = getSensorAxis(FXAS21000_OUT_Z_MSB) >> 2;
Mashu 4:41bd7f360406 140 normalize_14bits(z);
Mashu 4:41bd7f360406 141 return z;
Mashu 4:41bd7f360406 142 }
Mashu 4:41bd7f360406 143
Mashu 4:41bd7f360406 144 float FXAS21000Gyroscpe::getX(float &x = dummy_float) const
Mashu 4:41bd7f360406 145 {
Mashu 4:41bd7f360406 146 int16_t val = getSensorAxis(FXAS21000_OUT_X_MSB) >> 2;
Mashu 4:41bd7f360406 147 normalize_14bits(val);
Mashu 4:41bd7f360406 148 x = val / 4096.0f;
Mashu 4:41bd7f360406 149 return x;
JimCarver 0:4cfc774d6d85 150 }
JimCarver 0:4cfc774d6d85 151
Mashu 4:41bd7f360406 152 float FXAS21000Gyroscpe::getY(float &y = dummy_float) const
JimCarver 0:4cfc774d6d85 153 {
Mashu 4:41bd7f360406 154 int16_t val = getSensorAxis(FXAS21000_OUT_Y_MSB) >> 2;
Mashu 4:41bd7f360406 155 normalize_14bits(val);
Mashu 4:41bd7f360406 156 y = val / 4096.0f;
Mashu 4:41bd7f360406 157 return y;
Mashu 4:41bd7f360406 158 }
Mashu 4:41bd7f360406 159
Mashu 4:41bd7f360406 160 float FXAS21000Gyroscpe::getZ(float &z = dummy_float) const
Mashu 4:41bd7f360406 161 {
Mashu 4:41bd7f360406 162 int16_t val = getSensorAxis(FXAS21000_OUT_Z_MSB) >> 2;
Mashu 4:41bd7f360406 163 normalize_14bits(val);
Mashu 4:41bd7f360406 164 z = val / 4096.0f;
Mashu 4:41bd7f360406 165 return z;
Mashu 4:41bd7f360406 166 }
JimCarver 0:4cfc774d6d85 167
Mashu 4:41bd7f360406 168 void FXAS21000Gyroscpe::getAxis(motion_data_counts_t &xyz) const
Mashu 4:41bd7f360406 169 {
Mashu 4:41bd7f360406 170 xyz.x = getSensorAxis(FXAS21000_OUT_X_MSB) >> 2;
Mashu 4:41bd7f360406 171 xyz.y = getSensorAxis(FXAS21000_OUT_Y_MSB) >> 2;
Mashu 4:41bd7f360406 172 xyz.z = getSensorAxis(FXAS21000_OUT_Z_MSB) >> 2;
Mashu 4:41bd7f360406 173 normalize_14bits(xyz.x);
Mashu 4:41bd7f360406 174 normalize_14bits(xyz.y);
Mashu 4:41bd7f360406 175 normalize_14bits(xyz.z);
Mashu 4:41bd7f360406 176 }
Mashu 4:41bd7f360406 177
Mashu 4:41bd7f360406 178 void FXAS21000Gyroscpe::getAxis(motion_data_units_t &xyz) const
Mashu 4:41bd7f360406 179 {
Mashu 4:41bd7f360406 180 motion_data_counts_t _xyz;
Mashu 4:41bd7f360406 181 FXAS21000Gyroscpe::getAxis(_xyz);
Mashu 4:41bd7f360406 182 xyz.x = _xyz.x / 4096.0f;
Mashu 4:41bd7f360406 183 xyz.y = _xyz.y / 4096.0f;
Mashu 4:41bd7f360406 184 xyz.z = _xyz.z / 4096.0f;
Mashu 4:41bd7f360406 185 }