Matthias Grob / Mbed 2 deprecated IMU_10DOF

An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085

Dependencies:   mbed

IMU/Sensors/Comp/HMC5883.cpp@0:3e7450f1a938, 2013-08-27 (annotated)

Committer:
maetugr
Date:
Tue Aug 27 17:37:06 2013 +0000
Revision:
0:3e7450f1a938
Child:
4:f62337b907e5
before 10DOF class (only files already created)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:3e7450f1a938 1 #include "HMC5883.h"
maetugr 0:3e7450f1a938 2
maetugr 0:3e7450f1a938 3 HMC5883::HMC5883(PinName sda, PinName scl) : I2C_Sensor(sda, scl, HMC5883_I2C_ADDRESS)
maetugr 0:3e7450f1a938 4 {
maetugr 0:3e7450f1a938 5 offset[0] = -155; // offset calculated by hand... (min + ((max - min) / 2)
maetugr 0:3e7450f1a938 6 offset[1] = -142; // TODO: make this automatic with saving to filesystem
maetugr 0:3e7450f1a938 7 offset[2] = -33.5;
maetugr 0:3e7450f1a938 8
maetugr 0:3e7450f1a938 9 // load calibration values
maetugr 0:3e7450f1a938 10 //loadCalibrationValues(scale, 3, "COMPASS_SCALE.txt");
maetugr 0:3e7450f1a938 11 //loadCalibrationValues(offset, 3, "COMPASS_OFFSET.txt");
maetugr 0:3e7450f1a938 12
maetugr 0:3e7450f1a938 13 // initialize HMC5883
maetugr 0:3e7450f1a938 14 writeRegister(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode
maetugr 0:3e7450f1a938 15 //writeRegister(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode! (should get constant values from measurement, see datasheet)
maetugr 0:3e7450f1a938 16 writeRegister(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss)
maetugr 0:3e7450f1a938 17 writeRegister(HMC5883_MODE_REG, 0x00); // continuous measurement-mode
maetugr 0:3e7450f1a938 18 }
maetugr 0:3e7450f1a938 19
maetugr 0:3e7450f1a938 20 void HMC5883::read()
maetugr 0:3e7450f1a938 21 {
maetugr 0:3e7450f1a938 22 readraw();
maetugr 0:3e7450f1a938 23 for(int i = 0; i < 3; i++)
maetugr 0:3e7450f1a938 24 data[i] = (float)(raw[i]) - offset[i];
maetugr 0:3e7450f1a938 25 }
maetugr 0:3e7450f1a938 26
maetugr 0:3e7450f1a938 27 void HMC5883::calibrate(int s)
maetugr 0:3e7450f1a938 28 {
maetugr 0:3e7450f1a938 29 int Min[3]; // values for achieved maximum and minimum amplitude in calibrating environment
maetugr 0:3e7450f1a938 30 int Max[3];
maetugr 0:3e7450f1a938 31
maetugr 0:3e7450f1a938 32 Timer calibrate_timer; // timer to know when calibration is finished
maetugr 0:3e7450f1a938 33 calibrate_timer.start();
maetugr 0:3e7450f1a938 34
maetugr 0:3e7450f1a938 35 while(calibrate_timer.read() < s) // take measurements for s seconds
maetugr 0:3e7450f1a938 36 {
maetugr 0:3e7450f1a938 37 readraw();
maetugr 0:3e7450f1a938 38 for(int i = 0; i < 3; i++) {
maetugr 0:3e7450f1a938 39 Min[i] = Min[i] < raw[i] ? Min[i] : raw[i]; // after each measurement check if there's a new minimum or maximum
maetugr 0:3e7450f1a938 40 Max[i] = Max[i] > raw[i] ? Max[i] : raw[i];
maetugr 0:3e7450f1a938 41 }
maetugr 0:3e7450f1a938 42 }
maetugr 0:3e7450f1a938 43
maetugr 0:3e7450f1a938 44 for(int i = 0; i < 3; i++) {
maetugr 0:3e7450f1a938 45 //scale[i]= 2000 / (float)(Max[i]-Min[i]); // calculate scale and offset out of the measured maxima and minima
maetugr 0:3e7450f1a938 46 //offset[i]= 1000 - (float)(Max[i]) * scale[i]; // the lower bound is -1000, the higher one 1000
maetugr 0:3e7450f1a938 47 }
maetugr 0:3e7450f1a938 48
maetugr 0:3e7450f1a938 49 //saveCalibrationValues(scale, 3, "COM_SCALE"); // save new scale and offset values to flash
maetugr 0:3e7450f1a938 50 //saveCalibrationValues(offset, 3, "COM_OFFSE");
maetugr 0:3e7450f1a938 51 }
maetugr 0:3e7450f1a938 52
maetugr 0:3e7450f1a938 53 void HMC5883::readraw()
maetugr 0:3e7450f1a938 54 {
maetugr 0:3e7450f1a938 55 char buffer[6]; // 8-Bit pieces of axis data
maetugr 0:3e7450f1a938 56
maetugr 0:3e7450f1a938 57 readMultiRegister(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read axis registers using I2C
maetugr 0:3e7450f1a938 58
maetugr 0:3e7450f1a938 59 raw[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
maetugr 0:3e7450f1a938 60 raw[1] = (short) (buffer[4] << 8 | buffer[5]); // X, Z and Y (yes, order is stupid like this, see datasheet)
maetugr 0:3e7450f1a938 61 raw[2] = (short) (buffer[2] << 8 | buffer[3]);
maetugr 0:3e7450f1a938 62 }
maetugr 0:3e7450f1a938 63
maetugr 0:3e7450f1a938 64 float HMC5883::get_angle()
maetugr 0:3e7450f1a938 65 {
maetugr 0:3e7450f1a938 66 #define RAD2DEG 57.295779513082320876798154814105
maetugr 0:3e7450f1a938 67
maetugr 0:3e7450f1a938 68 float Heading;
maetugr 0:3e7450f1a938 69
maetugr 0:3e7450f1a938 70 Heading = RAD2DEG * atan2(data[0],data[1]);
maetugr 0:3e7450f1a938 71 Heading += 1.367; // correction of the angle between geographical and magnetical north direction, called declination
maetugr 0:3e7450f1a938 72 // if you need an east-declination += DecAngle, if you need west-declination -= DecAngle
maetugr 0:3e7450f1a938 73 // for me in Switzerland, Bern it's ca. 1.367 degree east
maetugr 0:3e7450f1a938 74 // see: http://magnetic-declination.com/
maetugr 0:3e7450f1a938 75 // for me: http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
maetugr 0:3e7450f1a938 76 if(Heading < 0)
maetugr 0:3e7450f1a938 77 Heading += 360; // minimum 0 degree
maetugr 0:3e7450f1a938 78
maetugr 0:3e7450f1a938 79 if(Heading > 360)
maetugr 0:3e7450f1a938 80 Heading -= 360; // maximum 360 degree
maetugr 0:3e7450f1a938 81
maetugr 0:3e7450f1a938 82 return Heading;
maetugr 0:3e7450f1a938 83 }
maetugr 0:3e7450f1a938 84
maetugr 0:3e7450f1a938 85