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