weeb grammers / MotionSensor

zzz

Dependencies:   MotionSensor

Dependents:   Assignment2_ver3 Assignment2_ver5 Assignment2_ver6

FXOS8700Q.cpp@0:cf7bca71c4e3, 2016-11-29 (annotated)

Committer:
weebgrammers
Date:
Tue Nov 29 20:30:41 2016 +0000
Revision:
0:cf7bca71c4e3
ZZZZ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
weebgrammers 0:cf7bca71c4e3 1 #include "FXOS8700Q.h"
weebgrammers 0:cf7bca71c4e3 2 #define UINT14_MAX 16383
weebgrammers 0:cf7bca71c4e3 3
weebgrammers 0:cf7bca71c4e3 4 uint8_t SensorBuffer[12];
weebgrammers 0:cf7bca71c4e3 5 int MagReadStatus;
weebgrammers 0:cf7bca71c4e3 6
weebgrammers 0:cf7bca71c4e3 7 FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
weebgrammers 0:cf7bca71c4e3 8
weebgrammers 0:cf7bca71c4e3 9 // activate the peripheral
weebgrammers 0:cf7bca71c4e3 10 uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
weebgrammers 0:cf7bca71c4e3 11 m_i2c.frequency(400000);
weebgrammers 0:cf7bca71c4e3 12 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 13
weebgrammers 0:cf7bca71c4e3 14 data[0] = FXOS8700Q_M_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 15 data[1] = 0x1F;
weebgrammers 0:cf7bca71c4e3 16 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 17
weebgrammers 0:cf7bca71c4e3 18 data[0] = FXOS8700Q_M_CTRL_REG2;
weebgrammers 0:cf7bca71c4e3 19 data[1] = 0x20;
weebgrammers 0:cf7bca71c4e3 20 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 21
weebgrammers 0:cf7bca71c4e3 22 data[0] = FXOS8700Q_XYZ_DATA_CFG;
weebgrammers 0:cf7bca71c4e3 23 data[1] = 0x11;
weebgrammers 0:cf7bca71c4e3 24 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 25
weebgrammers 0:cf7bca71c4e3 26 data[0] = FXOS8700Q_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 27 data[1] = 0x54;
weebgrammers 0:cf7bca71c4e3 28 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 29
weebgrammers 0:cf7bca71c4e3 30 // Initialize XYZ Data Configuration
weebgrammers 0:cf7bca71c4e3 31 data[0] = FXOS8700Q_XYZ_DATA_CFG;
weebgrammers 0:cf7bca71c4e3 32 data[1] = 0x11; // high pass active, +/- 4g mode
weebgrammers 0:cf7bca71c4e3 33 writeRegs(data,2);
weebgrammers 0:cf7bca71c4e3 34
weebgrammers 0:cf7bca71c4e3 35 // Initialize Motion Detection Register (A_FFMT_CFG)
weebgrammers 0:cf7bca71c4e3 36 data[0] = FXOS8700Q_A_FFMT_CFG;
weebgrammers 0:cf7bca71c4e3 37 data[1] = 0xE0; // latch, motion, z thresh active
weebgrammers 0:cf7bca71c4e3 38 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 39
weebgrammers 0:cf7bca71c4e3 40 // Initialize Motion Threshold Register (A_FFMT_THS)
weebgrammers 0:cf7bca71c4e3 41 data[0] = FXOS8700Q_A_FFMT_THS;
weebgrammers 0:cf7bca71c4e3 42 data[1] = 0x10;
weebgrammers 0:cf7bca71c4e3 43 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 44
weebgrammers 0:cf7bca71c4e3 45 // Initialize Motion Count (A_FFMT_COUNT)
weebgrammers 0:cf7bca71c4e3 46 data[0] = FXOS8700Q_A_FFMT_COUNT;
weebgrammers 0:cf7bca71c4e3 47 data[1] = 0x0A; // 10 time steps x 5ms ODR = 50ms debounce period
weebgrammers 0:cf7bca71c4e3 48 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 49
weebgrammers 0:cf7bca71c4e3 50
weebgrammers 0:cf7bca71c4e3 51
weebgrammers 0:cf7bca71c4e3 52 // // Initialize CTRL REG 2
weebgrammers 0:cf7bca71c4e3 53 // data[0] = FXOS8700Q_CTRL_REG2;
weebgrammers 0:cf7bca71c4e3 54 // data[1] = 0x1D; // low power sleep, auto-sleep, low power/noise OSR
weebgrammers 0:cf7bca71c4e3 55 // writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 56
weebgrammers 0:cf7bca71c4e3 57 // Iniitialize CTRL REG 3
weebgrammers 0:cf7bca71c4e3 58 data[0] = FXOS8700Q_CTRL_REG3;
weebgrammers 0:cf7bca71c4e3 59 data[1] = 0x00; // default settings ==> interrupts are active low!
weebgrammers 0:cf7bca71c4e3 60 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 61
weebgrammers 0:cf7bca71c4e3 62 // Initialize CTRL REG 4
weebgrammers 0:cf7bca71c4e3 63 data[0] = FXOS8700Q_CTRL_REG4;
weebgrammers 0:cf7bca71c4e3 64 data[1] = 0x04; // freefall/motion interrupt enabled
weebgrammers 0:cf7bca71c4e3 65 // could change: 0x06 to enable ffmt and a_vecm
weebgrammers 0:cf7bca71c4e3 66 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 67 // Initialize CTRL REG 5
weebgrammers 0:cf7bca71c4e3 68 data[0] = FXOS8700Q_CTRL_REG5;
weebgrammers 0:cf7bca71c4e3 69 data[1] = 0x04; // ffmt on INT1 pin, a_vecm on INT2 pin
weebgrammers 0:cf7bca71c4e3 70 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 71
weebgrammers 0:cf7bca71c4e3 72 MagReadStatus = 0;
weebgrammers 0:cf7bca71c4e3 73 }
weebgrammers 0:cf7bca71c4e3 74
weebgrammers 0:cf7bca71c4e3 75 FXOS8700Q_acc::~FXOS8700Q_acc() { }
weebgrammers 0:cf7bca71c4e3 76
weebgrammers 0:cf7bca71c4e3 77 void FXOS8700Q_acc::enable(void) {
weebgrammers 0:cf7bca71c4e3 78 uint8_t data[2];
weebgrammers 0:cf7bca71c4e3 79 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
weebgrammers 0:cf7bca71c4e3 80 printf("Is this working %04x \r\n", &data[1]);
weebgrammers 0:cf7bca71c4e3 81 data[1] |= 0x01;
weebgrammers 0:cf7bca71c4e3 82 data[0] = FXOS8700Q_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 83 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 84 MagReadStatus = 0;
weebgrammers 0:cf7bca71c4e3 85 }
weebgrammers 0:cf7bca71c4e3 86
weebgrammers 0:cf7bca71c4e3 87 void FXOS8700Q_acc::disable(void) {
weebgrammers 0:cf7bca71c4e3 88 uint8_t data[2];
weebgrammers 0:cf7bca71c4e3 89 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
weebgrammers 0:cf7bca71c4e3 90 data[1] &= 0xFE;
weebgrammers 0:cf7bca71c4e3 91 data[0] = FXOS8700Q_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 92 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 93 MagReadStatus = 0;
weebgrammers 0:cf7bca71c4e3 94 }
weebgrammers 0:cf7bca71c4e3 95
weebgrammers 0:cf7bca71c4e3 96 uint32_t FXOS8700Q_acc::whoAmI() {
weebgrammers 0:cf7bca71c4e3 97 uint8_t who_am_i = 0;
weebgrammers 0:cf7bca71c4e3 98 readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
weebgrammers 0:cf7bca71c4e3 99 return (uint32_t) who_am_i;
weebgrammers 0:cf7bca71c4e3 100 }
weebgrammers 0:cf7bca71c4e3 101
weebgrammers 0:cf7bca71c4e3 102 uint32_t FXOS8700Q_acc::dataReady(void) {
weebgrammers 0:cf7bca71c4e3 103 uint8_t stat = 0;
weebgrammers 0:cf7bca71c4e3 104 readRegs(FXOS8700Q_STATUS, &stat, 1);
weebgrammers 0:cf7bca71c4e3 105 return (uint32_t) stat;
weebgrammers 0:cf7bca71c4e3 106 }
weebgrammers 0:cf7bca71c4e3 107
weebgrammers 0:cf7bca71c4e3 108 uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) {
weebgrammers 0:cf7bca71c4e3 109 return(50); // for now sample rate is fixed at 50Hz
weebgrammers 0:cf7bca71c4e3 110 }
weebgrammers 0:cf7bca71c4e3 111
weebgrammers 0:cf7bca71c4e3 112 void FXOS8700Q_acc::getX(float * x) {
weebgrammers 0:cf7bca71c4e3 113 *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f);
weebgrammers 0:cf7bca71c4e3 114 }
weebgrammers 0:cf7bca71c4e3 115
weebgrammers 0:cf7bca71c4e3 116 void FXOS8700Q_acc::getY(float * y) {
weebgrammers 0:cf7bca71c4e3 117 *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f);
weebgrammers 0:cf7bca71c4e3 118 }
weebgrammers 0:cf7bca71c4e3 119
weebgrammers 0:cf7bca71c4e3 120 void FXOS8700Q_acc::getZ(float * z) {
weebgrammers 0:cf7bca71c4e3 121 *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f);
weebgrammers 0:cf7bca71c4e3 122 }
weebgrammers 0:cf7bca71c4e3 123
weebgrammers 0:cf7bca71c4e3 124 void FXOS8700Q_acc::getX(int16_t * d) {
weebgrammers 0:cf7bca71c4e3 125 *d = getAccAxis(FXOS8700Q_OUT_X_MSB);
weebgrammers 0:cf7bca71c4e3 126 }
weebgrammers 0:cf7bca71c4e3 127
weebgrammers 0:cf7bca71c4e3 128 void FXOS8700Q_acc::getY(int16_t * d) {
weebgrammers 0:cf7bca71c4e3 129 *d = getAccAxis(FXOS8700Q_OUT_Y_MSB);
weebgrammers 0:cf7bca71c4e3 130 }
weebgrammers 0:cf7bca71c4e3 131
weebgrammers 0:cf7bca71c4e3 132 void FXOS8700Q_acc::getZ(int16_t * d) {
weebgrammers 0:cf7bca71c4e3 133 *d = getAccAxis(FXOS8700Q_OUT_Z_MSB);
weebgrammers 0:cf7bca71c4e3 134 }
weebgrammers 0:cf7bca71c4e3 135
weebgrammers 0:cf7bca71c4e3 136
weebgrammers 0:cf7bca71c4e3 137 void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
weebgrammers 0:cf7bca71c4e3 138 int16_t acc, t[3];
weebgrammers 0:cf7bca71c4e3 139
weebgrammers 0:cf7bca71c4e3 140 readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
weebgrammers 0:cf7bca71c4e3 141
weebgrammers 0:cf7bca71c4e3 142 acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
weebgrammers 0:cf7bca71c4e3 143 if (acc > UINT14_MAX/2)
weebgrammers 0:cf7bca71c4e3 144 acc -= UINT14_MAX;
weebgrammers 0:cf7bca71c4e3 145 t[0] = acc;
weebgrammers 0:cf7bca71c4e3 146 acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
weebgrammers 0:cf7bca71c4e3 147 if (acc > UINT14_MAX/2)
weebgrammers 0:cf7bca71c4e3 148 acc -= UINT14_MAX;
weebgrammers 0:cf7bca71c4e3 149 t[1] = acc;
weebgrammers 0:cf7bca71c4e3 150 acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
weebgrammers 0:cf7bca71c4e3 151 if (acc > UINT14_MAX/2)
weebgrammers 0:cf7bca71c4e3 152 acc -= UINT14_MAX;
weebgrammers 0:cf7bca71c4e3 153 t[2] = acc;
weebgrammers 0:cf7bca71c4e3 154 data.x = ((float) t[0]) / 4096.0f;
weebgrammers 0:cf7bca71c4e3 155 data.y = ((float) t[1]) / 4096.0f;
weebgrammers 0:cf7bca71c4e3 156 data.z = ((float) t[2]) / 4096.0f;
weebgrammers 0:cf7bca71c4e3 157 MagReadStatus = 1;
weebgrammers 0:cf7bca71c4e3 158 }
weebgrammers 0:cf7bca71c4e3 159
weebgrammers 0:cf7bca71c4e3 160
weebgrammers 0:cf7bca71c4e3 161 void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
weebgrammers 0:cf7bca71c4e3 162 int16_t acc;
weebgrammers 0:cf7bca71c4e3 163 readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
weebgrammers 0:cf7bca71c4e3 164
weebgrammers 0:cf7bca71c4e3 165 acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
weebgrammers 0:cf7bca71c4e3 166 if (acc > UINT14_MAX/2)
weebgrammers 0:cf7bca71c4e3 167 acc -= UINT14_MAX;
weebgrammers 0:cf7bca71c4e3 168 data.x = acc;
weebgrammers 0:cf7bca71c4e3 169 acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
weebgrammers 0:cf7bca71c4e3 170 if (acc > UINT14_MAX/2)
weebgrammers 0:cf7bca71c4e3 171 acc -= UINT14_MAX;
weebgrammers 0:cf7bca71c4e3 172 data.y = acc;
weebgrammers 0:cf7bca71c4e3 173 acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
weebgrammers 0:cf7bca71c4e3 174 if (acc > UINT14_MAX/2)
weebgrammers 0:cf7bca71c4e3 175 acc -= UINT14_MAX;
weebgrammers 0:cf7bca71c4e3 176 data.z = acc;
weebgrammers 0:cf7bca71c4e3 177 MagReadStatus = 1;
weebgrammers 0:cf7bca71c4e3 178 }
weebgrammers 0:cf7bca71c4e3 179
weebgrammers 0:cf7bca71c4e3 180 void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
weebgrammers 0:cf7bca71c4e3 181 char t[1] = {addr};
weebgrammers 0:cf7bca71c4e3 182 m_i2c.write(m_addr, t, 1, true);
weebgrammers 0:cf7bca71c4e3 183 m_i2c.read(m_addr, (char *)data, len);
weebgrammers 0:cf7bca71c4e3 184 }
weebgrammers 0:cf7bca71c4e3 185
weebgrammers 0:cf7bca71c4e3 186 void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) {
weebgrammers 0:cf7bca71c4e3 187 m_i2c.write(m_addr, (char *)data, len);
weebgrammers 0:cf7bca71c4e3 188 }
weebgrammers 0:cf7bca71c4e3 189
weebgrammers 0:cf7bca71c4e3 190
weebgrammers 0:cf7bca71c4e3 191 int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) {
weebgrammers 0:cf7bca71c4e3 192 int16_t acc;
weebgrammers 0:cf7bca71c4e3 193 uint8_t res[2];
weebgrammers 0:cf7bca71c4e3 194 readRegs(addr, res, 2);
weebgrammers 0:cf7bca71c4e3 195
weebgrammers 0:cf7bca71c4e3 196 acc = (res[0] << 6) | (res[1] >> 2);
weebgrammers 0:cf7bca71c4e3 197 if (acc > UINT14_MAX/2)
weebgrammers 0:cf7bca71c4e3 198 acc -= UINT14_MAX;
weebgrammers 0:cf7bca71c4e3 199
weebgrammers 0:cf7bca71c4e3 200 return acc;
weebgrammers 0:cf7bca71c4e3 201 }
weebgrammers 0:cf7bca71c4e3 202
weebgrammers 0:cf7bca71c4e3 203 uint8_t FXOS8700Q_acc::getStatus(void) {
weebgrammers 0:cf7bca71c4e3 204 uint8_t stat = 0;
weebgrammers 0:cf7bca71c4e3 205 readRegs( FXOS8700Q_A_FFMT_SRC, &stat, 1);
weebgrammers 0:cf7bca71c4e3 206 return stat;
weebgrammers 0:cf7bca71c4e3 207 }
weebgrammers 0:cf7bca71c4e3 208
weebgrammers 0:cf7bca71c4e3 209 uint8_t FXOS8700Q_acc::getIntSource(void) {
weebgrammers 0:cf7bca71c4e3 210 uint8_t intSrc = 0;
weebgrammers 0:cf7bca71c4e3 211 readRegs ( 0x0C, &intSrc, 1);
weebgrammers 0:cf7bca71c4e3 212 return intSrc;
weebgrammers 0:cf7bca71c4e3 213 }
weebgrammers 0:cf7bca71c4e3 214
weebgrammers 0:cf7bca71c4e3 215
weebgrammers 0:cf7bca71c4e3 216 FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
weebgrammers 0:cf7bca71c4e3 217 // activate the peripheral
weebgrammers 0:cf7bca71c4e3 218 uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
weebgrammers 0:cf7bca71c4e3 219 m_i2c.frequency(400000);
weebgrammers 0:cf7bca71c4e3 220 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 221 data[0] = FXOS8700Q_M_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 222 data[1] = 0x1F;
weebgrammers 0:cf7bca71c4e3 223 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 224 data[0] = FXOS8700Q_M_CTRL_REG2;
weebgrammers 0:cf7bca71c4e3 225 data[1] = 0x20;
weebgrammers 0:cf7bca71c4e3 226 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 227 data[0] = FXOS8700Q_XYZ_DATA_CFG;
weebgrammers 0:cf7bca71c4e3 228 data[1] = 0x00;
weebgrammers 0:cf7bca71c4e3 229 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 230 data[0] = FXOS8700Q_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 231 data[1] = 0x18;//0x1D;
weebgrammers 0:cf7bca71c4e3 232 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 233 MagReadStatus = 0;
weebgrammers 0:cf7bca71c4e3 234 }
weebgrammers 0:cf7bca71c4e3 235
weebgrammers 0:cf7bca71c4e3 236 FXOS8700Q_mag::~FXOS8700Q_mag() { }
weebgrammers 0:cf7bca71c4e3 237
weebgrammers 0:cf7bca71c4e3 238 void FXOS8700Q_mag::enable(void) {
weebgrammers 0:cf7bca71c4e3 239 uint8_t data[2];
weebgrammers 0:cf7bca71c4e3 240 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
weebgrammers 0:cf7bca71c4e3 241 data[1] |= 0x01;
weebgrammers 0:cf7bca71c4e3 242 data[0] = FXOS8700Q_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 243 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 244 MagReadStatus = 0;
weebgrammers 0:cf7bca71c4e3 245 }
weebgrammers 0:cf7bca71c4e3 246
weebgrammers 0:cf7bca71c4e3 247 void FXOS8700Q_mag::disable(void) {
weebgrammers 0:cf7bca71c4e3 248 uint8_t data[2];
weebgrammers 0:cf7bca71c4e3 249 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
weebgrammers 0:cf7bca71c4e3 250 data[1] &= 0xFE;
weebgrammers 0:cf7bca71c4e3 251 data[0] = FXOS8700Q_CTRL_REG1;
weebgrammers 0:cf7bca71c4e3 252 writeRegs(data, 2);
weebgrammers 0:cf7bca71c4e3 253 MagReadStatus = 0;
weebgrammers 0:cf7bca71c4e3 254 }
weebgrammers 0:cf7bca71c4e3 255
weebgrammers 0:cf7bca71c4e3 256
weebgrammers 0:cf7bca71c4e3 257
weebgrammers 0:cf7bca71c4e3 258 uint32_t FXOS8700Q_mag::whoAmI() {
weebgrammers 0:cf7bca71c4e3 259 uint8_t who_am_i = 0;
weebgrammers 0:cf7bca71c4e3 260 readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
weebgrammers 0:cf7bca71c4e3 261 return (uint32_t) who_am_i;
weebgrammers 0:cf7bca71c4e3 262 }
weebgrammers 0:cf7bca71c4e3 263
weebgrammers 0:cf7bca71c4e3 264 uint32_t FXOS8700Q_mag::dataReady(void) {
weebgrammers 0:cf7bca71c4e3 265 uint8_t stat = 0;
weebgrammers 0:cf7bca71c4e3 266 readRegs(FXOS8700Q_STATUS, &stat, 1);
weebgrammers 0:cf7bca71c4e3 267 return (uint32_t) stat;
weebgrammers 0:cf7bca71c4e3 268 }
weebgrammers 0:cf7bca71c4e3 269
weebgrammers 0:cf7bca71c4e3 270 uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) {
weebgrammers 0:cf7bca71c4e3 271 return(50); // for now sample rate is fixed at 50Hz
weebgrammers 0:cf7bca71c4e3 272 }
weebgrammers 0:cf7bca71c4e3 273
weebgrammers 0:cf7bca71c4e3 274 void FXOS8700Q_mag::getX(float * x) {
weebgrammers 0:cf7bca71c4e3 275 *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f);
weebgrammers 0:cf7bca71c4e3 276 }
weebgrammers 0:cf7bca71c4e3 277
weebgrammers 0:cf7bca71c4e3 278 void FXOS8700Q_mag::getY(float * y) {
weebgrammers 0:cf7bca71c4e3 279 *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f);
weebgrammers 0:cf7bca71c4e3 280 }
weebgrammers 0:cf7bca71c4e3 281
weebgrammers 0:cf7bca71c4e3 282 void FXOS8700Q_mag::getZ(float * z) {
weebgrammers 0:cf7bca71c4e3 283 *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f);
weebgrammers 0:cf7bca71c4e3 284 }
weebgrammers 0:cf7bca71c4e3 285
weebgrammers 0:cf7bca71c4e3 286 void FXOS8700Q_mag::getX(int16_t * d) {
weebgrammers 0:cf7bca71c4e3 287 *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB);
weebgrammers 0:cf7bca71c4e3 288 }
weebgrammers 0:cf7bca71c4e3 289
weebgrammers 0:cf7bca71c4e3 290 void FXOS8700Q_mag::getY(int16_t * d) {
weebgrammers 0:cf7bca71c4e3 291 *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB);
weebgrammers 0:cf7bca71c4e3 292 }
weebgrammers 0:cf7bca71c4e3 293
weebgrammers 0:cf7bca71c4e3 294 void FXOS8700Q_mag::getZ(int16_t * d) {
weebgrammers 0:cf7bca71c4e3 295 *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB);
weebgrammers 0:cf7bca71c4e3 296 }
weebgrammers 0:cf7bca71c4e3 297
weebgrammers 0:cf7bca71c4e3 298
weebgrammers 0:cf7bca71c4e3 299 void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
weebgrammers 0:cf7bca71c4e3 300 int16_t t[3];
weebgrammers 0:cf7bca71c4e3 301 uint8_t res[6];
weebgrammers 0:cf7bca71c4e3 302
weebgrammers 0:cf7bca71c4e3 303 if(MagReadStatus) {
weebgrammers 0:cf7bca71c4e3 304 t[0] = (SensorBuffer[6] << 8) | SensorBuffer[7];
weebgrammers 0:cf7bca71c4e3 305 t[1] = (SensorBuffer[8] << 8) | SensorBuffer[9];
weebgrammers 0:cf7bca71c4e3 306 t[2] = (SensorBuffer[10] << 8) | SensorBuffer[11];
weebgrammers 0:cf7bca71c4e3 307 } else {
weebgrammers 0:cf7bca71c4e3 308 readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
weebgrammers 0:cf7bca71c4e3 309 t[0] = (res[0] << 8) | res[1];
weebgrammers 0:cf7bca71c4e3 310 t[1] = (res[2] << 8) | res[3];
weebgrammers 0:cf7bca71c4e3 311 t[2] = (res[4] << 8) | res[5];
weebgrammers 0:cf7bca71c4e3 312 }
weebgrammers 0:cf7bca71c4e3 313 data.x = ((float) t[0]) * 0.1f;
weebgrammers 0:cf7bca71c4e3 314 data.y = ((float) t[1]) * 0.1f;
weebgrammers 0:cf7bca71c4e3 315 data.z = ((float) t[2]) * 0.1f;
weebgrammers 0:cf7bca71c4e3 316 }
weebgrammers 0:cf7bca71c4e3 317
weebgrammers 0:cf7bca71c4e3 318
weebgrammers 0:cf7bca71c4e3 319 void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
weebgrammers 0:cf7bca71c4e3 320
weebgrammers 0:cf7bca71c4e3 321 uint8_t res[6];
weebgrammers 0:cf7bca71c4e3 322 readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
weebgrammers 0:cf7bca71c4e3 323
weebgrammers 0:cf7bca71c4e3 324 data.x = (res[0] << 8) | res[1];
weebgrammers 0:cf7bca71c4e3 325 data.y = (res[2] << 8) | res[3];
weebgrammers 0:cf7bca71c4e3 326 data.z = (res[4] << 8) | res[5];
weebgrammers 0:cf7bca71c4e3 327 }
weebgrammers 0:cf7bca71c4e3 328
weebgrammers 0:cf7bca71c4e3 329 void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) {
weebgrammers 0:cf7bca71c4e3 330 char t[1] = {addr};
weebgrammers 0:cf7bca71c4e3 331 m_i2c.write(m_addr, t, 1, true);
weebgrammers 0:cf7bca71c4e3 332 m_i2c.read(m_addr, (char *)data, len);
weebgrammers 0:cf7bca71c4e3 333 }
weebgrammers 0:cf7bca71c4e3 334
weebgrammers 0:cf7bca71c4e3 335 void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) {
weebgrammers 0:cf7bca71c4e3 336 m_i2c.write(m_addr, (char *)data, len);
weebgrammers 0:cf7bca71c4e3 337 }
weebgrammers 0:cf7bca71c4e3 338
weebgrammers 0:cf7bca71c4e3 339
weebgrammers 0:cf7bca71c4e3 340 int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) {
weebgrammers 0:cf7bca71c4e3 341 int16_t acc;
weebgrammers 0:cf7bca71c4e3 342 uint8_t res[2];
weebgrammers 0:cf7bca71c4e3 343 readRegs(addr, res, 2);
weebgrammers 0:cf7bca71c4e3 344
weebgrammers 0:cf7bca71c4e3 345 acc = (res[0] << 8) | res[1];
weebgrammers 0:cf7bca71c4e3 346
weebgrammers 0:cf7bca71c4e3 347 return acc;
weebgrammers 0:cf7bca71c4e3 348 }