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@2:03e5f7ab473f, 2013-09-09 (annotated)

Committer:
maetugr
Date:
Mon Sep 09 20:01:13 2013 +0000
Revision:
2:03e5f7ab473f
Parent:
0:12950aa67f2a 
SUCCESS! First stable flight! (A bit hard to controll, now working on controlling coefficients and yaw input)

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 2:03e5f7ab473f 6 offset[0] = 189.5; // offset calculated by hand... (min + ((max - min) / 2)
maetugr 2:03e5f7ab473f 7 offset[1] = -92.5; // TODO: make this automatic with saving to filesystem
maetugr 2:03e5f7ab473f 8 offset[2] = 29;
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 2:03e5f7ab473f 46 offset[i] = (float)Min[i] + ((Max[i] - Min[i]) / 2);
maetugr 0:12950aa67f2a 47 //scale[i]= 2000 / (float)(Max[i]-Min[i]); // calculate scale and offset out of the measured maxima and minima
maetugr 0:12950aa67f2a 48 //offset[i]= 1000 - (float)(Max[i]) * scale[i]; // the lower bound is -1000, the higher one 1000
maetugr 0:12950aa67f2a 49 }
maetugr 0:12950aa67f2a 50
maetugr 0:12950aa67f2a 51 //saveCalibrationValues(scale, 3, "COM_SCALE"); // save new scale and offset values to flash
maetugr 0:12950aa67f2a 52 //saveCalibrationValues(offset, 3, "COM_OFFSE");
maetugr 0:12950aa67f2a 53 }
maetugr 0:12950aa67f2a 54
maetugr 0:12950aa67f2a 55 void HMC5883::readraw()
maetugr 0:12950aa67f2a 56 {
maetugr 0:12950aa67f2a 57 char buffer[6]; // 8-Bit pieces of axis data
maetugr 0:12950aa67f2a 58
maetugr 0:12950aa67f2a 59 readMultiRegister(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read axis registers using I2C
maetugr 0:12950aa67f2a 60
maetugr 0:12950aa67f2a 61 raw[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
maetugr 0:12950aa67f2a 62 raw[1] = (short) (buffer[4] << 8 | buffer[5]); // X, Z and Y (yes, order is stupid like this, see datasheet)
maetugr 0:12950aa67f2a 63 raw[2] = (short) (buffer[2] << 8 | buffer[3]);
maetugr 0:12950aa67f2a 64 }
maetugr 0:12950aa67f2a 65
maetugr 0:12950aa67f2a 66 float HMC5883::get_angle()
maetugr 0:12950aa67f2a 67 {
maetugr 0:12950aa67f2a 68 #define RAD2DEG 57.295779513082320876798154814105
maetugr 0:12950aa67f2a 69
maetugr 0:12950aa67f2a 70 float Heading;
maetugr 0:12950aa67f2a 71
maetugr 0:12950aa67f2a 72 Heading = RAD2DEG * atan2(data[0],data[1]);
maetugr 0:12950aa67f2a 73 Heading += 1.367; // correction of the angle between geographical and magnetical north direction, called declination
maetugr 0:12950aa67f2a 74 // if you need an east-declination += DecAngle, if you need west-declination -= DecAngle
maetugr 0:12950aa67f2a 75 // for me in Switzerland, Bern it's ca. 1.367 degree east
maetugr 0:12950aa67f2a 76 // see: http://magnetic-declination.com/
maetugr 0:12950aa67f2a 77 // for me: http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
maetugr 0:12950aa67f2a 78 if(Heading < 0)
maetugr 0:12950aa67f2a 79 Heading += 360; // minimum 0 degree
maetugr 0:12950aa67f2a 80
maetugr 0:12950aa67f2a 81 if(Heading > 360)
maetugr 0:12950aa67f2a 82 Heading -= 360; // maximum 360 degree
maetugr 0:12950aa67f2a 83
maetugr 0:12950aa67f2a 84 return Heading;
maetugr 0:12950aa67f2a 85 }
maetugr 0:12950aa67f2a 86
maetugr 0:12950aa67f2a 87