Michael Ernst Peter / Mbed OS Test_GPS

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:   Eigen

QMC5883L.cpp@47:c74d09a252d4, 2022-06-02 (annotated)

Committer:
pmic
Date:
Thu Jun 02 07:43:14 2022 +0000
Revision:
47:c74d09a252d4
Child:
49:76fcaffb92ef
Removed M9N Libary and ported magnetometer driver to main project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 47:c74d09a252d4 1 #include "QMC5883L.h"
pmic 47:c74d09a252d4 2
pmic 47:c74d09a252d4 3 QMC5883L::QMC5883L(I2C& i2c): i2c(i2c)
pmic 47:c74d09a252d4 4 {
pmic 47:c74d09a252d4 5 init();
pmic 47:c74d09a252d4 6 }
pmic 47:c74d09a252d4 7
pmic 47:c74d09a252d4 8 void QMC5883L::readMag()
pmic 47:c74d09a252d4 9 {
pmic 47:c74d09a252d4 10 getMagValue(OUT_Y_LSB, OUT_Y_MSB, m_mag_val[0]); // x and y are swapped
pmic 47:c74d09a252d4 11 getMagValue(OUT_X_LSB, OUT_X_MSB, m_mag_val[1]);
pmic 47:c74d09a252d4 12 getMagValue(OUT_Z_LSB, OUT_Z_MSB, m_mag_val[2]);
pmic 47:c74d09a252d4 13 }
pmic 47:c74d09a252d4 14
pmic 47:c74d09a252d4 15 float QMC5883L::magX()
pmic 47:c74d09a252d4 16 {
pmic 47:c74d09a252d4 17 return m_scale * static_cast<float>(m_mag_val[0]);
pmic 47:c74d09a252d4 18 }
pmic 47:c74d09a252d4 19
pmic 47:c74d09a252d4 20 float QMC5883L::magY()
pmic 47:c74d09a252d4 21 {
pmic 47:c74d09a252d4 22 return m_scale * static_cast<float>(m_mag_val[1]);
pmic 47:c74d09a252d4 23 }
pmic 47:c74d09a252d4 24
pmic 47:c74d09a252d4 25 float QMC5883L::magZ()
pmic 47:c74d09a252d4 26 {
pmic 47:c74d09a252d4 27 return m_scale * static_cast<float>(m_mag_val[2]);
pmic 47:c74d09a252d4 28 }
pmic 47:c74d09a252d4 29
pmic 47:c74d09a252d4 30 void QMC5883L::init()
pmic 47:c74d09a252d4 31 {
pmic 47:c74d09a252d4 32 // 31.05.2022, based on experiment so it fits the former mag scaling (norm approx. 0.31)
pmic 47:c74d09a252d4 33 m_scale = 1.97e-4f;
pmic 47:c74d09a252d4 34
pmic 47:c74d09a252d4 35 static bool ok;
pmic 47:c74d09a252d4 36 static uint8_t cntr;
pmic 47:c74d09a252d4 37
pmic 47:c74d09a252d4 38 cntr = 0;
pmic 47:c74d09a252d4 39 while (true) {
pmic 47:c74d09a252d4 40 //ok = WriteByte(CONTROL_A, 0x59); // OSR: 256, RNG: 8G, ODR: 100 Hz, MODE: cont.
pmic 47:c74d09a252d4 41 ok = WriteByte(CONTROL_A, 0x1D); // OSR: 512, RNG: 8G, ODR: 200 Hz, MODE: cont. (betaflight and inav)
pmic 47:c74d09a252d4 42 cntr++;
pmic 47:c74d09a252d4 43 if (ok || cntr == 5) {
pmic 47:c74d09a252d4 44 break;
pmic 47:c74d09a252d4 45 }
pmic 47:c74d09a252d4 46 }
pmic 47:c74d09a252d4 47 if (!ok) printf(" QMC5883L initialisation failed");
pmic 47:c74d09a252d4 48
pmic 47:c74d09a252d4 49 cntr = 0;
pmic 47:c74d09a252d4 50 while (true) {
pmic 47:c74d09a252d4 51 ok = WriteByte(SET_RESET, 0x01);
pmic 47:c74d09a252d4 52 cntr++;
pmic 47:c74d09a252d4 53 if (ok || cntr == 5) {
pmic 47:c74d09a252d4 54 break;
pmic 47:c74d09a252d4 55 }
pmic 47:c74d09a252d4 56 }
pmic 47:c74d09a252d4 57 if (!ok) printf(" QMC5883L soft reset not successful");
pmic 47:c74d09a252d4 58
pmic 47:c74d09a252d4 59 thread_sleep_for(10);
pmic 47:c74d09a252d4 60 }
pmic 47:c74d09a252d4 61
pmic 47:c74d09a252d4 62 void QMC5883L::getMagValue(const uint8_t& OUT_LSB, const uint8_t& OUT_MSB, int16_t& retval)
pmic 47:c74d09a252d4 63 {
pmic 47:c74d09a252d4 64 static uint8_t LoByte, HiByte;
pmic 47:c74d09a252d4 65 bool ok = ReadByte(OUT_LSB, LoByte);
pmic 47:c74d09a252d4 66 if (ok) {
pmic 47:c74d09a252d4 67 ok = ReadByte(OUT_MSB, HiByte);
pmic 47:c74d09a252d4 68 if (ok) {
pmic 47:c74d09a252d4 69 retval = (HiByte<<8) | LoByte;
pmic 47:c74d09a252d4 70 }
pmic 47:c74d09a252d4 71 }
pmic 47:c74d09a252d4 72 }
pmic 47:c74d09a252d4 73
pmic 47:c74d09a252d4 74 bool QMC5883L::ReadByte(const uint8_t& reg, uint8_t& data)
pmic 47:c74d09a252d4 75 {
pmic 47:c74d09a252d4 76 static char data_out[1], data_in[1];
pmic 47:c74d09a252d4 77 data_out[0] = reg;
pmic 47:c74d09a252d4 78 bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 1, 0);
pmic 47:c74d09a252d4 79 if (!nack) {
pmic 47:c74d09a252d4 80 nack = i2c.read(QMC5883L_ADDRESS, data_in, 1, 0);
pmic 47:c74d09a252d4 81 if (!nack) {
pmic 47:c74d09a252d4 82 data = (data_in[0]);
pmic 47:c74d09a252d4 83 }
pmic 47:c74d09a252d4 84 }
pmic 47:c74d09a252d4 85 return !nack;
pmic 47:c74d09a252d4 86 }
pmic 47:c74d09a252d4 87
pmic 47:c74d09a252d4 88 bool QMC5883L::WriteByte(const uint8_t& reg, const uint8_t& data)
pmic 47:c74d09a252d4 89 {
pmic 47:c74d09a252d4 90 static char data_out[2];
pmic 47:c74d09a252d4 91 data_out[0] = reg;
pmic 47:c74d09a252d4 92 data_out[1] = data;
pmic 47:c74d09a252d4 93 bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 2, 0);
pmic 47:c74d09a252d4 94 return !nack;
pmic 47:c74d09a252d4 95 }