Modification to FXOS8700Q for use in Data Logger project
Dependencies: FXOS8700Q MotionSensor
Dependents: GPS_6Axis_DataLogger_SD_UDP
Fork of FXOS8700Q by
FXOS8700Q.cpp@10:f9db0e031a48, 2015-03-27 (annotated)
- Committer:
- rlinnmoran
- Date:
- Fri Mar 27 08:32:03 2015 +0000
- Revision:
- 10:f9db0e031a48
- Parent:
- 9:5553a64d0762
Initial version.; ; Working:; - Capture GPS; - Capture 6 Axis Sensor; - Write to SD Card (crude); ; Issues:; - Update frequency is slow (5s); - SD functions needs improved; - Need status LED + start/stop button; - Clean up class + docs; - Param for printf's
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 | 9:5553a64d0762 | 22 | uint8_t SensorBuffer[12]; |
JimCarver | 9:5553a64d0762 | 23 | int MagReadStatus; |
JimCarver | 1:8b53edef272f | 24 | |
JimCarver | 5:c4176a12f7d5 | 25 | FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) { |
JimCarver | 1:8b53edef272f | 26 | // activate the peripheral |
JimCarver | 1:8b53edef272f | 27 | uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00}; |
JimCarver | 4:be6abf9f2d59 | 28 | m_i2c.frequency(400000); |
JimCarver | 1:8b53edef272f | 29 | writeRegs(data, 2); |
JimCarver | 1:8b53edef272f | 30 | data[0] = FXOS8700Q_M_CTRL_REG1; |
JimCarver | 1:8b53edef272f | 31 | data[1] = 0x1F; |
JimCarver | 1:8b53edef272f | 32 | writeRegs(data, 2); |
JimCarver | 1:8b53edef272f | 33 | data[0] = FXOS8700Q_M_CTRL_REG2; |
JimCarver | 1:8b53edef272f | 34 | data[1] = 0x20; |
JimCarver | 1:8b53edef272f | 35 | writeRegs(data, 2); |
JimCarver | 1:8b53edef272f | 36 | data[0] = FXOS8700Q_XYZ_DATA_CFG; |
JimCarver | 1:8b53edef272f | 37 | data[1] = 0x00; |
JimCarver | 1:8b53edef272f | 38 | writeRegs(data, 2); |
JimCarver | 1:8b53edef272f | 39 | data[0] = FXOS8700Q_CTRL_REG1; |
JimCarver | 8:52116f8fb3da | 40 | data[1] = 0x1C; |
JimCarver | 6:cdc362f08339 | 41 | writeRegs(data, 2); |
JimCarver | 9:5553a64d0762 | 42 | MagReadStatus = 0; |
JimCarver | 6:cdc362f08339 | 43 | } |
JimCarver | 6:cdc362f08339 | 44 | |
JimCarver | 6:cdc362f08339 | 45 | FXOS8700Q_acc::~FXOS8700Q_acc() { } |
JimCarver | 6:cdc362f08339 | 46 | |
JimCarver | 6:cdc362f08339 | 47 | void FXOS8700Q_acc::enable(void) { |
JimCarver | 6:cdc362f08339 | 48 | uint8_t data[2]; |
JimCarver | 6:cdc362f08339 | 49 | readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); |
JimCarver | 6:cdc362f08339 | 50 | data[1] |= 0x01; |
JimCarver | 6:cdc362f08339 | 51 | data[0] = FXOS8700Q_CTRL_REG1; |
JimCarver | 6:cdc362f08339 | 52 | writeRegs(data, 2); |
JimCarver | 9:5553a64d0762 | 53 | MagReadStatus = 0; |
JimCarver | 6:cdc362f08339 | 54 | } |
JimCarver | 6:cdc362f08339 | 55 | |
JimCarver | 6:cdc362f08339 | 56 | void FXOS8700Q_acc::disable(void) { |
JimCarver | 6:cdc362f08339 | 57 | uint8_t data[2]; |
JimCarver | 6:cdc362f08339 | 58 | readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); |
JimCarver | 6:cdc362f08339 | 59 | data[1] &= 0xFE; |
JimCarver | 6:cdc362f08339 | 60 | data[0] = FXOS8700Q_CTRL_REG1; |
JimCarver | 1:8b53edef272f | 61 | writeRegs(data, 2); |
JimCarver | 9:5553a64d0762 | 62 | MagReadStatus = 0; |
JimCarver | 1:8b53edef272f | 63 | } |
JimCarver | 0:2562215f5bc0 | 64 | |
JimCarver | 0:2562215f5bc0 | 65 | |
JimCarver | 1:8b53edef272f | 66 | |
JimCarver | 6:cdc362f08339 | 67 | uint32_t FXOS8700Q_acc::whoAmI() { |
JimCarver | 1:8b53edef272f | 68 | uint8_t who_am_i = 0; |
JimCarver | 1:8b53edef272f | 69 | readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1); |
JimCarver | 6:cdc362f08339 | 70 | return (uint32_t) who_am_i; |
JimCarver | 6:cdc362f08339 | 71 | } |
JimCarver | 6:cdc362f08339 | 72 | |
JimCarver | 6:cdc362f08339 | 73 | uint32_t FXOS8700Q_acc::dataReady(void) { |
JimCarver | 6:cdc362f08339 | 74 | uint8_t stat = 0; |
JimCarver | 6:cdc362f08339 | 75 | readRegs(FXOS8700Q_STATUS, &stat, 1); |
JimCarver | 6:cdc362f08339 | 76 | return (uint32_t) stat; |
JimCarver | 6:cdc362f08339 | 77 | } |
JimCarver | 6:cdc362f08339 | 78 | |
JimCarver | 6:cdc362f08339 | 79 | uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) { |
JimCarver | 6:cdc362f08339 | 80 | return(50); // for now sample rate is fixed at 50Hz |
JimCarver | 1:8b53edef272f | 81 | } |
JimCarver | 1:8b53edef272f | 82 | |
JimCarver | 6:cdc362f08339 | 83 | void FXOS8700Q_acc::getX(float * x) { |
JimCarver | 6:cdc362f08339 | 84 | *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f); |
JimCarver | 6:cdc362f08339 | 85 | } |
JimCarver | 6:cdc362f08339 | 86 | |
JimCarver | 6:cdc362f08339 | 87 | void FXOS8700Q_acc::getY(float * y) { |
JimCarver | 6:cdc362f08339 | 88 | *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f); |
JimCarver | 0:2562215f5bc0 | 89 | } |
JimCarver | 1:8b53edef272f | 90 | |
JimCarver | 6:cdc362f08339 | 91 | void FXOS8700Q_acc::getZ(float * z) { |
JimCarver | 6:cdc362f08339 | 92 | *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f); |
JimCarver | 1:8b53edef272f | 93 | } |
JimCarver | 1:8b53edef272f | 94 | |
JimCarver | 6:cdc362f08339 | 95 | void FXOS8700Q_acc::getX(int16_t * d) { |
JimCarver | 6:cdc362f08339 | 96 | *d = getAccAxis(FXOS8700Q_OUT_X_MSB); |
JimCarver | 6:cdc362f08339 | 97 | } |
JimCarver | 6:cdc362f08339 | 98 | |
JimCarver | 6:cdc362f08339 | 99 | void FXOS8700Q_acc::getY(int16_t * d) { |
JimCarver | 6:cdc362f08339 | 100 | *d = getAccAxis(FXOS8700Q_OUT_Y_MSB); |
JimCarver | 6:cdc362f08339 | 101 | } |
JimCarver | 6:cdc362f08339 | 102 | |
JimCarver | 6:cdc362f08339 | 103 | void FXOS8700Q_acc::getZ(int16_t * d) { |
JimCarver | 6:cdc362f08339 | 104 | *d = getAccAxis(FXOS8700Q_OUT_Z_MSB); |
JimCarver | 1:8b53edef272f | 105 | } |
JimCarver | 1:8b53edef272f | 106 | |
JimCarver | 1:8b53edef272f | 107 | |
JimCarver | 6:cdc362f08339 | 108 | void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) { |
JimCarver | 6:cdc362f08339 | 109 | int16_t acc, t[3]; |
JimCarver | 6:cdc362f08339 | 110 | |
JimCarver | 9:5553a64d0762 | 111 | readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12); |
JimCarver | 9:5553a64d0762 | 112 | |
JimCarver | 9:5553a64d0762 | 113 | acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2); |
JimCarver | 6:cdc362f08339 | 114 | if (acc > UINT14_MAX/2) |
JimCarver | 6:cdc362f08339 | 115 | acc -= UINT14_MAX; |
JimCarver | 6:cdc362f08339 | 116 | t[0] = acc; |
JimCarver | 9:5553a64d0762 | 117 | acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2); |
JimCarver | 6:cdc362f08339 | 118 | if (acc > UINT14_MAX/2) |
JimCarver | 6:cdc362f08339 | 119 | acc -= UINT14_MAX; |
JimCarver | 6:cdc362f08339 | 120 | t[1] = acc; |
JimCarver | 9:5553a64d0762 | 121 | acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2); |
JimCarver | 6:cdc362f08339 | 122 | if (acc > UINT14_MAX/2) |
JimCarver | 6:cdc362f08339 | 123 | acc -= UINT14_MAX; |
JimCarver | 6:cdc362f08339 | 124 | t[2] = acc; |
JimCarver | 6:cdc362f08339 | 125 | data.x = ((float) t[0]) / 4096.0f; |
JimCarver | 6:cdc362f08339 | 126 | data.y = ((float) t[1]) / 4096.0f; |
JimCarver | 6:cdc362f08339 | 127 | data.z = ((float) t[2]) / 4096.0f; |
JimCarver | 9:5553a64d0762 | 128 | MagReadStatus = 1; |
JimCarver | 0:2562215f5bc0 | 129 | } |
JimCarver | 0:2562215f5bc0 | 130 | |
JimCarver | 6:cdc362f08339 | 131 | |
JimCarver | 6:cdc362f08339 | 132 | void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) { |
JimCarver | 1:8b53edef272f | 133 | int16_t acc; |
JimCarver | 9:5553a64d0762 | 134 | readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12); |
JimCarver | 0:2562215f5bc0 | 135 | |
JimCarver | 9:5553a64d0762 | 136 | acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2); |
JimCarver | 1:8b53edef272f | 137 | if (acc > UINT14_MAX/2) |
JimCarver | 1:8b53edef272f | 138 | acc -= UINT14_MAX; |
JimCarver | 6:cdc362f08339 | 139 | data.x = acc; |
JimCarver | 9:5553a64d0762 | 140 | acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2); |
JimCarver | 1:8b53edef272f | 141 | if (acc > UINT14_MAX/2) |
JimCarver | 1:8b53edef272f | 142 | acc -= UINT14_MAX; |
JimCarver | 6:cdc362f08339 | 143 | data.y = acc; |
JimCarver | 9:5553a64d0762 | 144 | acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2); |
JimCarver | 1:8b53edef272f | 145 | if (acc > UINT14_MAX/2) |
JimCarver | 1:8b53edef272f | 146 | acc -= UINT14_MAX; |
JimCarver | 6:cdc362f08339 | 147 | data.z = acc; |
JimCarver | 9:5553a64d0762 | 148 | MagReadStatus = 1; |
JimCarver | 0:2562215f5bc0 | 149 | } |
JimCarver | 0:2562215f5bc0 | 150 | |
JimCarver | 6:cdc362f08339 | 151 | void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) { |
JimCarver | 6:cdc362f08339 | 152 | char t[1] = {addr}; |
JimCarver | 6:cdc362f08339 | 153 | m_i2c.write(m_addr, t, 1, true); |
JimCarver | 6:cdc362f08339 | 154 | m_i2c.read(m_addr, (char *)data, len); |
JimCarver | 1:8b53edef272f | 155 | } |
JimCarver | 1:8b53edef272f | 156 | |
JimCarver | 6:cdc362f08339 | 157 | void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) { |
JimCarver | 6:cdc362f08339 | 158 | m_i2c.write(m_addr, (char *)data, len); |
JimCarver | 6:cdc362f08339 | 159 | } |
JimCarver | 6:cdc362f08339 | 160 | |
JimCarver | 6:cdc362f08339 | 161 | |
JimCarver | 5:c4176a12f7d5 | 162 | int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) { |
JimCarver | 1:8b53edef272f | 163 | int16_t acc; |
JimCarver | 1:8b53edef272f | 164 | uint8_t res[2]; |
JimCarver | 1:8b53edef272f | 165 | readRegs(addr, res, 2); |
JimCarver | 0:2562215f5bc0 | 166 | |
JimCarver | 1:8b53edef272f | 167 | acc = (res[0] << 6) | (res[1] >> 2); |
JimCarver | 1:8b53edef272f | 168 | if (acc > UINT14_MAX/2) |
JimCarver | 1:8b53edef272f | 169 | acc -= UINT14_MAX; |
JimCarver | 1:8b53edef272f | 170 | |
JimCarver | 1:8b53edef272f | 171 | return acc; |
JimCarver | 1:8b53edef272f | 172 | } |
JimCarver | 0:2562215f5bc0 | 173 | |
JimCarver | 6:cdc362f08339 | 174 | |
JimCarver | 6:cdc362f08339 | 175 | |
JimCarver | 6:cdc362f08339 | 176 | FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) { |
JimCarver | 6:cdc362f08339 | 177 | // activate the peripheral |
JimCarver | 6:cdc362f08339 | 178 | uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00}; |
JimCarver | 6:cdc362f08339 | 179 | m_i2c.frequency(400000); |
JimCarver | 6:cdc362f08339 | 180 | writeRegs(data, 2); |
JimCarver | 6:cdc362f08339 | 181 | data[0] = FXOS8700Q_M_CTRL_REG1; |
JimCarver | 6:cdc362f08339 | 182 | data[1] = 0x1F; |
JimCarver | 6:cdc362f08339 | 183 | writeRegs(data, 2); |
JimCarver | 6:cdc362f08339 | 184 | data[0] = FXOS8700Q_M_CTRL_REG2; |
JimCarver | 6:cdc362f08339 | 185 | data[1] = 0x20; |
JimCarver | 6:cdc362f08339 | 186 | writeRegs(data, 2); |
JimCarver | 6:cdc362f08339 | 187 | data[0] = FXOS8700Q_XYZ_DATA_CFG; |
JimCarver | 6:cdc362f08339 | 188 | data[1] = 0x00; |
JimCarver | 6:cdc362f08339 | 189 | writeRegs(data, 2); |
JimCarver | 6:cdc362f08339 | 190 | data[0] = FXOS8700Q_CTRL_REG1; |
JimCarver | 6:cdc362f08339 | 191 | data[1] = 0x18;//0x1D; |
JimCarver | 6:cdc362f08339 | 192 | writeRegs(data, 2); |
JimCarver | 9:5553a64d0762 | 193 | MagReadStatus = 0; |
JimCarver | 6:cdc362f08339 | 194 | } |
JimCarver | 6:cdc362f08339 | 195 | |
JimCarver | 6:cdc362f08339 | 196 | FXOS8700Q_mag::~FXOS8700Q_mag() { } |
JimCarver | 6:cdc362f08339 | 197 | |
JimCarver | 6:cdc362f08339 | 198 | void FXOS8700Q_mag::enable(void) { |
JimCarver | 6:cdc362f08339 | 199 | uint8_t data[2]; |
JimCarver | 6:cdc362f08339 | 200 | readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); |
JimCarver | 6:cdc362f08339 | 201 | data[1] |= 0x01; |
JimCarver | 6:cdc362f08339 | 202 | data[0] = FXOS8700Q_CTRL_REG1; |
JimCarver | 6:cdc362f08339 | 203 | writeRegs(data, 2); |
JimCarver | 9:5553a64d0762 | 204 | MagReadStatus = 0; |
JimCarver | 6:cdc362f08339 | 205 | } |
JimCarver | 6:cdc362f08339 | 206 | |
JimCarver | 6:cdc362f08339 | 207 | void FXOS8700Q_mag::disable(void) { |
JimCarver | 6:cdc362f08339 | 208 | uint8_t data[2]; |
JimCarver | 6:cdc362f08339 | 209 | readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1); |
JimCarver | 6:cdc362f08339 | 210 | data[1] &= 0xFE; |
JimCarver | 6:cdc362f08339 | 211 | data[0] = FXOS8700Q_CTRL_REG1; |
JimCarver | 6:cdc362f08339 | 212 | writeRegs(data, 2); |
JimCarver | 9:5553a64d0762 | 213 | MagReadStatus = 0; |
JimCarver | 6:cdc362f08339 | 214 | } |
JimCarver | 6:cdc362f08339 | 215 | |
JimCarver | 6:cdc362f08339 | 216 | |
JimCarver | 6:cdc362f08339 | 217 | |
JimCarver | 6:cdc362f08339 | 218 | uint32_t FXOS8700Q_mag::whoAmI() { |
JimCarver | 6:cdc362f08339 | 219 | uint8_t who_am_i = 0; |
JimCarver | 6:cdc362f08339 | 220 | readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1); |
JimCarver | 6:cdc362f08339 | 221 | return (uint32_t) who_am_i; |
JimCarver | 6:cdc362f08339 | 222 | } |
JimCarver | 6:cdc362f08339 | 223 | |
JimCarver | 6:cdc362f08339 | 224 | uint32_t FXOS8700Q_mag::dataReady(void) { |
JimCarver | 6:cdc362f08339 | 225 | uint8_t stat = 0; |
JimCarver | 6:cdc362f08339 | 226 | readRegs(FXOS8700Q_STATUS, &stat, 1); |
JimCarver | 6:cdc362f08339 | 227 | return (uint32_t) stat; |
JimCarver | 6:cdc362f08339 | 228 | } |
JimCarver | 6:cdc362f08339 | 229 | |
JimCarver | 6:cdc362f08339 | 230 | uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) { |
JimCarver | 6:cdc362f08339 | 231 | return(50); // for now sample rate is fixed at 50Hz |
JimCarver | 6:cdc362f08339 | 232 | } |
JimCarver | 6:cdc362f08339 | 233 | |
JimCarver | 6:cdc362f08339 | 234 | void FXOS8700Q_mag::getX(float * x) { |
JimCarver | 7:c53dda05b8cf | 235 | *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f); |
JimCarver | 6:cdc362f08339 | 236 | } |
JimCarver | 6:cdc362f08339 | 237 | |
JimCarver | 6:cdc362f08339 | 238 | void FXOS8700Q_mag::getY(float * y) { |
JimCarver | 7:c53dda05b8cf | 239 | *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f); |
JimCarver | 6:cdc362f08339 | 240 | } |
JimCarver | 6:cdc362f08339 | 241 | |
JimCarver | 6:cdc362f08339 | 242 | void FXOS8700Q_mag::getZ(float * z) { |
JimCarver | 7:c53dda05b8cf | 243 | *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f); |
JimCarver | 6:cdc362f08339 | 244 | } |
JimCarver | 6:cdc362f08339 | 245 | |
JimCarver | 6:cdc362f08339 | 246 | void FXOS8700Q_mag::getX(int16_t * d) { |
JimCarver | 6:cdc362f08339 | 247 | *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB); |
JimCarver | 6:cdc362f08339 | 248 | } |
JimCarver | 6:cdc362f08339 | 249 | |
JimCarver | 6:cdc362f08339 | 250 | void FXOS8700Q_mag::getY(int16_t * d) { |
JimCarver | 6:cdc362f08339 | 251 | *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB); |
JimCarver | 6:cdc362f08339 | 252 | } |
JimCarver | 6:cdc362f08339 | 253 | |
JimCarver | 6:cdc362f08339 | 254 | void FXOS8700Q_mag::getZ(int16_t * d) { |
JimCarver | 6:cdc362f08339 | 255 | *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB); |
JimCarver | 6:cdc362f08339 | 256 | } |
JimCarver | 6:cdc362f08339 | 257 | |
JimCarver | 6:cdc362f08339 | 258 | |
JimCarver | 6:cdc362f08339 | 259 | void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) { |
JimCarver | 6:cdc362f08339 | 260 | int16_t t[3]; |
JimCarver | 6:cdc362f08339 | 261 | uint8_t res[6]; |
JimCarver | 9:5553a64d0762 | 262 | |
JimCarver | 9:5553a64d0762 | 263 | if(MagReadStatus) { |
JimCarver | 9:5553a64d0762 | 264 | t[0] = (SensorBuffer[6] << 8) | SensorBuffer[7]; |
JimCarver | 9:5553a64d0762 | 265 | t[1] = (SensorBuffer[8] << 8) | SensorBuffer[9]; |
JimCarver | 9:5553a64d0762 | 266 | t[2] = (SensorBuffer[10] << 8) | SensorBuffer[11]; |
JimCarver | 9:5553a64d0762 | 267 | } else { |
JimCarver | 9:5553a64d0762 | 268 | readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6); |
JimCarver | 9:5553a64d0762 | 269 | t[0] = (res[0] << 8) | res[1]; |
JimCarver | 9:5553a64d0762 | 270 | t[1] = (res[2] << 8) | res[3]; |
JimCarver | 9:5553a64d0762 | 271 | t[2] = (res[4] << 8) | res[5]; |
JimCarver | 9:5553a64d0762 | 272 | } |
JimCarver | 6:cdc362f08339 | 273 | data.x = ((float) t[0]) * 0.1f; |
JimCarver | 6:cdc362f08339 | 274 | data.y = ((float) t[1]) * 0.1f; |
JimCarver | 6:cdc362f08339 | 275 | data.z = ((float) t[2]) * 0.1f; |
JimCarver | 6:cdc362f08339 | 276 | } |
JimCarver | 6:cdc362f08339 | 277 | |
JimCarver | 6:cdc362f08339 | 278 | |
JimCarver | 6:cdc362f08339 | 279 | void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) { |
JimCarver | 9:5553a64d0762 | 280 | |
JimCarver | 6:cdc362f08339 | 281 | uint8_t res[6]; |
JimCarver | 7:c53dda05b8cf | 282 | readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6); |
JimCarver | 6:cdc362f08339 | 283 | |
JimCarver | 6:cdc362f08339 | 284 | data.x = (res[0] << 8) | res[1]; |
JimCarver | 6:cdc362f08339 | 285 | data.y = (res[2] << 8) | res[3]; |
JimCarver | 6:cdc362f08339 | 286 | data.z = (res[4] << 8) | res[5]; |
JimCarver | 6:cdc362f08339 | 287 | } |
JimCarver | 6:cdc362f08339 | 288 | |
JimCarver | 6:cdc362f08339 | 289 | void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) { |
JimCarver | 1:8b53edef272f | 290 | char t[1] = {addr}; |
JimCarver | 1:8b53edef272f | 291 | m_i2c.write(m_addr, t, 1, true); |
JimCarver | 1:8b53edef272f | 292 | m_i2c.read(m_addr, (char *)data, len); |
JimCarver | 0:2562215f5bc0 | 293 | } |
JimCarver | 1:8b53edef272f | 294 | |
JimCarver | 6:cdc362f08339 | 295 | void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) { |
JimCarver | 1:8b53edef272f | 296 | m_i2c.write(m_addr, (char *)data, len); |
JimCarver | 1:8b53edef272f | 297 | } |
JimCarver | 1:8b53edef272f | 298 | |
JimCarver | 6:cdc362f08339 | 299 | |
JimCarver | 6:cdc362f08339 | 300 | int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) { |
JimCarver | 6:cdc362f08339 | 301 | int16_t acc; |
JimCarver | 6:cdc362f08339 | 302 | uint8_t res[2]; |
JimCarver | 6:cdc362f08339 | 303 | readRegs(addr, res, 2); |
JimCarver | 6:cdc362f08339 | 304 | |
JimCarver | 6:cdc362f08339 | 305 | acc = (res[0] << 8) | res[1]; |
JimCarver | 6:cdc362f08339 | 306 | |
JimCarver | 6:cdc362f08339 | 307 | return acc; |
JimCarver | 6:cdc362f08339 | 308 | } |