Oleg Goncharovskiy / my_QMC5883L

for my students

QMC5883L.cpp@0:5c3a7cf4cb2a, 2020-07-10 (annotated)

Committer:
docent
Date:
Fri Jul 10 08:45:27 2020 +0000
Revision:
0:5c3a7cf4cb2a
for my students

Who changed what in which revision?

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