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@34:3aa1cbcde59d, 2013-06-10 (annotated)

Committer:
maetugr
Date:
Mon Jun 10 13:22:46 2013 +0000
Revision:
34:3aa1cbcde59d
First version with new ESCs (they are working with the KK Board); some strange kicks in the behaviour of balancing, next step a logger

Who changed what in which revision?

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