Matthias Grob / Mbed 2 deprecated FlyBed1

NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.

Dependencies:   mbed MODI2C

Sensors/Comp/HMC5883.cpp@21:c2a2e7cbabdd, 2012-11-17 (annotated)

Committer:
maetugr
Date:
Sat Nov 17 11:49:21 2012 +0000
Revision:
21:c2a2e7cbabdd
Parent:
19:40c252b4a792 
Erster Flugtest, noch nicht stabil!

Who changed what in which revision?

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