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.
QMC5883L.cpp
- Committer:
- pmic
- Date:
- 2022-06-02
- Revision:
- 47:c74d09a252d4
- Child:
- 49:76fcaffb92ef
File content as of revision 47:c74d09a252d4:
#include "QMC5883L.h" QMC5883L::QMC5883L(I2C& i2c): i2c(i2c) { init(); } void QMC5883L::readMag() { getMagValue(OUT_Y_LSB, OUT_Y_MSB, m_mag_val[0]); // x and y are swapped getMagValue(OUT_X_LSB, OUT_X_MSB, m_mag_val[1]); getMagValue(OUT_Z_LSB, OUT_Z_MSB, m_mag_val[2]); } float QMC5883L::magX() { return m_scale * static_cast<float>(m_mag_val[0]); } float QMC5883L::magY() { return m_scale * static_cast<float>(m_mag_val[1]); } float QMC5883L::magZ() { return m_scale * static_cast<float>(m_mag_val[2]); } void QMC5883L::init() { // 31.05.2022, based on experiment so it fits the former mag scaling (norm approx. 0.31) m_scale = 1.97e-4f; static bool ok; static uint8_t cntr; cntr = 0; while (true) { //ok = WriteByte(CONTROL_A, 0x59); // OSR: 256, RNG: 8G, ODR: 100 Hz, MODE: cont. ok = WriteByte(CONTROL_A, 0x1D); // OSR: 512, RNG: 8G, ODR: 200 Hz, MODE: cont. (betaflight and inav) cntr++; if (ok || cntr == 5) { break; } } if (!ok) printf(" QMC5883L initialisation failed"); cntr = 0; while (true) { ok = WriteByte(SET_RESET, 0x01); cntr++; if (ok || cntr == 5) { break; } } if (!ok) printf(" QMC5883L soft reset not successful"); thread_sleep_for(10); } void QMC5883L::getMagValue(const uint8_t& OUT_LSB, const uint8_t& OUT_MSB, int16_t& retval) { static uint8_t LoByte, HiByte; bool ok = ReadByte(OUT_LSB, LoByte); if (ok) { ok = ReadByte(OUT_MSB, HiByte); if (ok) { retval = (HiByte<<8) | LoByte; } } } bool QMC5883L::ReadByte(const uint8_t& reg, uint8_t& data) { static char data_out[1], data_in[1]; data_out[0] = reg; bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 1, 0); if (!nack) { nack = i2c.read(QMC5883L_ADDRESS, data_in, 1, 0); if (!nack) { data = (data_in[0]); } } return !nack; } bool QMC5883L::WriteByte(const uint8_t& reg, const uint8_t& data) { static char data_out[2]; data_out[0] = reg; data_out[1] = data; bool nack = i2c.write(QMC5883L_ADDRESS, data_out, 2, 0); return !nack; }