Thundersoft / Mbed OS FRDM_FXS_MULTI_B_for_TT_Mxx

Create a project for TT_Mxx.

FRDM_FXS_MULTI_B/FXOS8700Q.cpp@0:369a1b265ddb, 2019-03-21 (annotated)

Committer:
ThunderSoft
Date:
Thu Mar 21 09:03:32 2019 +0000
Revision:
0:369a1b265ddb
Add code for FRDM_FXS_MULTI_B

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThunderSoft 0:369a1b265ddb 1 /* Copyright (c) 2010-2011 mbed.org, MIT License
ThunderSoft 0:369a1b265ddb 2 *
ThunderSoft 0:369a1b265ddb 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
ThunderSoft 0:369a1b265ddb 4 * and associated documentation files (the "Software"), to deal in the Software without
ThunderSoft 0:369a1b265ddb 5 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
ThunderSoft 0:369a1b265ddb 6 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
ThunderSoft 0:369a1b265ddb 7 * Software is furnished to do so, subject to the following conditions:
ThunderSoft 0:369a1b265ddb 8 *
ThunderSoft 0:369a1b265ddb 9 * The above copyright notice and this permission notice shall be included in all copies or
ThunderSoft 0:369a1b265ddb 10 * substantial portions of the Software.
ThunderSoft 0:369a1b265ddb 11 *
ThunderSoft 0:369a1b265ddb 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
ThunderSoft 0:369a1b265ddb 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
ThunderSoft 0:369a1b265ddb 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
ThunderSoft 0:369a1b265ddb 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
ThunderSoft 0:369a1b265ddb 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
ThunderSoft 0:369a1b265ddb 17 */
ThunderSoft 0:369a1b265ddb 18
ThunderSoft 0:369a1b265ddb 19 #include "FXOS8700Q.h"
ThunderSoft 0:369a1b265ddb 20 #define UINT14_MAX 16383
ThunderSoft 0:369a1b265ddb 21
ThunderSoft 0:369a1b265ddb 22 uint8_t SensorBuffer[12];
ThunderSoft 0:369a1b265ddb 23 int MagReadStatus;
ThunderSoft 0:369a1b265ddb 24
ThunderSoft 0:369a1b265ddb 25 FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
ThunderSoft 0:369a1b265ddb 26 // activate the peripheral
ThunderSoft 0:369a1b265ddb 27 uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
ThunderSoft 0:369a1b265ddb 28 m_i2c.frequency(400000);
ThunderSoft 0:369a1b265ddb 29 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 30 data[0] = FXOS8700Q_M_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 31 data[1] = 0x1F;
ThunderSoft 0:369a1b265ddb 32 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 33 data[0] = FXOS8700Q_M_CTRL_REG2;
ThunderSoft 0:369a1b265ddb 34 data[1] = 0x20;
ThunderSoft 0:369a1b265ddb 35 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 36 data[0] = FXOS8700Q_XYZ_DATA_CFG;
ThunderSoft 0:369a1b265ddb 37 data[1] = 0x00;
ThunderSoft 0:369a1b265ddb 38 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 39 data[0] = FXOS8700Q_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 40 data[1] = 0x1C;
ThunderSoft 0:369a1b265ddb 41 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 42 MagReadStatus = 0;
ThunderSoft 0:369a1b265ddb 43 }
ThunderSoft 0:369a1b265ddb 44
ThunderSoft 0:369a1b265ddb 45 FXOS8700Q_acc::~FXOS8700Q_acc() { }
ThunderSoft 0:369a1b265ddb 46
ThunderSoft 0:369a1b265ddb 47 void FXOS8700Q_acc::enable(void) {
ThunderSoft 0:369a1b265ddb 48 uint8_t data[2];
ThunderSoft 0:369a1b265ddb 49 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
ThunderSoft 0:369a1b265ddb 50 data[1] |= 0x01;
ThunderSoft 0:369a1b265ddb 51 data[0] = FXOS8700Q_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 52 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 53 MagReadStatus = 0;
ThunderSoft 0:369a1b265ddb 54 }
ThunderSoft 0:369a1b265ddb 55
ThunderSoft 0:369a1b265ddb 56 void FXOS8700Q_acc::disable(void) {
ThunderSoft 0:369a1b265ddb 57 uint8_t data[2];
ThunderSoft 0:369a1b265ddb 58 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
ThunderSoft 0:369a1b265ddb 59 data[1] &= 0xFE;
ThunderSoft 0:369a1b265ddb 60 data[0] = FXOS8700Q_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 61 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 62 MagReadStatus = 0;
ThunderSoft 0:369a1b265ddb 63 }
ThunderSoft 0:369a1b265ddb 64
ThunderSoft 0:369a1b265ddb 65
ThunderSoft 0:369a1b265ddb 66
ThunderSoft 0:369a1b265ddb 67 uint32_t FXOS8700Q_acc::whoAmI() {
ThunderSoft 0:369a1b265ddb 68 uint8_t who_am_i = 0;
ThunderSoft 0:369a1b265ddb 69 readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
ThunderSoft 0:369a1b265ddb 70 return (uint32_t) who_am_i;
ThunderSoft 0:369a1b265ddb 71 }
ThunderSoft 0:369a1b265ddb 72
ThunderSoft 0:369a1b265ddb 73 uint32_t FXOS8700Q_acc::dataReady(void) {
ThunderSoft 0:369a1b265ddb 74 uint8_t stat = 0;
ThunderSoft 0:369a1b265ddb 75 readRegs(FXOS8700Q_STATUS, &stat, 1);
ThunderSoft 0:369a1b265ddb 76 return (uint32_t) stat;
ThunderSoft 0:369a1b265ddb 77 }
ThunderSoft 0:369a1b265ddb 78
ThunderSoft 0:369a1b265ddb 79 uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) {
ThunderSoft 0:369a1b265ddb 80 return(50); // for now sample rate is fixed at 50Hz
ThunderSoft 0:369a1b265ddb 81 }
ThunderSoft 0:369a1b265ddb 82
ThunderSoft 0:369a1b265ddb 83 void FXOS8700Q_acc::getX(float * x) {
ThunderSoft 0:369a1b265ddb 84 *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f);
ThunderSoft 0:369a1b265ddb 85 }
ThunderSoft 0:369a1b265ddb 86
ThunderSoft 0:369a1b265ddb 87 void FXOS8700Q_acc::getY(float * y) {
ThunderSoft 0:369a1b265ddb 88 *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f);
ThunderSoft 0:369a1b265ddb 89 }
ThunderSoft 0:369a1b265ddb 90
ThunderSoft 0:369a1b265ddb 91 void FXOS8700Q_acc::getZ(float * z) {
ThunderSoft 0:369a1b265ddb 92 *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f);
ThunderSoft 0:369a1b265ddb 93 }
ThunderSoft 0:369a1b265ddb 94
ThunderSoft 0:369a1b265ddb 95 void FXOS8700Q_acc::getX(int16_t * d) {
ThunderSoft 0:369a1b265ddb 96 *d = getAccAxis(FXOS8700Q_OUT_X_MSB);
ThunderSoft 0:369a1b265ddb 97 }
ThunderSoft 0:369a1b265ddb 98
ThunderSoft 0:369a1b265ddb 99 void FXOS8700Q_acc::getY(int16_t * d) {
ThunderSoft 0:369a1b265ddb 100 *d = getAccAxis(FXOS8700Q_OUT_Y_MSB);
ThunderSoft 0:369a1b265ddb 101 }
ThunderSoft 0:369a1b265ddb 102
ThunderSoft 0:369a1b265ddb 103 void FXOS8700Q_acc::getZ(int16_t * d) {
ThunderSoft 0:369a1b265ddb 104 *d = getAccAxis(FXOS8700Q_OUT_Z_MSB);
ThunderSoft 0:369a1b265ddb 105 }
ThunderSoft 0:369a1b265ddb 106
ThunderSoft 0:369a1b265ddb 107
ThunderSoft 0:369a1b265ddb 108 void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
ThunderSoft 0:369a1b265ddb 109 int16_t acc, t[3];
ThunderSoft 0:369a1b265ddb 110
ThunderSoft 0:369a1b265ddb 111 readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
ThunderSoft 0:369a1b265ddb 112
ThunderSoft 0:369a1b265ddb 113 acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
ThunderSoft 0:369a1b265ddb 114 if (acc > UINT14_MAX/2)
ThunderSoft 0:369a1b265ddb 115 acc -= UINT14_MAX;
ThunderSoft 0:369a1b265ddb 116 t[0] = acc;
ThunderSoft 0:369a1b265ddb 117 acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
ThunderSoft 0:369a1b265ddb 118 if (acc > UINT14_MAX/2)
ThunderSoft 0:369a1b265ddb 119 acc -= UINT14_MAX;
ThunderSoft 0:369a1b265ddb 120 t[1] = acc;
ThunderSoft 0:369a1b265ddb 121 acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
ThunderSoft 0:369a1b265ddb 122 if (acc > UINT14_MAX/2)
ThunderSoft 0:369a1b265ddb 123 acc -= UINT14_MAX;
ThunderSoft 0:369a1b265ddb 124 t[2] = acc;
ThunderSoft 0:369a1b265ddb 125 data.x = ((float) t[0]) / 4096.0f;
ThunderSoft 0:369a1b265ddb 126 data.y = ((float) t[1]) / 4096.0f;
ThunderSoft 0:369a1b265ddb 127 data.z = ((float) t[2]) / 4096.0f;
ThunderSoft 0:369a1b265ddb 128 MagReadStatus = 1;
ThunderSoft 0:369a1b265ddb 129 }
ThunderSoft 0:369a1b265ddb 130
ThunderSoft 0:369a1b265ddb 131
ThunderSoft 0:369a1b265ddb 132 void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
ThunderSoft 0:369a1b265ddb 133 int16_t acc;
ThunderSoft 0:369a1b265ddb 134 readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
ThunderSoft 0:369a1b265ddb 135
ThunderSoft 0:369a1b265ddb 136 acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
ThunderSoft 0:369a1b265ddb 137 if (acc > UINT14_MAX/2)
ThunderSoft 0:369a1b265ddb 138 acc -= UINT14_MAX;
ThunderSoft 0:369a1b265ddb 139 data.x = acc;
ThunderSoft 0:369a1b265ddb 140 acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
ThunderSoft 0:369a1b265ddb 141 if (acc > UINT14_MAX/2)
ThunderSoft 0:369a1b265ddb 142 acc -= UINT14_MAX;
ThunderSoft 0:369a1b265ddb 143 data.y = acc;
ThunderSoft 0:369a1b265ddb 144 acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
ThunderSoft 0:369a1b265ddb 145 if (acc > UINT14_MAX/2)
ThunderSoft 0:369a1b265ddb 146 acc -= UINT14_MAX;
ThunderSoft 0:369a1b265ddb 147 data.z = acc;
ThunderSoft 0:369a1b265ddb 148 MagReadStatus = 1;
ThunderSoft 0:369a1b265ddb 149 }
ThunderSoft 0:369a1b265ddb 150
ThunderSoft 0:369a1b265ddb 151 void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
ThunderSoft 0:369a1b265ddb 152 char t[1] = {addr};
ThunderSoft 0:369a1b265ddb 153 m_i2c.write(m_addr, t, 1,true);
ThunderSoft 0:369a1b265ddb 154 m_i2c.read(m_addr, (char *)data, len);
ThunderSoft 0:369a1b265ddb 155 }
ThunderSoft 0:369a1b265ddb 156
ThunderSoft 0:369a1b265ddb 157 void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) {
ThunderSoft 0:369a1b265ddb 158 m_i2c.write(m_addr, (char *)data, len);
ThunderSoft 0:369a1b265ddb 159 }
ThunderSoft 0:369a1b265ddb 160
ThunderSoft 0:369a1b265ddb 161
ThunderSoft 0:369a1b265ddb 162 int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) {
ThunderSoft 0:369a1b265ddb 163 int16_t acc;
ThunderSoft 0:369a1b265ddb 164 uint8_t res[2];
ThunderSoft 0:369a1b265ddb 165 readRegs(addr, res, 2);
ThunderSoft 0:369a1b265ddb 166
ThunderSoft 0:369a1b265ddb 167 acc = (res[0] << 6) | (res[1] >> 2);
ThunderSoft 0:369a1b265ddb 168 if (acc > UINT14_MAX/2)
ThunderSoft 0:369a1b265ddb 169 acc -= UINT14_MAX;
ThunderSoft 0:369a1b265ddb 170
ThunderSoft 0:369a1b265ddb 171 return acc;
ThunderSoft 0:369a1b265ddb 172 }
ThunderSoft 0:369a1b265ddb 173
ThunderSoft 0:369a1b265ddb 174
ThunderSoft 0:369a1b265ddb 175
ThunderSoft 0:369a1b265ddb 176 FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
ThunderSoft 0:369a1b265ddb 177 // activate the peripheral
ThunderSoft 0:369a1b265ddb 178 uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
ThunderSoft 0:369a1b265ddb 179 m_i2c.frequency(400000);
ThunderSoft 0:369a1b265ddb 180 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 181 data[0] = FXOS8700Q_M_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 182 data[1] = 0x1F;
ThunderSoft 0:369a1b265ddb 183 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 184 data[0] = FXOS8700Q_M_CTRL_REG2;
ThunderSoft 0:369a1b265ddb 185 data[1] = 0x20;
ThunderSoft 0:369a1b265ddb 186 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 187 data[0] = FXOS8700Q_XYZ_DATA_CFG;
ThunderSoft 0:369a1b265ddb 188 data[1] = 0x00;
ThunderSoft 0:369a1b265ddb 189 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 190 data[0] = FXOS8700Q_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 191 data[1] = 0x18;//0x1D;
ThunderSoft 0:369a1b265ddb 192 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 193 MagReadStatus = 0;
ThunderSoft 0:369a1b265ddb 194 }
ThunderSoft 0:369a1b265ddb 195
ThunderSoft 0:369a1b265ddb 196 FXOS8700Q_mag::~FXOS8700Q_mag() { }
ThunderSoft 0:369a1b265ddb 197
ThunderSoft 0:369a1b265ddb 198 void FXOS8700Q_mag::enable(void) {
ThunderSoft 0:369a1b265ddb 199 uint8_t data[2];
ThunderSoft 0:369a1b265ddb 200 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
ThunderSoft 0:369a1b265ddb 201 data[1] |= 0x01;
ThunderSoft 0:369a1b265ddb 202 data[0] = FXOS8700Q_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 203 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 204 MagReadStatus = 0;
ThunderSoft 0:369a1b265ddb 205 }
ThunderSoft 0:369a1b265ddb 206
ThunderSoft 0:369a1b265ddb 207 void FXOS8700Q_mag::disable(void) {
ThunderSoft 0:369a1b265ddb 208 uint8_t data[2];
ThunderSoft 0:369a1b265ddb 209 readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
ThunderSoft 0:369a1b265ddb 210 data[1] &= 0xFE;
ThunderSoft 0:369a1b265ddb 211 data[0] = FXOS8700Q_CTRL_REG1;
ThunderSoft 0:369a1b265ddb 212 writeRegs(data, 2);
ThunderSoft 0:369a1b265ddb 213 MagReadStatus = 0;
ThunderSoft 0:369a1b265ddb 214 }
ThunderSoft 0:369a1b265ddb 215
ThunderSoft 0:369a1b265ddb 216
ThunderSoft 0:369a1b265ddb 217
ThunderSoft 0:369a1b265ddb 218 uint32_t FXOS8700Q_mag::whoAmI() {
ThunderSoft 0:369a1b265ddb 219 uint8_t who_am_i = 0;
ThunderSoft 0:369a1b265ddb 220 readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
ThunderSoft 0:369a1b265ddb 221 return (uint32_t) who_am_i;
ThunderSoft 0:369a1b265ddb 222 }
ThunderSoft 0:369a1b265ddb 223
ThunderSoft 0:369a1b265ddb 224 uint32_t FXOS8700Q_mag::dataReady(void) {
ThunderSoft 0:369a1b265ddb 225 uint8_t stat = 0;
ThunderSoft 0:369a1b265ddb 226 readRegs(FXOS8700Q_STATUS, &stat, 1);
ThunderSoft 0:369a1b265ddb 227 return (uint32_t) stat;
ThunderSoft 0:369a1b265ddb 228 }
ThunderSoft 0:369a1b265ddb 229
ThunderSoft 0:369a1b265ddb 230 uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) {
ThunderSoft 0:369a1b265ddb 231 return(50); // for now sample rate is fixed at 50Hz
ThunderSoft 0:369a1b265ddb 232 }
ThunderSoft 0:369a1b265ddb 233
ThunderSoft 0:369a1b265ddb 234 void FXOS8700Q_mag::getX(float * x) {
ThunderSoft 0:369a1b265ddb 235 *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f);
ThunderSoft 0:369a1b265ddb 236 }
ThunderSoft 0:369a1b265ddb 237
ThunderSoft 0:369a1b265ddb 238 void FXOS8700Q_mag::getY(float * y) {
ThunderSoft 0:369a1b265ddb 239 *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f);
ThunderSoft 0:369a1b265ddb 240 }
ThunderSoft 0:369a1b265ddb 241
ThunderSoft 0:369a1b265ddb 242 void FXOS8700Q_mag::getZ(float * z) {
ThunderSoft 0:369a1b265ddb 243 *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f);
ThunderSoft 0:369a1b265ddb 244 }
ThunderSoft 0:369a1b265ddb 245
ThunderSoft 0:369a1b265ddb 246 void FXOS8700Q_mag::getX(int16_t * d) {
ThunderSoft 0:369a1b265ddb 247 *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB);
ThunderSoft 0:369a1b265ddb 248 }
ThunderSoft 0:369a1b265ddb 249
ThunderSoft 0:369a1b265ddb 250 void FXOS8700Q_mag::getY(int16_t * d) {
ThunderSoft 0:369a1b265ddb 251 *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB);
ThunderSoft 0:369a1b265ddb 252 }
ThunderSoft 0:369a1b265ddb 253
ThunderSoft 0:369a1b265ddb 254 void FXOS8700Q_mag::getZ(int16_t * d) {
ThunderSoft 0:369a1b265ddb 255 *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB);
ThunderSoft 0:369a1b265ddb 256 }
ThunderSoft 0:369a1b265ddb 257
ThunderSoft 0:369a1b265ddb 258
ThunderSoft 0:369a1b265ddb 259 void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
ThunderSoft 0:369a1b265ddb 260 int16_t t[3];
ThunderSoft 0:369a1b265ddb 261 uint8_t res[6];
ThunderSoft 0:369a1b265ddb 262
ThunderSoft 0:369a1b265ddb 263 if(MagReadStatus) {
ThunderSoft 0:369a1b265ddb 264 t[0] = (SensorBuffer[6] << 8) | SensorBuffer[7];
ThunderSoft 0:369a1b265ddb 265 t[1] = (SensorBuffer[8] << 8) | SensorBuffer[9];
ThunderSoft 0:369a1b265ddb 266 t[2] = (SensorBuffer[10] << 8) | SensorBuffer[11];
ThunderSoft 0:369a1b265ddb 267 } else {
ThunderSoft 0:369a1b265ddb 268 readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
ThunderSoft 0:369a1b265ddb 269 t[0] = (res[0] << 8) | res[1];
ThunderSoft 0:369a1b265ddb 270 t[1] = (res[2] << 8) | res[3];
ThunderSoft 0:369a1b265ddb 271 t[2] = (res[4] << 8) | res[5];
ThunderSoft 0:369a1b265ddb 272 }
ThunderSoft 0:369a1b265ddb 273 data.x = ((float) t[0]) * 0.1f;
ThunderSoft 0:369a1b265ddb 274 data.y = ((float) t[1]) * 0.1f;
ThunderSoft 0:369a1b265ddb 275 data.z = ((float) t[2]) * 0.1f;
ThunderSoft 0:369a1b265ddb 276 }
ThunderSoft 0:369a1b265ddb 277
ThunderSoft 0:369a1b265ddb 278
ThunderSoft 0:369a1b265ddb 279 void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
ThunderSoft 0:369a1b265ddb 280
ThunderSoft 0:369a1b265ddb 281 uint8_t res[6];
ThunderSoft 0:369a1b265ddb 282 readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
ThunderSoft 0:369a1b265ddb 283
ThunderSoft 0:369a1b265ddb 284 data.x = (res[0] << 8) | res[1];
ThunderSoft 0:369a1b265ddb 285 data.y = (res[2] << 8) | res[3];
ThunderSoft 0:369a1b265ddb 286 data.z = (res[4] << 8) | res[5];
ThunderSoft 0:369a1b265ddb 287 }
ThunderSoft 0:369a1b265ddb 288
ThunderSoft 0:369a1b265ddb 289 void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) {
ThunderSoft 0:369a1b265ddb 290 char t[1] = {addr};
ThunderSoft 0:369a1b265ddb 291 m_i2c.write(m_addr, t, 1, true);
ThunderSoft 0:369a1b265ddb 292 m_i2c.read(m_addr, (char *)data, len);
ThunderSoft 0:369a1b265ddb 293 }
ThunderSoft 0:369a1b265ddb 294
ThunderSoft 0:369a1b265ddb 295 void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) {
ThunderSoft 0:369a1b265ddb 296 m_i2c.write(m_addr, (char *)data, len);
ThunderSoft 0:369a1b265ddb 297 }
ThunderSoft 0:369a1b265ddb 298
ThunderSoft 0:369a1b265ddb 299
ThunderSoft 0:369a1b265ddb 300 int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) {
ThunderSoft 0:369a1b265ddb 301 int16_t acc;
ThunderSoft 0:369a1b265ddb 302 uint8_t res[2];
ThunderSoft 0:369a1b265ddb 303 readRegs(addr, res, 2);
ThunderSoft 0:369a1b265ddb 304
ThunderSoft 0:369a1b265ddb 305 acc = (res[0] << 8) | res[1];
ThunderSoft 0:369a1b265ddb 306
ThunderSoft 0:369a1b265ddb 307 return acc;
ThunderSoft 0:369a1b265ddb 308 }
ThunderSoft 0:369a1b265ddb 309