File content as of revision 3:6aac221b613d:

/*   QMC5883L Digital Compass Library
*
*    @author: Baser Kandehir 
*    @date: August 5, 2015
*    @license: MIT license
*     
*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
*
*   Permission is hereby granted, free of charge, to any person obtaining a copy
*   of this software and associated documentation files (the "Software"), to deal
*   in the Software without restriction, including without limitation the rights
*   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
*   copies of the Software, and to permit persons to whom the Software is
*   furnished to do so, subject to the following conditions:
*
*   The above copyright notice and this permission notice shall be included in
*   all copies or substantial portions of the Software.
*
*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
*   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
*   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
*   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
*   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
*   THE SOFTWARE.
*
*/

// Some part of the code is adapted from Adafruit HMC5883 library

#include "QMC5883L.h"

/* NUCLEO F411RE board */
static I2C QMC5883L_i2c(D2, D4);         // setup i2c (SDA,SCL)  
float mRes; // Varies with gain
 
float QMC5883L::setMagRange(MagScale Mscale)
{   
    switch(Mscale)
    {
        case MagScale_2G:
            mRes = 1.0/12000;  //LSB/G
            break;
        case MagScale_8G:
            mRes = 1.0/3000;
            break;
    } 
    return mRes;
}

//void QMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data)
//{
//    char data_write[2];
//    data_write[0]=regAddress;           // I2C sends MSB first. Namely  >>|regAddress|>>|data|
//    data_write[1]=data;
//    i2c.write(address,data_write,2,0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
//}

//char QMC5883L::readByte(uint8_t address, uint8_t regAddress)
//{
//    char data_read[1];                   // will store the register data    
//    char data_write[1];
//    data_write[0]=regAddress;
//    i2c.write(address,data_write,1,1);   // repeated = true
//    i2c.read(address,data_read,1,0);     // read the data and stop
//    return data_read[0];
//} 

//void QMC5883L::readBytes(uint8_t address, uint8_t regAddress, uint8_t byteNum, uint8_t* dest)
//{
//    char data[10],data_write[1];  
//    data_write[0]=regAddress;      
//    i2c.write(address,data_write,1,1);
//    i2c.read(address,data,byteNum,0);
//    for(int i=0;i<byteNum;i++)          // equate the addresses
//        dest[i]=data[i];
//}

void QMC5883L_WriteByte(uint8_t QMC5883L_reg, uint8_t QMC5883L_data)
{
    char data_out[2];
    data_out[0]=QMC5883L_reg;
    data_out[1]=QMC5883L_data;
    QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
}

uint8_t QMC5883L_ReadByte(uint8_t QMC5883L_reg)
{
    char data_out[1], data_in[1];
    data_out[0] = QMC5883L_reg;
    QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 1, 1);
    QMC5883L_i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
    return (data_in[0]);
}

void QMC5883L::ChipID()
{
    uint8_t ChipID = QMC5883L_ReadByte(CHIP_ID);   // Should return 0x68
    pc.printf("I AM QMC5883: 0x%x \r\n",ChipID);
    
//    if(whoAmI==0x12)//0x68)
//    {
//        pc.printf("ICM20602 is online... \r\n");  
////        led2=1;
////        ledToggle(2);
//    }
//    else
//    {
//        pc.printf("Could not connect to ICM20602 \r\nCheck the connections... \r\n");  
////        toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
//    } 
//pc.printf("I AM 0x%x \r\n",QMC5883L_ADDRESS);    
}

void QMC5883L::init()
{   
    setMagRange(MagScale_8G);
    QMC5883L_WriteByte(CONTROL_A, 0x0D | MagScale_8G);  // Range: 8G, ODR: 200 Hz, mode:Continuous-Measurement
    QMC5883L_WriteByte(SET_RESET, 0x01);
    //QMC5883L_WriteByte(STATUS, 0x01);
    //QMC5883L_WriteByte(0X20, 0x40);
//    QMC5883L_WriteByte(0X21, 0x01);
    wait_ms(10);
}

int16_t QMC5883L::getMagXvalue()
{
    uint8_t LoByte, HiByte;
    LoByte = QMC5883L_ReadByte(OUT_X_LSB); // read Accelerometer X_Low  value
    HiByte = QMC5883L_ReadByte(OUT_X_MSB); // read Accelerometer X_High value
    return((HiByte<<8) | LoByte);
//    pc1.printf("accx:%d,%d\r\n",HiByte,LoByte);  // send data to matlab
}

int16_t QMC5883L::getMagYvalue()
{
    uint8_t LoByte, HiByte;
    LoByte = QMC5883L_ReadByte(OUT_Y_LSB); // read Accelerometer X_Low  value
    HiByte = QMC5883L_ReadByte(OUT_Y_MSB); // read Accelerometer X_High value
    return ((HiByte<<8) | LoByte);
}

int16_t QMC5883L::getMagZvalue()
{
    uint8_t LoByte, HiByte;
    LoByte = QMC5883L_ReadByte(OUT_Z_LSB); // read Accelerometer X_Low  value
    HiByte = QMC5883L_ReadByte(OUT_Z_MSB); // read Accelerometer X_High value
    return ((HiByte<<8) | LoByte);
}

int16_t QMC5883L::getMagTemp()
{
    uint8_t LoByte, HiByte;
    LoByte = QMC5883L_ReadByte(TEMP_LSB); // read Accelerometer X_Low  value
    HiByte = QMC5883L_ReadByte(TEMP_MSB); // read Accelerometer X_High value
    return ((HiByte<<8) | LoByte);
}

//void QMC5883L::readMagData(float* dest)
//{
//    uint8_t rawData[6]; // x,y,z mag data
//    
//    /* Read six raw data registers sequentially and write them into data array */
//    readBytes(QMC5883L_ADDRESS, OUT_X_MSB, 6, &rawData[0]); 
//    
//    /* Turn the MSB LSB into signed 16-bit value */
//    dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // MAG_XOUT
//    dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // MAG_ZOUT
//    dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // MAG_YOUT 
//    
//    /* Convert raw data to magnetic field values in microtesla */
//     dest[0] = dest[0] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
//     dest[1] = dest[1] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
//     dest[2] = dest[2] / Gauss_LSB_Z  * GAUSS_TO_MICROTESLA;
//}

//double QMC5883L::getHeading()
//{
//    float magData[3];
//    readMagData(magData);
//    
//    /* Calculate the heading while Z axis of the module is pointing up */
//    double heading = atan2(magData[1], magData[0]);
//    
//    // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
//    // declinationAngle can be found here http://www.magnetic-declination.com/
//    // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
//    float declinationAngle = 0.096;
//    heading += declinationAngle;
//    
//    // Correct for when signs are reversed.
//    if(heading < 0)
//        heading += 2*PI;
//    
//    // Check for wrap due to addition of declination.
//    if(heading > 2*PI)
//        heading -= 2*PI;
//    
//    /* Convert radian to degrees */
//    heading = heading * 180 / PI;  
//    
//    return heading;    
//}