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 uint8_t SensorBuffer[12];
00023 int     MagReadStatus;
00024 
00025 FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
00026     // activate the peripheral
00027     uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
00028     m_i2c.frequency(400000);
00029     writeRegs(data, 2);
00030     data[0] = FXOS8700Q_M_CTRL_REG1;
00031     data[1] = 0x1F;
00032     writeRegs(data, 2);
00033     data[0] = FXOS8700Q_M_CTRL_REG2;
00034     data[1] = 0x20;
00035     writeRegs(data, 2);
00036     data[0] = FXOS8700Q_XYZ_DATA_CFG;
00037     data[1] = 0x00;
00038     writeRegs(data, 2);
00039     data[0] = FXOS8700Q_CTRL_REG1;
00040     data[1] = 0x1C;
00041     writeRegs(data, 2);
00042     MagReadStatus = 0;
00043 }
00044 
00045 FXOS8700Q_acc::~FXOS8700Q_acc() { }
00046 
00047 void FXOS8700Q_acc::enable(void) {
00048     uint8_t data[2];
00049     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00050     data[1] |= 0x01;
00051     data[0] = FXOS8700Q_CTRL_REG1;
00052     writeRegs(data, 2);
00053     MagReadStatus = 0;
00054 }
00055 
00056 void FXOS8700Q_acc::disable(void) {
00057     uint8_t data[2];
00058     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00059     data[1] &= 0xFE;
00060     data[0] = FXOS8700Q_CTRL_REG1;
00061     writeRegs(data, 2);
00062     MagReadStatus = 0;
00063 }
00064 
00065 
00066 
00067 uint32_t FXOS8700Q_acc::whoAmI() {
00068     uint8_t who_am_i = 0;
00069     readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
00070     return (uint32_t) who_am_i;
00071 }
00072 
00073 uint32_t FXOS8700Q_acc::dataReady(void) {
00074     uint8_t stat = 0;
00075     readRegs(FXOS8700Q_STATUS, &stat, 1);
00076     return (uint32_t) stat;
00077 }
00078 
00079 uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) {
00080     return(50); // for now sample rate is fixed at 50Hz
00081 }
00082 
00083 void FXOS8700Q_acc::getX(float * x) {
00084     *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f);
00085 }
00086 
00087 void FXOS8700Q_acc::getY(float * y) {
00088     *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f);
00089 }
00090 
00091 void FXOS8700Q_acc::getZ(float * z) {
00092     *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f);
00093 }
00094 
00095 void FXOS8700Q_acc::getX(int16_t * d) {
00096     *d = getAccAxis(FXOS8700Q_OUT_X_MSB);
00097 }
00098 
00099 void FXOS8700Q_acc::getY(int16_t * d) {
00100     *d = getAccAxis(FXOS8700Q_OUT_Y_MSB);
00101 }
00102 
00103 void FXOS8700Q_acc::getZ(int16_t * d) {
00104     *d = getAccAxis(FXOS8700Q_OUT_Z_MSB);
00105 }
00106 
00107 
00108 void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
00109     int16_t acc, t[3];
00110 
00111    readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
00112 
00113     acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
00114     if (acc > UINT14_MAX/2)
00115         acc -= UINT14_MAX;
00116     t[0] = acc;
00117     acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
00118     if (acc > UINT14_MAX/2)
00119         acc -= UINT14_MAX;
00120     t[1] = acc;
00121     acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
00122     if (acc > UINT14_MAX/2)
00123         acc -= UINT14_MAX;
00124     t[2] = acc;
00125     data.x = ((float) t[0]) / 4096.0f;
00126     data.y = ((float) t[1]) / 4096.0f;
00127     data.z = ((float) t[2]) / 4096.0f;
00128     MagReadStatus = 1;
00129 }
00130 
00131 
00132 void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
00133     int16_t acc;
00134     readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
00135 
00136     acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
00137     if (acc > UINT14_MAX/2)
00138         acc -= UINT14_MAX;
00139     data.x = acc;
00140     acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
00141     if (acc > UINT14_MAX/2)
00142         acc -= UINT14_MAX;
00143     data.y = acc;
00144     acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
00145     if (acc > UINT14_MAX/2)
00146         acc -= UINT14_MAX;
00147     data.z = acc;
00148     MagReadStatus = 1;
00149 }
00150 
00151 void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
00152     char t[1] = {addr};
00153     m_i2c.write(m_addr, t, 1, true);
00154     m_i2c.read(m_addr, (char *)data, len);
00155 }
00156 
00157 void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) {
00158     m_i2c.write(m_addr, (char *)data, len);
00159 }
00160 
00161 
00162 int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) {
00163     int16_t acc;
00164     uint8_t res[2];
00165     readRegs(addr, res, 2);
00166 
00167     acc = (res[0] << 6) | (res[1] >> 2);
00168     if (acc > UINT14_MAX/2)
00169         acc -= UINT14_MAX;
00170 
00171     return acc;
00172 }
00173 
00174 
00175 
00176 FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
00177     // activate the peripheral
00178     uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
00179     m_i2c.frequency(400000);
00180     writeRegs(data, 2);
00181     data[0] = FXOS8700Q_M_CTRL_REG1;
00182     data[1] = 0x1F;
00183     writeRegs(data, 2);
00184     data[0] = FXOS8700Q_M_CTRL_REG2;
00185     data[1] = 0x20;
00186     writeRegs(data, 2);
00187     data[0] = FXOS8700Q_XYZ_DATA_CFG;
00188     data[1] = 0x00;
00189     writeRegs(data, 2);
00190     data[0] = FXOS8700Q_CTRL_REG1;
00191     data[1] = 0x18;//0x1D;
00192     writeRegs(data, 2);
00193     MagReadStatus = 0;
00194 }
00195 
00196 FXOS8700Q_mag::~FXOS8700Q_mag() { }
00197 
00198 void FXOS8700Q_mag::enable(void) {
00199     uint8_t data[2];
00200     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00201     data[1] |= 0x01;
00202     data[0] = FXOS8700Q_CTRL_REG1;
00203     writeRegs(data, 2);
00204     MagReadStatus = 0;
00205 }
00206 
00207 void FXOS8700Q_mag::disable(void) {
00208     uint8_t data[2];
00209     readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
00210     data[1] &= 0xFE;
00211     data[0] = FXOS8700Q_CTRL_REG1;
00212     writeRegs(data, 2);
00213     MagReadStatus = 0;
00214 }
00215 
00216 
00217 
00218 uint32_t FXOS8700Q_mag::whoAmI() {
00219     uint8_t who_am_i = 0;
00220     readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
00221     return (uint32_t) who_am_i;
00222 }
00223 
00224 uint32_t FXOS8700Q_mag::dataReady(void) {
00225     uint8_t stat = 0;
00226     readRegs(FXOS8700Q_STATUS, &stat, 1);
00227     return (uint32_t) stat;
00228 }
00229 
00230 uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) {
00231     return(50); // for now sample rate is fixed at 50Hz
00232 }
00233 
00234 void FXOS8700Q_mag::getX(float * x) {
00235     *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f);
00236 }
00237 
00238 void FXOS8700Q_mag::getY(float * y) {
00239     *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f);
00240 }
00241 
00242 void FXOS8700Q_mag::getZ(float * z) {
00243     *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f);
00244 }
00245 
00246 void FXOS8700Q_mag::getX(int16_t * d) {
00247     *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB);
00248 }
00249 
00250 void FXOS8700Q_mag::getY(int16_t * d) {
00251     *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB);
00252 }
00253 
00254 void FXOS8700Q_mag::getZ(int16_t * d) {
00255     *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB);
00256 }
00257 
00258 
00259 void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
00260     int16_t t[3];
00261     uint8_t res[6];
00262     
00263    if(MagReadStatus) {
00264         t[0] = (SensorBuffer[6] << 8) | SensorBuffer[7];
00265         t[1] = (SensorBuffer[8] << 8) | SensorBuffer[9];
00266         t[2] = (SensorBuffer[10] << 8) | SensorBuffer[11];        
00267         } else {
00268         readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
00269         t[0] = (res[0] << 8) | res[1];
00270         t[1] = (res[2] << 8) | res[3];
00271         t[2] = (res[4] << 8) | res[5];
00272         }
00273     data.x = ((float) t[0]) * 0.1f;
00274     data.y = ((float) t[1]) * 0.1f;
00275     data.z = ((float) t[2]) * 0.1f;
00276 }
00277 
00278 
00279 void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
00280 
00281     uint8_t res[6];
00282     readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
00283 
00284     data.x = (res[0] << 8) | res[1];
00285     data.y = (res[2] << 8) | res[3];
00286     data.z = (res[4] << 8) | res[5];
00287 }
00288 
00289 void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) {
00290     char t[1] = {addr};
00291     m_i2c.write(m_addr, t, 1, true);
00292     m_i2c.read(m_addr, (char *)data, len);
00293 }
00294 
00295 void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) {
00296     m_i2c.write(m_addr, (char *)data, len);
00297 }
00298 
00299 
00300 int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) {
00301     int16_t acc;
00302     uint8_t res[2];
00303     readRegs(addr, res, 2);
00304 
00305     acc = (res[0] << 8) | res[1];
00306 
00307     return acc;
00308 }