Matthias Grob / Mbed 2 deprecated FlyBed2

My fully self designed first stable working Quadrocopter Software.

Dependencies:   mbed

Dependents:   fluy343

/media/uploads/maetugr/dsc09031.jpg

IMU/Sensors/Comp/HMC5883.cpp@0:12950aa67f2a, 2013-09-02 (annotated)

Committer:
maetugr
Date:
Mon Sep 02 15:04:22 2013 +0000
Revision:
0:12950aa67f2a
Child:
2:03e5f7ab473f
first commit of new version (only one axis for test until now); note: no more Ticker! (made problems because of interrupts)

Who changed what in which revision?

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