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