z jx / QMC5883L

QMC5883L(stm32f767zi)

Dependencies:   HMC5883L

Dependents:   IMU_fusion_9DOF

Fork of HMC5883L by Baser Kandehir

QMC5883L.cpp@3:6aac221b613d, 2017-07-19 (annotated)

Committer:
sarahbest
Date:
Wed Jul 19 07:58:23 2017 +0000
Revision:
3:6aac221b613d
Parent:
HMC5883L.cpp@2:bbc9ad18fd3e 
qmc5883 output 4 variables: mag 3,temp.; the temperature is negative, which has something wrong

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sarahbest 3:6aac221b613d 1 /* QMC5883L Digital Compass Library
BaserK 0:e5f8da308b60 2 *
BaserK 0:e5f8da308b60 3 * @author: Baser Kandehir
BaserK 0:e5f8da308b60 4 * @date: August 5, 2015
BaserK 0:e5f8da308b60 5 * @license: MIT license
BaserK 0:e5f8da308b60 6 *
BaserK 0:e5f8da308b60 7 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
BaserK 0:e5f8da308b60 8 *
BaserK 0:e5f8da308b60 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
BaserK 0:e5f8da308b60 10 * of this software and associated documentation files (the "Software"), to deal
BaserK 0:e5f8da308b60 11 * in the Software without restriction, including without limitation the rights
BaserK 0:e5f8da308b60 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BaserK 0:e5f8da308b60 13 * copies of the Software, and to permit persons to whom the Software is
BaserK 0:e5f8da308b60 14 * furnished to do so, subject to the following conditions:
BaserK 0:e5f8da308b60 15 *
BaserK 0:e5f8da308b60 16 * The above copyright notice and this permission notice shall be included in
BaserK 0:e5f8da308b60 17 * all copies or substantial portions of the Software.
BaserK 0:e5f8da308b60 18 *
BaserK 0:e5f8da308b60 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BaserK 0:e5f8da308b60 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BaserK 0:e5f8da308b60 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BaserK 0:e5f8da308b60 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BaserK 0:e5f8da308b60 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BaserK 0:e5f8da308b60 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BaserK 0:e5f8da308b60 25 * THE SOFTWARE.
BaserK 0:e5f8da308b60 26 *
BaserK 0:e5f8da308b60 27 */
BaserK 0:e5f8da308b60 28
BaserK 0:e5f8da308b60 29 // Some part of the code is adapted from Adafruit HMC5883 library
BaserK 0:e5f8da308b60 30
sarahbest 3:6aac221b613d 31 #include "QMC5883L.h"
BaserK 0:e5f8da308b60 32
BaserK 0:e5f8da308b60 33 /* NUCLEO F411RE board */
sarahbest 3:6aac221b613d 34 static I2C QMC5883L_i2c(D2, D4); // setup i2c (SDA,SCL)
sarahbest 3:6aac221b613d 35 float mRes; // Varies with gain
BaserK 0:e5f8da308b60 36
sarahbest 3:6aac221b613d 37 float QMC5883L::setMagRange(MagScale Mscale)
sarahbest 3:6aac221b613d 38 {
sarahbest 3:6aac221b613d 39 switch(Mscale)
BaserK 0:e5f8da308b60 40 {
sarahbest 3:6aac221b613d 41 case MagScale_2G:
sarahbest 3:6aac221b613d 42 mRes = 1.0/12000; //LSB/G
BaserK 0:e5f8da308b60 43 break;
sarahbest 3:6aac221b613d 44 case MagScale_8G:
sarahbest 3:6aac221b613d 45 mRes = 1.0/3000;
BaserK 0:e5f8da308b60 46 break;
sarahbest 3:6aac221b613d 47 }
sarahbest 3:6aac221b613d 48 return mRes;
BaserK 0:e5f8da308b60 49 }
BaserK 0:e5f8da308b60 50
sarahbest 3:6aac221b613d 51 //void QMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data)
sarahbest 3:6aac221b613d 52 //{
sarahbest 3:6aac221b613d 53 // char data_write[2];
sarahbest 3:6aac221b613d 54 // data_write[0]=regAddress; // I2C sends MSB first. Namely >>|regAddress|>>|data|
sarahbest 3:6aac221b613d 55 // data_write[1]=data;
sarahbest 3:6aac221b613d 56 // i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false);
sarahbest 3:6aac221b613d 57 //}
sarahbest 3:6aac221b613d 58
sarahbest 3:6aac221b613d 59 //char QMC5883L::readByte(uint8_t address, uint8_t regAddress)
sarahbest 3:6aac221b613d 60 //{
sarahbest 3:6aac221b613d 61 // char data_read[1]; // will store the register data
sarahbest 3:6aac221b613d 62 // char data_write[1];
sarahbest 3:6aac221b613d 63 // data_write[0]=regAddress;
sarahbest 3:6aac221b613d 64 // i2c.write(address,data_write,1,1); // repeated = true
sarahbest 3:6aac221b613d 65 // i2c.read(address,data_read,1,0); // read the data and stop
sarahbest 3:6aac221b613d 66 // return data_read[0];
sarahbest 3:6aac221b613d 67 //}
sarahbest 3:6aac221b613d 68
sarahbest 3:6aac221b613d 69 //void QMC5883L::readBytes(uint8_t address, uint8_t regAddress, uint8_t byteNum, uint8_t* dest)
sarahbest 3:6aac221b613d 70 //{
sarahbest 3:6aac221b613d 71 // char data[10],data_write[1];
sarahbest 3:6aac221b613d 72 // data_write[0]=regAddress;
sarahbest 3:6aac221b613d 73 // i2c.write(address,data_write,1,1);
sarahbest 3:6aac221b613d 74 // i2c.read(address,data,byteNum,0);
sarahbest 3:6aac221b613d 75 // for(int i=0;i<byteNum;i++) // equate the addresses
sarahbest 3:6aac221b613d 76 // dest[i]=data[i];
sarahbest 3:6aac221b613d 77 //}
sarahbest 3:6aac221b613d 78
sarahbest 3:6aac221b613d 79 void QMC5883L_WriteByte(uint8_t QMC5883L_reg, uint8_t QMC5883L_data)
BaserK 0:e5f8da308b60 80 {
sarahbest 3:6aac221b613d 81 char data_out[2];
sarahbest 3:6aac221b613d 82 data_out[0]=QMC5883L_reg;
sarahbest 3:6aac221b613d 83 data_out[1]=QMC5883L_data;
sarahbest 3:6aac221b613d 84 QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
BaserK 0:e5f8da308b60 85 }
BaserK 0:e5f8da308b60 86
sarahbest 3:6aac221b613d 87 uint8_t QMC5883L_ReadByte(uint8_t QMC5883L_reg)
BaserK 0:e5f8da308b60 88 {
sarahbest 3:6aac221b613d 89 char data_out[1], data_in[1];
sarahbest 3:6aac221b613d 90 data_out[0] = QMC5883L_reg;
sarahbest 3:6aac221b613d 91 QMC5883L_i2c.write(QMC5883L_ADDRESS, data_out, 1, 1);
sarahbest 3:6aac221b613d 92 QMC5883L_i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
sarahbest 3:6aac221b613d 93 return (data_in[0]);
BaserK 0:e5f8da308b60 94 }
BaserK 0:e5f8da308b60 95
sarahbest 3:6aac221b613d 96 void QMC5883L::ChipID()
BaserK 0:e5f8da308b60 97 {
sarahbest 3:6aac221b613d 98 uint8_t ChipID = QMC5883L_ReadByte(CHIP_ID); // Should return 0x68
sarahbest 3:6aac221b613d 99 pc.printf("I AM QMC5883: 0x%x \r\n",ChipID);
sarahbest 3:6aac221b613d 100
sarahbest 3:6aac221b613d 101 // if(whoAmI==0x12)//0x68)
sarahbest 3:6aac221b613d 102 // {
sarahbest 3:6aac221b613d 103 // pc.printf("ICM20602 is online... \r\n");
sarahbest 3:6aac221b613d 104 //// led2=1;
sarahbest 3:6aac221b613d 105 //// ledToggle(2);
sarahbest 3:6aac221b613d 106 // }
sarahbest 3:6aac221b613d 107 // else
sarahbest 3:6aac221b613d 108 // {
sarahbest 3:6aac221b613d 109 // pc.printf("Could not connect to ICM20602 \r\nCheck the connections... \r\n");
sarahbest 3:6aac221b613d 110 //// toggler1.attach(&toggle_led1,0.1); // toggles led1 every 100 ms
sarahbest 3:6aac221b613d 111 // }
sarahbest 3:6aac221b613d 112 //pc.printf("I AM 0x%x \r\n",QMC5883L_ADDRESS);
sarahbest 3:6aac221b613d 113 }
sarahbest 3:6aac221b613d 114
sarahbest 3:6aac221b613d 115 void QMC5883L::init()
sarahbest 3:6aac221b613d 116 {
sarahbest 3:6aac221b613d 117 setMagRange(MagScale_8G);
sarahbest 3:6aac221b613d 118 QMC5883L_WriteByte(CONTROL_A, 0x0D | MagScale_8G); // Range: 8G, ODR: 200 Hz, mode:Continuous-Measurement
sarahbest 3:6aac221b613d 119 QMC5883L_WriteByte(SET_RESET, 0x01);
sarahbest 3:6aac221b613d 120 //QMC5883L_WriteByte(STATUS, 0x01);
sarahbest 3:6aac221b613d 121 //QMC5883L_WriteByte(0X20, 0x40);
sarahbest 3:6aac221b613d 122 // QMC5883L_WriteByte(0X21, 0x01);
BaserK 0:e5f8da308b60 123 wait_ms(10);
BaserK 0:e5f8da308b60 124 }
BaserK 0:e5f8da308b60 125
sarahbest 3:6aac221b613d 126 int16_t QMC5883L::getMagXvalue()
sarahbest 3:6aac221b613d 127 {
sarahbest 3:6aac221b613d 128 uint8_t LoByte, HiByte;
sarahbest 3:6aac221b613d 129 LoByte = QMC5883L_ReadByte(OUT_X_LSB); // read Accelerometer X_Low value
sarahbest 3:6aac221b613d 130 HiByte = QMC5883L_ReadByte(OUT_X_MSB); // read Accelerometer X_High value
sarahbest 3:6aac221b613d 131 return((HiByte<<8) | LoByte);
sarahbest 3:6aac221b613d 132 // pc1.printf("accx:%d,%d\r\n",HiByte,LoByte); // send data to matlab
sarahbest 3:6aac221b613d 133 }
sarahbest 3:6aac221b613d 134
sarahbest 3:6aac221b613d 135 int16_t QMC5883L::getMagYvalue()
BaserK 0:e5f8da308b60 136 {
sarahbest 3:6aac221b613d 137 uint8_t LoByte, HiByte;
sarahbest 3:6aac221b613d 138 LoByte = QMC5883L_ReadByte(OUT_Y_LSB); // read Accelerometer X_Low value
sarahbest 3:6aac221b613d 139 HiByte = QMC5883L_ReadByte(OUT_Y_MSB); // read Accelerometer X_High value
sarahbest 3:6aac221b613d 140 return ((HiByte<<8) | LoByte);
sarahbest 3:6aac221b613d 141 }
sarahbest 3:6aac221b613d 142
sarahbest 3:6aac221b613d 143 int16_t QMC5883L::getMagZvalue()
sarahbest 3:6aac221b613d 144 {
sarahbest 3:6aac221b613d 145 uint8_t LoByte, HiByte;
sarahbest 3:6aac221b613d 146 LoByte = QMC5883L_ReadByte(OUT_Z_LSB); // read Accelerometer X_Low value
sarahbest 3:6aac221b613d 147 HiByte = QMC5883L_ReadByte(OUT_Z_MSB); // read Accelerometer X_High value
sarahbest 3:6aac221b613d 148 return ((HiByte<<8) | LoByte);
sarahbest 3:6aac221b613d 149 }
sarahbest 3:6aac221b613d 150
sarahbest 3:6aac221b613d 151 int16_t QMC5883L::getMagTemp()
sarahbest 3:6aac221b613d 152 {
sarahbest 3:6aac221b613d 153 uint8_t LoByte, HiByte;
sarahbest 3:6aac221b613d 154 LoByte = QMC5883L_ReadByte(TEMP_LSB); // read Accelerometer X_Low value
sarahbest 3:6aac221b613d 155 HiByte = QMC5883L_ReadByte(TEMP_MSB); // read Accelerometer X_High value
sarahbest 3:6aac221b613d 156 return ((HiByte<<8) | LoByte);
BaserK 0:e5f8da308b60 157 }
BaserK 0:e5f8da308b60 158
sarahbest 3:6aac221b613d 159 //void QMC5883L::readMagData(float* dest)
sarahbest 3:6aac221b613d 160 //{
sarahbest 3:6aac221b613d 161 // uint8_t rawData[6]; // x,y,z mag data
sarahbest 3:6aac221b613d 162 //
sarahbest 3:6aac221b613d 163 // /* Read six raw data registers sequentially and write them into data array */
sarahbest 3:6aac221b613d 164 // readBytes(QMC5883L_ADDRESS, OUT_X_MSB, 6, &rawData[0]);
sarahbest 3:6aac221b613d 165 //
sarahbest 3:6aac221b613d 166 // /* Turn the MSB LSB into signed 16-bit value */
sarahbest 3:6aac221b613d 167 // dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // MAG_XOUT
sarahbest 3:6aac221b613d 168 // dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // MAG_ZOUT
sarahbest 3:6aac221b613d 169 // dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // MAG_YOUT
sarahbest 3:6aac221b613d 170 //
sarahbest 3:6aac221b613d 171 // /* Convert raw data to magnetic field values in microtesla */
sarahbest 3:6aac221b613d 172 // dest[0] = dest[0] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
sarahbest 3:6aac221b613d 173 // dest[1] = dest[1] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
sarahbest 3:6aac221b613d 174 // dest[2] = dest[2] / Gauss_LSB_Z * GAUSS_TO_MICROTESLA;
sarahbest 3:6aac221b613d 175 //}
sarahbest 3:6aac221b613d 176
sarahbest 3:6aac221b613d 177 //double QMC5883L::getHeading()
sarahbest 3:6aac221b613d 178 //{
sarahbest 3:6aac221b613d 179 // float magData[3];
sarahbest 3:6aac221b613d 180 // readMagData(magData);
sarahbest 3:6aac221b613d 181 //
sarahbest 3:6aac221b613d 182 // /* Calculate the heading while Z axis of the module is pointing up */
sarahbest 3:6aac221b613d 183 // double heading = atan2(magData[1], magData[0]);
sarahbest 3:6aac221b613d 184 //
sarahbest 3:6aac221b613d 185 // // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
sarahbest 3:6aac221b613d 186 // // declinationAngle can be found here http://www.magnetic-declination.com/
sarahbest 3:6aac221b613d 187 // // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
sarahbest 3:6aac221b613d 188 // float declinationAngle = 0.096;
sarahbest 3:6aac221b613d 189 // heading += declinationAngle;
sarahbest 3:6aac221b613d 190 //
sarahbest 3:6aac221b613d 191 // // Correct for when signs are reversed.
sarahbest 3:6aac221b613d 192 // if(heading < 0)
sarahbest 3:6aac221b613d 193 // heading += 2*PI;
sarahbest 3:6aac221b613d 194 //
sarahbest 3:6aac221b613d 195 // // Check for wrap due to addition of declination.
sarahbest 3:6aac221b613d 196 // if(heading > 2*PI)
sarahbest 3:6aac221b613d 197 // heading -= 2*PI;
sarahbest 3:6aac221b613d 198 //
sarahbest 3:6aac221b613d 199 // /* Convert radian to degrees */
sarahbest 3:6aac221b613d 200 // heading = heading * 180 / PI;
sarahbest 3:6aac221b613d 201 //
sarahbest 3:6aac221b613d 202 // return heading;
sarahbest 3:6aac221b613d 203 //}