Stepan Oslejsek / Mbed 2 deprecated OLEDMag

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

MMC5883L/MMC5883L.cpp@5:778bddacaa80, 2022-04-21 (annotated)

Committer:
oslejste
Date:
Thu Apr 21 19:07:15 2022 +0000
Revision:
5:778bddacaa80
Parent:
4:3618abce1646 
Oledmag;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vitpetrik 4:3618abce1646 1 #include "MMC5883L.h"
vitpetrik 4:3618abce1646 2 #include <new>
vitpetrik 4:3618abce1646 3
vitpetrik 4:3618abce1646 4 MMC5883L::MMC5883L(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
vitpetrik 4:3618abce1646 5 {
vitpetrik 4:3618abce1646 6 // Placement new to avoid additional heap memory allocation.
vitpetrik 4:3618abce1646 7 new(i2cRaw) I2C(sda, scl);
vitpetrik 4:3618abce1646 8
vitpetrik 4:3618abce1646 9 init();
vitpetrik 4:3618abce1646 10 }
vitpetrik 4:3618abce1646 11
vitpetrik 4:3618abce1646 12 MMC5883L::~MMC5883L()
vitpetrik 4:3618abce1646 13 {
vitpetrik 4:3618abce1646 14 // If the I2C object is initialized in the buffer in this object, call destructor of it.
vitpetrik 4:3618abce1646 15 if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
vitpetrik 4:3618abce1646 16 reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
vitpetrik 4:3618abce1646 17 }
vitpetrik 4:3618abce1646 18
vitpetrik 4:3618abce1646 19 void MMC5883L::init()
vitpetrik 4:3618abce1646 20 {
vitpetrik 4:3618abce1646 21 setConfiguration0(FLIP_SET); // restore sensitivity by flip pulse
vitpetrik 4:3618abce1646 22 setConfiguration1(DATARATE_100); // smallest rate
vitpetrik 4:3618abce1646 23 setConfiguration2(OUTPUT_RATE_OFF); // single mode
vitpetrik 4:3618abce1646 24 wait(0.01);
vitpetrik 4:3618abce1646 25 }
vitpetrik 4:3618abce1646 26
vitpetrik 4:3618abce1646 27 void MMC5883L::setConfiguration0(char config)
vitpetrik 4:3618abce1646 28 {
vitpetrik 4:3618abce1646 29 char cmd[2];
vitpetrik 4:3618abce1646 30 cmd[0] = INTERNAL_CONTROL_0; // register a address
vitpetrik 4:3618abce1646 31 cmd[1] = config; // register data
vitpetrik 4:3618abce1646 32
vitpetrik 4:3618abce1646 33 i2c_.write(I2C_ADDRESS, cmd, 2);
vitpetrik 4:3618abce1646 34 }
vitpetrik 4:3618abce1646 35
vitpetrik 4:3618abce1646 36 void MMC5883L::startMeasurement_mag()
vitpetrik 4:3618abce1646 37 {
vitpetrik 4:3618abce1646 38 char cmd[2];
vitpetrik 4:3618abce1646 39 cmd[0] = INTERNAL_CONTROL_0; // register a address
vitpetrik 4:3618abce1646 40 cmd[1] = MAGNETIC_MEASUREMENT_START; // register data
vitpetrik 4:3618abce1646 41
vitpetrik 4:3618abce1646 42 i2c_.write(I2C_ADDRESS, cmd, 2);
vitpetrik 4:3618abce1646 43 }
vitpetrik 4:3618abce1646 44
vitpetrik 4:3618abce1646 45 void MMC5883L::flipSet()
vitpetrik 4:3618abce1646 46 {
vitpetrik 4:3618abce1646 47 char cmd[2];
vitpetrik 4:3618abce1646 48 cmd[0] = INTERNAL_CONTROL_0; // register a address
vitpetrik 4:3618abce1646 49 cmd[1] = FLIP_SET; // register data
vitpetrik 4:3618abce1646 50
vitpetrik 4:3618abce1646 51 i2c_.write(I2C_ADDRESS, cmd, 2);
vitpetrik 4:3618abce1646 52 }
vitpetrik 4:3618abce1646 53 void MMC5883L::flipReset()
vitpetrik 4:3618abce1646 54 {
vitpetrik 4:3618abce1646 55 char cmd[2];
vitpetrik 4:3618abce1646 56 cmd[0] = INTERNAL_CONTROL_0; // register a address
vitpetrik 4:3618abce1646 57 cmd[1] = FLIP_RESET; // register data
vitpetrik 4:3618abce1646 58
vitpetrik 4:3618abce1646 59 i2c_.write(I2C_ADDRESS, cmd, 2);
vitpetrik 4:3618abce1646 60 }
vitpetrik 4:3618abce1646 61
vitpetrik 4:3618abce1646 62 void MMC5883L::startMeasurement_temp()
vitpetrik 4:3618abce1646 63 {
vitpetrik 4:3618abce1646 64 char cmd[2];
vitpetrik 4:3618abce1646 65 cmd[0] = INTERNAL_CONTROL_0; // register a address
vitpetrik 4:3618abce1646 66 cmd[1] = TEMP_MEASUREMENT_START; // register data
vitpetrik 4:3618abce1646 67
vitpetrik 4:3618abce1646 68 i2c_.write(I2C_ADDRESS, cmd, 2);
vitpetrik 4:3618abce1646 69 }
vitpetrik 4:3618abce1646 70
vitpetrik 4:3618abce1646 71 void MMC5883L::setConfiguration1(char config)
vitpetrik 4:3618abce1646 72 {
vitpetrik 4:3618abce1646 73 char cmd[2];
vitpetrik 4:3618abce1646 74 cmd[0] = INTERNAL_CONTROL_1; // register b address
vitpetrik 4:3618abce1646 75 cmd[1] = config; // register data
vitpetrik 4:3618abce1646 76
vitpetrik 4:3618abce1646 77 i2c_.write(I2C_ADDRESS, cmd, 2);
vitpetrik 4:3618abce1646 78 }
vitpetrik 4:3618abce1646 79
vitpetrik 4:3618abce1646 80 void MMC5883L::setConfiguration2(char config)
vitpetrik 4:3618abce1646 81 {
vitpetrik 4:3618abce1646 82 char cmd[2];
vitpetrik 4:3618abce1646 83 cmd[0] = INTERNAL_CONTROL_2; // register b address
vitpetrik 4:3618abce1646 84 cmd[1] = config; // register data
vitpetrik 4:3618abce1646 85
vitpetrik 4:3618abce1646 86 i2c_.write(I2C_ADDRESS, cmd, 2);
vitpetrik 4:3618abce1646 87 }
vitpetrik 4:3618abce1646 88
vitpetrik 4:3618abce1646 89 char MMC5883L::getConfiguration0()
vitpetrik 4:3618abce1646 90 {
vitpetrik 4:3618abce1646 91 char cmd[2];
vitpetrik 4:3618abce1646 92 cmd[0] = INTERNAL_CONTROL_0; // register address
vitpetrik 4:3618abce1646 93 i2c_.write(I2C_ADDRESS, cmd, 1, true);
vitpetrik 4:3618abce1646 94 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
vitpetrik 4:3618abce1646 95 return cmd[1];
vitpetrik 4:3618abce1646 96 }
vitpetrik 4:3618abce1646 97
vitpetrik 4:3618abce1646 98 char MMC5883L::getConfiguration1()
vitpetrik 4:3618abce1646 99 {
vitpetrik 4:3618abce1646 100 char cmd[2];
vitpetrik 4:3618abce1646 101 cmd[0] = INTERNAL_CONTROL_1; // register address
vitpetrik 4:3618abce1646 102 i2c_.write(I2C_ADDRESS, cmd, 1, true);
vitpetrik 4:3618abce1646 103 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
vitpetrik 4:3618abce1646 104 return cmd[1];
vitpetrik 4:3618abce1646 105 }
vitpetrik 4:3618abce1646 106
vitpetrik 4:3618abce1646 107
vitpetrik 4:3618abce1646 108 char MMC5883L::getConfiguration2()
vitpetrik 4:3618abce1646 109 {
vitpetrik 4:3618abce1646 110 char cmd[2];
vitpetrik 4:3618abce1646 111 cmd[0] = INTERNAL_CONTROL_2; // register address
vitpetrik 4:3618abce1646 112 i2c_.write(I2C_ADDRESS, cmd, 1, true);
vitpetrik 4:3618abce1646 113 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
vitpetrik 4:3618abce1646 114 return cmd[1];
vitpetrik 4:3618abce1646 115 }
vitpetrik 4:3618abce1646 116
vitpetrik 4:3618abce1646 117 char MMC5883L::getStatus()
vitpetrik 4:3618abce1646 118 {
vitpetrik 4:3618abce1646 119 char cmd[2];
vitpetrik 4:3618abce1646 120 cmd[0] = STATUS_REG; // status register
vitpetrik 4:3618abce1646 121 i2c_.write(I2C_ADDRESS, cmd, 1, true);
vitpetrik 4:3618abce1646 122 i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
vitpetrik 4:3618abce1646 123 return cmd[1];
vitpetrik 4:3618abce1646 124 }
vitpetrik 4:3618abce1646 125
vitpetrik 4:3618abce1646 126 void MMC5883L::getXYZ_RAW(int16_t output[3])
vitpetrik 4:3618abce1646 127 {
vitpetrik 4:3618abce1646 128 char cmd[2];
vitpetrik 4:3618abce1646 129 char data[6];
vitpetrik 4:3618abce1646 130
vitpetrik 4:3618abce1646 131 startMeasurement_mag(); // start measurement
vitpetrik 4:3618abce1646 132 while((getStatus()&STATUS_M_DONE)==0); // wait until complete
vitpetrik 4:3618abce1646 133
vitpetrik 4:3618abce1646 134 cmd[0] = OUTPUT_REG; // starting addr for reading
vitpetrik 4:3618abce1646 135 i2c_.write(I2C_ADDRESS, cmd, 1, true);
vitpetrik 4:3618abce1646 136 i2c_.read(I2C_ADDRESS, data, 6, false);
vitpetrik 4:3618abce1646 137 int32_t tmp[3];
vitpetrik 4:3618abce1646 138 for(int i = 0; i < 3; i++) { // fill the output variables, X, Y, Z, LSB first
vitpetrik 4:3618abce1646 139 tmp[i] = ((uint16_t)(data[i*2+1] << 8)) | (uint16_t)(data[i*2]);
vitpetrik 4:3618abce1646 140 tmp[i] -= 65536/2;
vitpetrik 4:3618abce1646 141 output[i] = tmp[i];
vitpetrik 4:3618abce1646 142 }
vitpetrik 4:3618abce1646 143 }
vitpetrik 4:3618abce1646 144
vitpetrik 4:3618abce1646 145 void MMC5883L::getXYZ_nT(int32_t output[3])
vitpetrik 4:3618abce1646 146 {
vitpetrik 4:3618abce1646 147 int16_t data[3];
vitpetrik 4:3618abce1646 148 getXYZ_RAW(data);
vitpetrik 4:3618abce1646 149
vitpetrik 4:3618abce1646 150 for(int i = 0; i < 3; i++) {
vitpetrik 4:3618abce1646 151 int64_t tmp = data[i];
vitpetrik 4:3618abce1646 152 tmp *= 800000;
vitpetrik 4:3618abce1646 153 tmp /= (65536/2); // +- 8G = 16G FS
vitpetrik 4:3618abce1646 154 output[i] = tmp;
vitpetrik 4:3618abce1646 155 }
vitpetrik 4:3618abce1646 156 }
vitpetrik 4:3618abce1646 157
vitpetrik 4:3618abce1646 158 void MMC5883L::getXYZ_OffsetRemoved_RAW(int16_t output[3])
vitpetrik 4:3618abce1646 159 {
vitpetrik 4:3618abce1646 160 // TODO: offset reemove
oslejste 5:778bddacaa80 161 double sensitivity = 0.05;
oslejste 5:778bddacaa80 162 int16_t set_data[3];
oslejste 5:778bddacaa80 163 int16_t reset_data[3];
vitpetrik 4:3618abce1646 164 flipSet();
oslejste 5:778bddacaa80 165 getXYZ_RAW(set_data);
vitpetrik 4:3618abce1646 166 flipReset();
oslejste 5:778bddacaa80 167 getXYZ_RAW(reset_data);
vitpetrik 4:3618abce1646 168 for(int i = 0; i < 3; i++) {
oslejste 5:778bddacaa80 169 output[i] = (reset_data[i] - set_data[i])/2*sensitivity;
vitpetrik 4:3618abce1646 170 }
vitpetrik 4:3618abce1646 171 }
vitpetrik 4:3618abce1646 172
vitpetrik 4:3618abce1646 173 void MMC5883L::getXYZ_OffsetRemoved_nT(int32_t output[3])
vitpetrik 4:3618abce1646 174 {
vitpetrik 4:3618abce1646 175 int16_t data[3];
vitpetrik 4:3618abce1646 176 getXYZ_OffsetRemoved_RAW(data);
vitpetrik 4:3618abce1646 177
vitpetrik 4:3618abce1646 178 for(int i = 0; i < 3; i++) {
vitpetrik 4:3618abce1646 179 int64_t tmp = data[i];
vitpetrik 4:3618abce1646 180 tmp *= 800000;
vitpetrik 4:3618abce1646 181 tmp /= (65536/2); // +- 8G = 16G FS
vitpetrik 4:3618abce1646 182 output[i] = tmp;
vitpetrik 4:3618abce1646 183 }
vitpetrik 4:3618abce1646 184 }
vitpetrik 4:3618abce1646 185
vitpetrik 4:3618abce1646 186
vitpetrik 4:3618abce1646 187 double MMC5883L::getHeadingXY()
vitpetrik 4:3618abce1646 188 {
vitpetrik 4:3618abce1646 189 int16_t raw_data[3];
vitpetrik 4:3618abce1646 190 getXYZ_OffsetRemoved_RAW(raw_data);
vitpetrik 4:3618abce1646 191
vitpetrik 4:3618abce1646 192 double heading = atan2(static_cast<double>(raw_data[0]), static_cast<double>(raw_data[1])); // heading = arctan(X/Y)
vitpetrik 4:3618abce1646 193
vitpetrik 4:3618abce1646 194 heading += DECLINATION_ANGLE;
vitpetrik 4:3618abce1646 195
vitpetrik 4:3618abce1646 196 if(heading < 0.0) // fix sign
vitpetrik 4:3618abce1646 197 heading += PI2;
vitpetrik 4:3618abce1646 198
vitpetrik 4:3618abce1646 199 if(heading > PI2) // fix overflow
vitpetrik 4:3618abce1646 200 heading -= PI2;
vitpetrik 4:3618abce1646 201 return heading;
vitpetrik 4:3618abce1646 202 }
vitpetrik 4:3618abce1646 203 double MMC5883L::getHeadingXY(int16_t output[3])
vitpetrik 4:3618abce1646 204 {
vitpetrik 4:3618abce1646 205 getXYZ_OffsetRemoved_RAW(output);
vitpetrik 4:3618abce1646 206
vitpetrik 4:3618abce1646 207 double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
vitpetrik 4:3618abce1646 208
vitpetrik 4:3618abce1646 209 heading += DECLINATION_ANGLE;
vitpetrik 4:3618abce1646 210
vitpetrik 4:3618abce1646 211 if(heading < 0.0) // fix sign
vitpetrik 4:3618abce1646 212 heading += PI2;
vitpetrik 4:3618abce1646 213
vitpetrik 4:3618abce1646 214 if(heading > PI2) // fix overflow
vitpetrik 4:3618abce1646 215 heading -= PI2;
vitpetrik 4:3618abce1646 216 return heading;
vitpetrik 4:3618abce1646 217 }
vitpetrik 4:3618abce1646 218 double MMC5883L::getHeadingXY(int32_t output[3])
vitpetrik 4:3618abce1646 219 {
vitpetrik 4:3618abce1646 220 getXYZ_OffsetRemoved_nT(output);
vitpetrik 4:3618abce1646 221
vitpetrik 4:3618abce1646 222 double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
vitpetrik 4:3618abce1646 223
vitpetrik 4:3618abce1646 224 heading += DECLINATION_ANGLE;
vitpetrik 4:3618abce1646 225
vitpetrik 4:3618abce1646 226 if(heading < 0.0) // fix sign
vitpetrik 4:3618abce1646 227 heading += PI2;
vitpetrik 4:3618abce1646 228
vitpetrik 4:3618abce1646 229 if(heading > PI2) // fix overflow
vitpetrik 4:3618abce1646 230 heading -= PI2;
vitpetrik 4:3618abce1646 231 return heading;
vitpetrik 4:3618abce1646 232 }
vitpetrik 4:3618abce1646 233
vitpetrik 4:3618abce1646 234 double MMC5883L::getHeadingXYDeg()
vitpetrik 4:3618abce1646 235 {
vitpetrik 4:3618abce1646 236 return (getHeadingXY() * RAD_TO_DEG);
vitpetrik 4:3618abce1646 237 }
vitpetrik 4:3618abce1646 238 double MMC5883L::getHeadingXYDeg_RAW(int16_t output[3])
vitpetrik 4:3618abce1646 239 {
vitpetrik 4:3618abce1646 240 return (getHeadingXY(output) * RAD_TO_DEG);
vitpetrik 4:3618abce1646 241 }
vitpetrik 4:3618abce1646 242 double MMC5883L::getHeadingXYDeg_nT(int32_t output[3])
vitpetrik 4:3618abce1646 243 {
vitpetrik 4:3618abce1646 244 return (getHeadingXY(output) * RAD_TO_DEG);
vitpetrik 4:3618abce1646 245 }
vitpetrik 4:3618abce1646 246
vitpetrik 4:3618abce1646 247