QMC5883L.cpp

00001 /*   QMC5883L Digital Compass Library
00002 *
00003 *    @author: Baser Kandehir 
00004 *    @date: August 5, 2015
00005 *    @license: MIT license
00006 *     
00007 *   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
00008 *
00009 *   Permission is hereby granted, free of charge, to any person obtaining a copy
00010 *   of this software and associated documentation files (the "Software"), to deal
00011 *   in the Software without restriction, including without limitation the rights
00012 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00013 *   copies of the Software, and to permit persons to whom the Software is
00014 *   furnished to do so, subject to the following conditions:
00015 *
00016 *   The above copyright notice and this permission notice shall be included in
00017 *   all copies or substantial portions of the Software.
00018 *
00019 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00020 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00021 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00022 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00023 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00024 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00025 *   THE SOFTWARE.
00026 *
00027 */
00028 
00029 // Some part of the code is adapted from Adafruit HMC5883 library
00030 
00031 #include "QMC5883L.h"
00032 
00033 /* NUCLEO F411RE board */
00034 static I2C QMC5883L_i2c(D2, D4);         // setup i2c (SDA,SCL)  
00035 float mRes; // Varies with gain
00036  
00037 float QMC5883L::setMagRange(MagScale Mscale)
00038 {   
00039     switch(Mscale)
00040     {
00041         case MagScale_2G:
00042             mRes = 1.0/12000;  //LSB/G
00043             break;
00044         case MagScale_8G:
00045             mRes = 1.0/3000;
00046             break;
00047     } 
00048     return mRes;
00049 }
00050 
00051 //void QMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data)
00052 //{
00053 //    char data_write[2];
00054 //    data_write[0]=regAddress;           // I2C sends MSB first. Namely  >>|regAddress|>>|data|
00055 //    data_write[1]=data;
00056 //    i2c.write(address,data_write,2,0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
00057 //}
00058 
00059 //char QMC5883L::readByte(uint8_t address, uint8_t regAddress)
00060 //{
00061 //    char data_read[1];                   // will store the register data    
00062 //    char data_write[1];
00063 //    data_write[0]=regAddress;
00064 //    i2c.write(address,data_write,1,1);   // repeated = true
00065 //    i2c.read(address,data_read,1,0);     // read the data and stop
00066 //    return data_read[0];
00067 //} 
00068 
00069 //void QMC5883L::readBytes(uint8_t address, uint8_t regAddress, uint8_t byteNum, uint8_t* dest)
00070 //{
00071 //    char data[10],data_write[1];  
00072 //    data_write[0]=regAddress;      
00073 //    i2c.write(address,data_write,1,1);
00074 //    i2c.read(address,data,byteNum,0);
00075 //    for(int i=0;i<byteNum;i++)          // equate the addresses
00076 //        dest[i]=data[i];
00077 //}
00078 
00079 void QMC5883L_WriteByte(uint8_t QMC5883L_reg, uint8_t QMC5883L_data)
00080 {
00081     char data_out[2];
00082     data_out[0]=QMC5883L_reg;
00083     data_out[1]=QMC5883L_data;
00084     QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
00085 }
00086 
00087 uint8_t QMC5883L_ReadByte(uint8_t QMC5883L_reg)
00088 {
00089     char data_out[1], data_in[1];
00090     data_out[0] = QMC5883L_reg;
00091     QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 1, 1);
00092     QMC5883L_i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
00093     return (data_in[0]);
00094 }
00095 
00096 void QMC5883L::ChipID()
00097 {
00098     uint8_t ChipID = QMC5883L_ReadByte(CHIP_ID);   // Should return 0x68
00099     pc.printf("I AM QMC5883: 0x%x \r\n",ChipID);
00100     
00101 //    if(whoAmI==0x12)//0x68)
00102 //    {
00103 //        pc.printf("ICM20602 is online... \r\n");  
00104 ////        led2=1;
00105 ////        ledToggle(2);
00106 //    }
00107 //    else
00108 //    {
00109 //        pc.printf("Could not connect to ICM20602 \r\nCheck the connections... \r\n");  
00110 ////        toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
00111 //    } 
00112 //pc.printf("I AM 0x%x \r\n",QMC5883L_ADDRESS);    
00113 }
00114 
00115 void QMC5883L::init()
00116 {   
00117     setMagRange(MagScale_8G);
00118     QMC5883L_WriteByte(CONTROL_A, 0x0D | MagScale_8G);  // Range: 8G, ODR: 200 Hz, mode:Continuous-Measurement
00119     QMC5883L_WriteByte(SET_RESET, 0x01);
00120     //QMC5883L_WriteByte(STATUS, 0x01);
00121     //QMC5883L_WriteByte(0X20, 0x40);
00122 //    QMC5883L_WriteByte(0X21, 0x01);
00123     wait_ms(10);
00124 }
00125 
00126 int16_t QMC5883L::getMagXvalue()
00127 {
00128     uint8_t LoByte, HiByte;
00129     LoByte = QMC5883L_ReadByte(OUT_X_LSB); // read Accelerometer X_Low  value
00130     HiByte = QMC5883L_ReadByte(OUT_X_MSB); // read Accelerometer X_High value
00131     return((HiByte<<8) | LoByte);
00132 //    pc1.printf("accx:%d,%d\r\n",HiByte,LoByte);  // send data to matlab
00133 }
00134 
00135 int16_t QMC5883L::getMagYvalue()
00136 {
00137     uint8_t LoByte, HiByte;
00138     LoByte = QMC5883L_ReadByte(OUT_Y_LSB); // read Accelerometer X_Low  value
00139     HiByte = QMC5883L_ReadByte(OUT_Y_MSB); // read Accelerometer X_High value
00140     return ((HiByte<<8) | LoByte);
00141 }
00142 
00143 int16_t QMC5883L::getMagZvalue()
00144 {
00145     uint8_t LoByte, HiByte;
00146     LoByte = QMC5883L_ReadByte(OUT_Z_LSB); // read Accelerometer X_Low  value
00147     HiByte = QMC5883L_ReadByte(OUT_Z_MSB); // read Accelerometer X_High value
00148     return ((HiByte<<8) | LoByte);
00149 }
00150 
00151 int16_t QMC5883L::getMagTemp()
00152 {
00153     uint8_t LoByte, HiByte;
00154     LoByte = QMC5883L_ReadByte(TEMP_LSB); // read Accelerometer X_Low  value
00155     HiByte = QMC5883L_ReadByte(TEMP_MSB); // read Accelerometer X_High value
00156     return ((HiByte<<8) | LoByte);
00157 }
00158 
00159 //void QMC5883L::readMagData(float* dest)
00160 //{
00161 //    uint8_t rawData[6]; // x,y,z mag data
00162 //    
00163 //    /* Read six raw data registers sequentially and write them into data array */
00164 //    readBytes(QMC5883L_ADDRESS, OUT_X_MSB, 6, &rawData[0]); 
00165 //    
00166 //    /* Turn the MSB LSB into signed 16-bit value */
00167 //    dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // MAG_XOUT
00168 //    dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // MAG_ZOUT
00169 //    dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // MAG_YOUT 
00170 //    
00171 //    /* Convert raw data to magnetic field values in microtesla */
00172 //     dest[0] = dest[0] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
00173 //     dest[1] = dest[1] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
00174 //     dest[2] = dest[2] / Gauss_LSB_Z  * GAUSS_TO_MICROTESLA;
00175 //}
00176 
00177 //double QMC5883L::getHeading()
00178 //{
00179 //    float magData[3];
00180 //    readMagData(magData);
00181 //    
00182 //    /* Calculate the heading while Z axis of the module is pointing up */
00183 //    double heading = atan2(magData[1], magData[0]);
00184 //    
00185 //    // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
00186 //    // declinationAngle can be found here http://www.magnetic-declination.com/
00187 //    // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
00188 //    float declinationAngle = 0.096;
00189 //    heading += declinationAngle;
00190 //    
00191 //    // Correct for when signs are reversed.
00192 //    if(heading < 0)
00193 //        heading += 2*PI;
00194 //    
00195 //    // Check for wrap due to addition of declination.
00196 //    if(heading > 2*PI)
00197 //        heading -= 2*PI;
00198 //    
00199 //    /* Convert radian to degrees */
00200 //    heading = heading * 180 / PI;  
00201 //    
00202 //    return heading;    
00203 //}