vit petrik / 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@4:3618abce1646, 2022-04-21 (annotated)

Committer:
vitpetrik
Date:
Thu Apr 21 13:19:41 2022 +0000
Revision:
4:3618abce1646
Share for Stepan Oslejsek

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
vitpetrik 4:3618abce1646 161 int16_t data_set[3];
vitpetrik 4:3618abce1646 162 flipSet();
vitpetrik 4:3618abce1646 163 getXYZ_RAW(data_set);
vitpetrik 4:3618abce1646 164 int16_t data_reset[3];
vitpetrik 4:3618abce1646 165 flipReset();
vitpetrik 4:3618abce1646 166 getXYZ_RAW(data_reset);
vitpetrik 4:3618abce1646 167
vitpetrik 4:3618abce1646 168 for(int i = 0; i < 3; i++) {
vitpetrik 4:3618abce1646 169 int16_t diff = data_reset[i] - data_set[i];
vitpetrik 4:3618abce1646 170 output[i] = data_reset[i] - diff/2;
vitpetrik 4:3618abce1646 171 }
vitpetrik 4:3618abce1646 172 }
vitpetrik 4:3618abce1646 173
vitpetrik 4:3618abce1646 174 void MMC5883L::getXYZ_OffsetRemoved_nT(int32_t output[3])
vitpetrik 4:3618abce1646 175 {
vitpetrik 4:3618abce1646 176 int16_t data[3];
vitpetrik 4:3618abce1646 177 getXYZ_OffsetRemoved_RAW(data);
vitpetrik 4:3618abce1646 178
vitpetrik 4:3618abce1646 179 for(int i = 0; i < 3; i++) {
vitpetrik 4:3618abce1646 180 int64_t tmp = data[i];
vitpetrik 4:3618abce1646 181 tmp *= 800000;
vitpetrik 4:3618abce1646 182 tmp /= (65536/2); // +- 8G = 16G FS
vitpetrik 4:3618abce1646 183 output[i] = tmp;
vitpetrik 4:3618abce1646 184 }
vitpetrik 4:3618abce1646 185 }
vitpetrik 4:3618abce1646 186
vitpetrik 4:3618abce1646 187
vitpetrik 4:3618abce1646 188 double MMC5883L::getHeadingXY()
vitpetrik 4:3618abce1646 189 {
vitpetrik 4:3618abce1646 190 int16_t raw_data[3];
vitpetrik 4:3618abce1646 191 getXYZ_OffsetRemoved_RAW(raw_data);
vitpetrik 4:3618abce1646 192
vitpetrik 4:3618abce1646 193 double heading = atan2(static_cast<double>(raw_data[0]), static_cast<double>(raw_data[1])); // heading = arctan(X/Y)
vitpetrik 4:3618abce1646 194
vitpetrik 4:3618abce1646 195 heading += DECLINATION_ANGLE;
vitpetrik 4:3618abce1646 196
vitpetrik 4:3618abce1646 197 if(heading < 0.0) // fix sign
vitpetrik 4:3618abce1646 198 heading += PI2;
vitpetrik 4:3618abce1646 199
vitpetrik 4:3618abce1646 200 if(heading > PI2) // fix overflow
vitpetrik 4:3618abce1646 201 heading -= PI2;
vitpetrik 4:3618abce1646 202 return heading;
vitpetrik 4:3618abce1646 203 }
vitpetrik 4:3618abce1646 204 double MMC5883L::getHeadingXY(int16_t output[3])
vitpetrik 4:3618abce1646 205 {
vitpetrik 4:3618abce1646 206 getXYZ_OffsetRemoved_RAW(output);
vitpetrik 4:3618abce1646 207
vitpetrik 4:3618abce1646 208 double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
vitpetrik 4:3618abce1646 209
vitpetrik 4:3618abce1646 210 heading += DECLINATION_ANGLE;
vitpetrik 4:3618abce1646 211
vitpetrik 4:3618abce1646 212 if(heading < 0.0) // fix sign
vitpetrik 4:3618abce1646 213 heading += PI2;
vitpetrik 4:3618abce1646 214
vitpetrik 4:3618abce1646 215 if(heading > PI2) // fix overflow
vitpetrik 4:3618abce1646 216 heading -= PI2;
vitpetrik 4:3618abce1646 217 return heading;
vitpetrik 4:3618abce1646 218 }
vitpetrik 4:3618abce1646 219 double MMC5883L::getHeadingXY(int32_t output[3])
vitpetrik 4:3618abce1646 220 {
vitpetrik 4:3618abce1646 221 getXYZ_OffsetRemoved_nT(output);
vitpetrik 4:3618abce1646 222
vitpetrik 4:3618abce1646 223 double heading = atan2(static_cast<double>(output[0]), static_cast<double>(output[1])); // heading = arctan(X/Y)
vitpetrik 4:3618abce1646 224
vitpetrik 4:3618abce1646 225 heading += DECLINATION_ANGLE;
vitpetrik 4:3618abce1646 226
vitpetrik 4:3618abce1646 227 if(heading < 0.0) // fix sign
vitpetrik 4:3618abce1646 228 heading += PI2;
vitpetrik 4:3618abce1646 229
vitpetrik 4:3618abce1646 230 if(heading > PI2) // fix overflow
vitpetrik 4:3618abce1646 231 heading -= PI2;
vitpetrik 4:3618abce1646 232 return heading;
vitpetrik 4:3618abce1646 233 }
vitpetrik 4:3618abce1646 234
vitpetrik 4:3618abce1646 235 double MMC5883L::getHeadingXYDeg()
vitpetrik 4:3618abce1646 236 {
vitpetrik 4:3618abce1646 237 return (getHeadingXY() * RAD_TO_DEG);
vitpetrik 4:3618abce1646 238 }
vitpetrik 4:3618abce1646 239 double MMC5883L::getHeadingXYDeg_RAW(int16_t output[3])
vitpetrik 4:3618abce1646 240 {
vitpetrik 4:3618abce1646 241 return (getHeadingXY(output) * RAD_TO_DEG);
vitpetrik 4:3618abce1646 242 }
vitpetrik 4:3618abce1646 243 double MMC5883L::getHeadingXYDeg_nT(int32_t output[3])
vitpetrik 4:3618abce1646 244 {
vitpetrik 4:3618abce1646 245 return (getHeadingXY(output) * RAD_TO_DEG);
vitpetrik 4:3618abce1646 246 }
vitpetrik 4:3618abce1646 247
vitpetrik 4:3618abce1646 248