QMC5883L(stm32f767zi)
Fork of HMC5883L by
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?
User | Revision | Line number | New 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 | //} |