FXOS8700Q.cpp

00001 /* Copyright (c) 2010-2011 mbed.org, MIT License
00002 *
00003 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00004 * and associated documentation files (the "Software"), to deal in the Software without
00005 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
00006 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
00007 * Software is furnished to do so, subject to the following conditions:
00008 *
00009 * The above copyright notice and this permission notice shall be included in all copies or
00010 * substantial portions of the Software.
00011 *
00012 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00013 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00014 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017 */
00018 
00019 #include "FXOS8700Q.h"
00020 #define UINT14_MAX        16383
00021 
00022 
00023 FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
00024     // activate the peripheral
00025     uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
00026     m_i2c.frequency(400000);
00027     writeRegs(data, 2);
00028     data[0] = FXOS8700Q_M_CTRL_REG1;
00029     data[1] = 0x1F;
00030     writeRegs(data, 2);
00031     data[0] = FXOS8700Q_M_CTRL_REG2;
00032     data[1] = 0x20;
00033     writeRegs(data, 2);
00034     data[0] = FXOS8700Q_XYZ_DATA_CFG;
00035     data[1] = 0x00;
00036     writeRegs(data, 2);
00037     data[0] = FXOS8700Q_CTRL_REG1;
00038     data[1] = 0x18;//0x1D;
00039     writeRegs(data, 2);
00040 }
00041 
00042 FXOS8700Q_acc::~FXOS8700Q_acc() { }
00043 
00044 void FXOS8700Q_acc::enable(void) {
00045     uint8_t data[2];
00046     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00047     data[1] |= 0x01;
00048     data[0] = FXOS8700Q_CTRL_REG1;
00049     writeRegs(data, 2);
00050 }
00051 
00052 void FXOS8700Q_acc::disable(void) {
00053     uint8_t data[2];
00054     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00055     data[1] &= 0xFE;
00056     data[0] = FXOS8700Q_CTRL_REG1;
00057     writeRegs(data, 2);
00058 }
00059 
00060 
00061 
00062 uint32_t FXOS8700Q_acc::whoAmI() {
00063     uint8_t who_am_i = 0;
00064     readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
00065     return (uint32_t) who_am_i;
00066 }
00067 
00068 uint32_t FXOS8700Q_acc::dataReady(void) {
00069     uint8_t stat = 0;
00070     readRegs(FXOS8700Q_STATUS, &stat, 1);
00071     return (uint32_t) stat;
00072 }
00073 
00074 uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) {
00075     return(50); // for now sample rate is fixed at 50Hz
00076 }
00077 
00078 void FXOS8700Q_acc::getX(float * x) {
00079     *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f);
00080 }
00081 
00082 void FXOS8700Q_acc::getY(float * y) {
00083     *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f);
00084 }
00085 
00086 void FXOS8700Q_acc::getZ(float * z) {
00087     *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f);
00088 }
00089 
00090 void FXOS8700Q_acc::getX(int16_t * d) {
00091     *d = getAccAxis(FXOS8700Q_OUT_X_MSB);
00092 }
00093 
00094 void FXOS8700Q_acc::getY(int16_t * d) {
00095     *d = getAccAxis(FXOS8700Q_OUT_Y_MSB);
00096 }
00097 
00098 void FXOS8700Q_acc::getZ(int16_t * d) {
00099     *d = getAccAxis(FXOS8700Q_OUT_Z_MSB);
00100 }
00101 
00102 
00103 void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
00104     int16_t acc, t[3];
00105     uint8_t res[6];
00106    readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
00107 
00108     acc = (res[0] << 6) | (res[1] >> 2);
00109     if (acc > UINT14_MAX/2)
00110         acc -= UINT14_MAX;
00111     t[0] = acc;
00112     acc = (res[2] << 6) | (res[3] >> 2);
00113     if (acc > UINT14_MAX/2)
00114         acc -= UINT14_MAX;
00115     t[1] = acc;
00116     acc = (res[4] << 6) | (res[5] >> 2);
00117     if (acc > UINT14_MAX/2)
00118         acc -= UINT14_MAX;
00119     t[2] = acc;
00120     data.x = ((float) t[0]) / 4096.0f;
00121     data.y = ((float) t[1]) / 4096.0f;
00122     data.z = ((float) t[2]) / 4096.0f;
00123 }
00124 
00125 
00126 void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
00127     int16_t acc;
00128     uint8_t res[6];
00129     readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
00130 
00131     acc = (res[0] << 6) | (res[1] >> 2);
00132     if (acc > UINT14_MAX/2)
00133         acc -= UINT14_MAX;
00134     data.x = acc;
00135     acc = (res[2] << 6) | (res[3] >> 2);
00136     if (acc > UINT14_MAX/2)
00137         acc -= UINT14_MAX;
00138     data.y = acc;
00139     acc = (res[4] << 6) | (res[5] >> 2);
00140     if (acc > UINT14_MAX/2)
00141         acc -= UINT14_MAX;
00142     data.z = acc;
00143 }
00144 
00145 void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
00146     char t[1] = {addr};
00147     m_i2c.write(m_addr, t, 1, true);
00148     m_i2c.read(m_addr, (char *)data, len);
00149 }
00150 
00151 void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) {
00152     m_i2c.write(m_addr, (char *)data, len);
00153 }
00154 
00155 
00156 int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) {
00157     int16_t acc;
00158     uint8_t res[2];
00159     readRegs(addr, res, 2);
00160 
00161     acc = (res[0] << 6) | (res[1] >> 2);
00162     if (acc > UINT14_MAX/2)
00163         acc -= UINT14_MAX;
00164 
00165     return acc;
00166 }
00167 
00168 
00169 
00170 FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
00171     // activate the peripheral
00172     uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
00173     m_i2c.frequency(400000);
00174     writeRegs(data, 2);
00175     data[0] = FXOS8700Q_M_CTRL_REG1;
00176     data[1] = 0x1F;
00177     writeRegs(data, 2);
00178     data[0] = FXOS8700Q_M_CTRL_REG2;
00179     data[1] = 0x20;
00180     writeRegs(data, 2);
00181     data[0] = FXOS8700Q_XYZ_DATA_CFG;
00182     data[1] = 0x00;
00183     writeRegs(data, 2);
00184     data[0] = FXOS8700Q_CTRL_REG1;
00185     data[1] = 0x18;//0x1D;
00186     writeRegs(data, 2);
00187 }
00188 
00189 FXOS8700Q_mag::~FXOS8700Q_mag() { }
00190 
00191 void FXOS8700Q_mag::enable(void) {
00192     uint8_t data[2];
00193     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00194     data[1] |= 0x01;
00195     data[0] = FXOS8700Q_CTRL_REG1;
00196     writeRegs(data, 2);
00197 }
00198 
00199 void FXOS8700Q_mag::disable(void) {
00200     uint8_t data[2];
00201     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00202     data[1] &= 0xFE;
00203     data[0] = FXOS8700Q_CTRL_REG1;
00204     writeRegs(data, 2);
00205 }
00206 
00207 
00208 
00209 uint32_t FXOS8700Q_mag::whoAmI() {
00210     uint8_t who_am_i = 0;
00211     readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
00212     return (uint32_t) who_am_i;
00213 }
00214 
00215 uint32_t FXOS8700Q_mag::dataReady(void) {
00216     uint8_t stat = 0;
00217     readRegs(FXOS8700Q_STATUS, &stat, 1);
00218     return (uint32_t) stat;
00219 }
00220 
00221 uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) {
00222     return(50); // for now sample rate is fixed at 50Hz
00223 }
00224 
00225 void FXOS8700Q_mag::getX(float * x) {
00226     *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f);
00227 }
00228 
00229 void FXOS8700Q_mag::getY(float * y) {
00230     *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f);
00231 }
00232 
00233 void FXOS8700Q_mag::getZ(float * z) {
00234     *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f);
00235 }
00236 
00237 void FXOS8700Q_mag::getX(int16_t * d) {
00238     *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB);
00239 }
00240 
00241 void FXOS8700Q_mag::getY(int16_t * d) {
00242     *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB);
00243 }
00244 
00245 void FXOS8700Q_mag::getZ(int16_t * d) {
00246     *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB);
00247 }
00248 
00249 
00250 void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
00251     int16_t t[3];
00252     uint8_t res[6];
00253    readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
00254 
00255     t[0] = (res[0] << 8) | res[1];
00256     t[1] = (res[2] << 8) | res[3];
00257     t[2] = (res[4] << 8) | res[5];
00258 
00259     data.x = ((float) t[0]) * 0.1f;
00260     data.y = ((float) t[1]) * 0.1f;
00261     data.z = ((float) t[2]) * 0.1f;
00262 }
00263 
00264 
00265 void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
00266     int16_t acc;
00267     uint8_t res[6];
00268     readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
00269 
00270     data.x = (res[0] << 8) | res[1];
00271     data.y = (res[2] << 8) | res[3];
00272     data.z = (res[4] << 8) | res[5];
00273 }
00274 
00275 void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) {
00276     char t[1] = {addr};
00277     m_i2c.write(m_addr, t, 1, true);
00278     m_i2c.read(m_addr, (char *)data, len);
00279 }
00280 
00281 void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) {
00282     m_i2c.write(m_addr, (char *)data, len);
00283 }
00284 
00285 
00286 int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) {
00287     int16_t acc;
00288     uint8_t res[2];
00289     readRegs(addr, res, 2);
00290 
00291     acc = (res[0] << 8) | res[1];
00292 
00293     return acc;
00294 }