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.
HMC5883.cpp
00001 #include "HMC5883.h" 00002 00003 HMC5883::HMC5883(PinName sda, PinName scl) : I2C_Sensor(sda, scl, HMC5883_I2C_ADDRESS) 00004 { 00005 // load calibration values 00006 //loadCalibrationValues(scale, 3, "COMPASS_SCALE.txt"); 00007 //loadCalibrationValues(offset, 3, "COMPASS_OFFSET.txt"); 00008 00009 // initialize HMC5883 00010 writeRegister(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode 00011 //writeRegister(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode! (should get constant values from measurement, see datasheet) 00012 writeRegister(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss) 00013 writeRegister(HMC5883_MODE_REG, 0x00); // continuous measurement-mode 00014 } 00015 00016 void HMC5883::read() 00017 { 00018 readraw(); 00019 for(int i = 0; i < 3; i++) 00020 data[i] = scale[i] * (float)(raw[i]) + offset[i]; 00021 } 00022 00023 void HMC5883::calibrate(int s) 00024 { 00025 int Min[3]; // values for achieved maximum and minimum amplitude in calibrating environment 00026 int Max[3]; 00027 00028 Timer calibrate_timer; // timer to know when calibration is finished 00029 calibrate_timer.start(); 00030 00031 while(calibrate_timer.read() < s) // take measurements for s seconds 00032 { 00033 readraw(); 00034 for(int i = 0; i < 3; i++) { 00035 Min[i] = Min[i] < raw[i] ? Min[i] : raw[i]; // after each measurement check if there's a new minimum or maximum 00036 Max[i] = Max[i] > raw[i] ? Max[i] : raw[i]; 00037 } 00038 } 00039 00040 for(int i = 0; i < 3; i++) { 00041 scale[i]= 2000 / (float)(Max[i]-Min[i]); // calculate scale and offset out of the measured maxima and minima 00042 offset[i]= 1000 - (float)(Max[i]) * scale[i]; // the lower bound is -1000, the higher one 1000 00043 } 00044 00045 saveCalibrationValues(scale, 3, "COMPASS_SCALE.txt"); // save new scale and offset values to flash 00046 saveCalibrationValues(offset, 3, "COMPASS_OFFSET.txt"); 00047 } 00048 00049 void HMC5883::readraw() 00050 { 00051 char buffer[6]; // 8-Bit pieces of axis data 00052 00053 readMultiRegister(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read axis registers using I2C 00054 00055 raw[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers 00056 raw[1] = (short) (buffer[4] << 8 | buffer[5]); // X, Z and Y (yes, order is stupid like this, see datasheet) 00057 raw[2] = (short) (buffer[2] << 8 | buffer[3]); 00058 } 00059 00060 float HMC5883::get_angle() 00061 { 00062 #define RAD2DEG 57.295779513082320876798154814105 00063 00064 float Heading; 00065 00066 Heading = RAD2DEG * atan2(data[0],data[1]); 00067 Heading += 1.367; // correction of the angle between geographical and magnetical north direction, called declination 00068 // if you need an east-declination += DecAngle, if you need west-declination -= DecAngle 00069 // for me in Switzerland, Bern it's ca. 1.367 degree east 00070 // see: http://magnetic-declination.com/ 00071 // for me: http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html 00072 if(Heading < 0) 00073 Heading += 360; // minimum 0 degree 00074 00075 if(Heading > 360) 00076 Heading -= 360; // maximum 360 degree 00077 00078 return Heading; 00079 } 00080 00081
Generated on Tue Jul 12 2022 20:54:01 by 1.7.2