MMC5883L.cpp

00001 #include "MMC5883L.h"
00002 #include <new>
00003 
00004 MMC5883L::MMC5883L(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
00005 {
00006     // Placement new to avoid additional heap memory allocation.
00007     new(i2cRaw) I2C(sda, scl);
00008 
00009     init();
00010 }
00011 
00012 MMC5883L::~MMC5883L()
00013 {
00014     // If the I2C object is initialized in the buffer in this object, call destructor of it.
00015     if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
00016         reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
00017 }
00018 
00019 void MMC5883L::init()
00020 {
00021     setConfiguration0(FLIP_SET);        // restore sensitivity by flip pulse
00022     setConfiguration1(DATARATE_100);    // smallest rate
00023     setConfiguration2(OUTPUT_RATE_OFF); // single mode
00024     wait(0.01);
00025 }
00026 
00027 void MMC5883L::setConfiguration0(char config)
00028 {
00029     char cmd[2];
00030     cmd[0] = INTERNAL_CONTROL_0; // register a address
00031     cmd[1] = config;             // register data
00032 
00033     i2c_.write(I2C_ADDRESS, cmd, 2);
00034 }
00035 
00036 void MMC5883L::startMeasurement_mag()
00037 {
00038     char cmd[2];
00039     cmd[0] = INTERNAL_CONTROL_0; // register a address
00040     cmd[1] = MAGNETIC_MEASUREMENT_START; // register data
00041 
00042     i2c_.write(I2C_ADDRESS, cmd, 2);
00043 }
00044 
00045 void MMC5883L::flipSet()
00046 {
00047     char cmd[2];
00048     cmd[0] = INTERNAL_CONTROL_0; // register a address
00049     cmd[1] = FLIP_SET;          // register data
00050 
00051     i2c_.write(I2C_ADDRESS, cmd, 2);
00052 }
00053 void MMC5883L::flipReset()
00054 {
00055     char cmd[2];
00056     cmd[0] = INTERNAL_CONTROL_0; // register a address
00057     cmd[1] = FLIP_RESET;         // register data
00058 
00059     i2c_.write(I2C_ADDRESS, cmd, 2);
00060 }
00061 
00062 void MMC5883L::startMeasurement_temp()
00063 {
00064     char cmd[2];
00065     cmd[0] = INTERNAL_CONTROL_0; // register a address
00066     cmd[1] = TEMP_MEASUREMENT_START; // register data
00067 
00068     i2c_.write(I2C_ADDRESS, cmd, 2);
00069 }
00070 
00071 void MMC5883L::setConfiguration1(char config)
00072 {
00073     char cmd[2];
00074     cmd[0] = INTERNAL_CONTROL_1; // register b address
00075     cmd[1] = config;            // register data
00076 
00077     i2c_.write(I2C_ADDRESS, cmd, 2);
00078 }
00079 
00080 void MMC5883L::setConfiguration2(char config)
00081 {
00082     char cmd[2];
00083     cmd[0] = INTERNAL_CONTROL_2; // register b address
00084     cmd[1] = config;            // register data
00085 
00086     i2c_.write(I2C_ADDRESS, cmd, 2);
00087 }
00088 
00089 char MMC5883L::getConfiguration0()
00090 {
00091     char cmd[2];
00092     cmd[0] = INTERNAL_CONTROL_0; // register address
00093     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00094     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00095     return cmd[1];
00096 }
00097 
00098 char MMC5883L::getConfiguration1()
00099 {
00100     char cmd[2];
00101     cmd[0] = INTERNAL_CONTROL_1; // register address
00102     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00103     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00104     return cmd[1];
00105 }
00106 
00107 
00108 char MMC5883L::getConfiguration2()
00109 {
00110     char cmd[2];
00111     cmd[0] = INTERNAL_CONTROL_2; // register address
00112     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00113     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00114     return cmd[1];
00115 }
00116 
00117 char MMC5883L::getStatus()
00118 {
00119     char cmd[2];
00120     cmd[0] = STATUS_REG; // status register
00121     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00122     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00123     return cmd[1];
00124 }
00125 
00126 void MMC5883L::getXYZ_RAW(int16_t output[3])
00127 {
00128     char cmd[2];
00129     char data[6];
00130 
00131     startMeasurement_mag(); // start measurement
00132     while((getStatus()&STATUS_M_DONE)==0); // wait until complete
00133 
00134     cmd[0] = OUTPUT_REG; // starting addr for reading
00135     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00136     i2c_.read(I2C_ADDRESS, data, 6, false);
00137     int32_t tmp[3];
00138     for(int i = 0; i < 3; i++) { // fill the output variables, X, Y, Z, LSB first
00139         tmp[i] = ((uint16_t)(data[i*2+1] << 8)) | (uint16_t)(data[i*2]);
00140         tmp[i] -= 65536/2;
00141         output[i] = tmp[i];
00142     }
00143 }
00144 
00145 void MMC5883L::getXYZ_nT(int32_t output[3])
00146 {
00147     int16_t data[3];
00148     getXYZ_RAW(data);
00149 
00150     for(int i = 0; i < 3; i++) {
00151         int64_t tmp =  data[i];
00152         tmp *= 800000;
00153         tmp /= (65536/2); // +- 8G = 16G FS
00154         output[i] = tmp;
00155     }
00156 }
00157 
00158 void MMC5883L::getXYZ_OffsetRemoved_RAW(int16_t output[3])
00159 {
00160     // TODO: offset reemove
00161     double sensitivity = 0.05;
00162     int16_t set_data[3];
00163     int16_t reset_data[3];
00164     flipSet();
00165     getXYZ_RAW(set_data);
00166     flipReset();
00167     getXYZ_RAW(reset_data);
00168     for(int i = 0; i < 3; i++) {
00169         output[i] = (reset_data[i] - set_data[i])/2*sensitivity;
00170     }
00171 }
00172 
00173 void MMC5883L::getXYZ_OffsetRemoved_nT(int32_t output[3])
00174 {
00175     int16_t data[3];
00176     getXYZ_OffsetRemoved_RAW(data);
00177 
00178     for(int i = 0; i < 3; i++) {
00179         int64_t tmp =  data[i];
00180         tmp *= 800000;
00181         tmp /= (65536/2); // +- 8G = 16G FS
00182         output[i] = tmp;
00183     }
00184 }
00185 
00186 
00187 double MMC5883L::getHeadingXY()
00188 {
00189     int16_t raw_data[3];
00190     getXYZ_OffsetRemoved_RAW(raw_data);
00191 
00192     double heading = atan2(static_cast<double>(raw_data[0]), static_cast<double>(raw_data[1])); // heading = arctan(X/Y)
00193 
00194     heading += DECLINATION_ANGLE;
00195 
00196     if(heading < 0.0) // fix sign
00197         heading += PI2;
00198 
00199     if(heading > PI2) // fix overflow
00200         heading -= PI2;
00201     return heading;
00202 }
00203 double MMC5883L::getHeadingXY(int16_t output[3])
00204 {
00205     getXYZ_OffsetRemoved_RAW(output);
00206 
00207     double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
00208 
00209     heading += DECLINATION_ANGLE;
00210 
00211     if(heading < 0.0) // fix sign
00212         heading += PI2;
00213 
00214     if(heading > PI2) // fix overflow
00215         heading -= PI2;
00216     return heading;
00217 }
00218 double MMC5883L::getHeadingXY(int32_t output[3])
00219 {
00220     getXYZ_OffsetRemoved_nT(output);
00221 
00222     double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
00223 
00224     heading += DECLINATION_ANGLE;
00225 
00226     if(heading < 0.0) // fix sign
00227         heading += PI2;
00228 
00229     if(heading > PI2) // fix overflow
00230         heading -= PI2;
00231     return heading;
00232 }
00233 
00234 double MMC5883L::getHeadingXYDeg()
00235 {
00236     return (getHeadingXY() * RAD_TO_DEG);
00237 }
00238 double MMC5883L::getHeadingXYDeg_RAW(int16_t output[3])
00239 {
00240     return (getHeadingXY(output) * RAD_TO_DEG);
00241 }
00242 double MMC5883L::getHeadingXYDeg_nT(int32_t output[3])
00243 {
00244     return (getHeadingXY(output) * RAD_TO_DEG);
00245 }
00246 
00247