Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2194 | - | 1 | /* |
| 2 | hmc5883l lib 0x01 |
||
| 3 | |||
| 4 | copyright (c) Davide Gironi, 2012 |
||
| 5 | |||
| 6 | Released under GPLv3. |
||
| 7 | Please refer to LICENSE file for licensing information. |
||
| 8 | */ |
||
| 2198 | - | 9 | //############################################################################ |
| 10 | //# HISTORY hmc5883l.c |
||
| 11 | //# |
||
| 12 | //# 19.09.2015 cebra |
||
| 13 | //# - add: Library für HMC5883L Compass, GY-87 Sensorboard |
||
| 14 | //# |
||
| 15 | //############################################################################ |
||
| 2194 | - | 16 | |
| 2198 | - | 17 | #ifdef USE_KOMPASS |
| 2194 | - | 18 | #include <stdlib.h> |
| 19 | #include <avr/io.h> |
||
| 20 | #include <util/delay.h> |
||
| 21 | |||
| 22 | #include "hmc5883l.h" |
||
| 23 | |||
| 24 | //path to i2c fleury lib |
||
| 25 | #include HMC5883L_I2CFLEURYPATH |
||
| 26 | |||
| 27 | static double hmc5883l_scale = 0; |
||
| 28 | |||
| 29 | /* |
||
| 30 | * init the hmc5883l |
||
| 31 | */ |
||
| 32 | void hmc5883l_init() { |
||
| 33 | #if HMC5883L_I2CINIT == 1 |
||
| 34 | //init i2c |
||
| 35 | i2c_init(); |
||
| 36 | _delay_us(10); |
||
| 37 | #endif |
||
| 38 | |||
| 39 | //set scale |
||
| 40 | hmc5883l_scale = 0; |
||
| 41 | uint8_t regValue = 0x00; |
||
| 42 | #if HMC5883L_SCALE == HMC5883L_SCALE088 |
||
| 43 | regValue = 0x00; |
||
| 44 | hmc5883l_scale = 0.73; |
||
| 45 | #elif HMC5883L_SCALE == HMC5883L_SCALE13 |
||
| 46 | regValue = 0x01; |
||
| 47 | hmc5883l_scale = 0.92; |
||
| 48 | #elif HMC5883L_SCALE == HMC5883L_SCALE19 |
||
| 49 | regValue = 0x02; |
||
| 50 | hmc5883l_scale = 1.22; |
||
| 51 | #elif HMC5883L_SCALE == HMC5883L_SCALE25 |
||
| 52 | regValue = 0x03; |
||
| 53 | hmc5883l_scale = 1.52; |
||
| 54 | #elif HMC5883L_SCALE == HMC5883L_SCALE40 |
||
| 55 | regValue = 0x04; |
||
| 56 | hmc5883l_scale = 2.27; |
||
| 57 | #elif HMC5883L_SCALE == HMC5883L_SCALE47 |
||
| 58 | regValue = 0x05; |
||
| 59 | hmc5883l_scale = 2.56; |
||
| 60 | #elif HMC5883L_SCALE == HMC5883L_SCALE56 |
||
| 61 | regValue = 0x06; |
||
| 62 | hmc5883l_scale = 3.03; |
||
| 63 | #elif HMC5883L_SCALE == HMC5883L_SCALE81 |
||
| 64 | regValue = 0x07; |
||
| 65 | hmc5883l_scale = 4.35; |
||
| 66 | #endif |
||
| 67 | |||
| 68 | //setting is in the top 3 bits of the register. |
||
| 69 | regValue = regValue << 5; |
||
| 70 | i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
||
| 71 | i2c_write(HMC5883L_CONFREGB); |
||
| 72 | i2c_write(regValue); |
||
| 73 | i2c_stop(); |
||
| 74 | |||
| 75 | //set measurement mode |
||
| 76 | i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
||
| 77 | i2c_write(HMC5883L_MODEREG); |
||
| 78 | i2c_write(HMC5883L_MEASUREMODE); |
||
| 79 | i2c_stop(); |
||
| 80 | } |
||
| 81 | |||
| 82 | /* |
||
| 83 | * get raw data |
||
| 84 | */ |
||
| 85 | void hmc5883l_getrawdata(int16_t *mxraw, int16_t *myraw, int16_t *mzraw) { |
||
| 86 | uint8_t i = 0; |
||
| 87 | uint8_t buff[6]; |
||
| 88 | |||
| 89 | i2c_start_wait(HMC5883L_ADDR | I2C_WRITE); |
||
| 90 | i2c_write(HMC5883L_DATAREGBEGIN); |
||
| 91 | i2c_stop(); |
||
| 92 | i2c_start_wait(HMC5883L_ADDR | I2C_READ); |
||
| 93 | for(i=0; i<6; i++) { |
||
| 94 | if(i==6-1) |
||
| 95 | buff[i] = i2c_readNak(); |
||
| 96 | else |
||
| 97 | buff[i] = i2c_readAck(); |
||
| 98 | } |
||
| 99 | i2c_stop(); |
||
| 100 | |||
| 101 | *mxraw = (int16_t)((buff[0] << 8) | buff[1]); |
||
| 102 | *mzraw = (int16_t)((buff[2] << 8) | buff[3]); |
||
| 103 | *myraw = (int16_t)((buff[4] << 8) | buff[5]); |
||
| 104 | } |
||
| 105 | |||
| 106 | /* |
||
| 107 | * get scaled data |
||
| 108 | */ |
||
| 109 | void hmc5883l_getdata(double *mx, double *my, double *mz) { |
||
| 110 | int16_t mxraw = 0; |
||
| 111 | int16_t myraw = 0; |
||
| 112 | int16_t mzraw = 0; |
||
| 113 | hmc5883l_getrawdata(&mxraw, &myraw, &mzraw); |
||
| 114 | |||
| 115 | #if HMC5883L_CALIBRATED == 1 |
||
| 116 | float mxt = mxraw - HMC5883L_OFFSETX; |
||
| 117 | float myt = myraw - HMC5883L_OFFSETY; |
||
| 118 | float mzt = mzraw - HMC5883L_OFFSETZ; |
||
| 119 | *mx = HMC5883L_GAINX1 * mxt + HMC5883L_GAINX2 * myt + HMC5883L_GAINX3 * mzt; |
||
| 120 | *my = HMC5883L_GAINY1 * mxt + HMC5883L_GAINY2 * myt + HMC5883L_GAINY3 * mzt; |
||
| 121 | *mz = HMC5883L_GAINZ1 * mxt + HMC5883L_GAINZ2 * myt + HMC5883L_GAINZ3 * mzt; |
||
| 122 | #else |
||
| 123 | *mx = mxraw * hmc5883l_scale; |
||
| 124 | *my = myraw * hmc5883l_scale; |
||
| 125 | *mz = mzraw * hmc5883l_scale; |
||
| 126 | #endif |
||
| 127 | |||
| 128 | |||
| 129 | |||
| 130 | } |
||
| 2198 | - | 131 | #endif |