diff --git a/lib/Espfc/src/Device/MagQMC5338P.h b/lib/Espfc/src/Device/MagQMC5338P.h index a6ef86e2..1d8e782b 100644 --- a/lib/Espfc/src/Device/MagQMC5338P.h +++ b/lib/Espfc/src/Device/MagQMC5338P.h @@ -4,120 +4,171 @@ #include "MagDevice.h" #include "BusDevice.h" -// address for QMC5883P -#define QMC5883P_ADDRESS 0x2C -#define QMC5883P_DEFAULT_ADDRESS 0x2C - -// QMC5883P records according to Adafruit specifications -#define QMC5883P_REG_CHIPID 0x00 -#define QMC5883P_REG_XOUT_LSB 0x01 -#define QMC5883P_REG_XOUT_MSB 0x02 -#define QMC5883P_REG_YOUT_LSB 0x03 -#define QMC5883P_REG_YOUT_MSB 0x04 -#define QMC5883P_REG_ZOUT_LSB 0x05 -#define QMC5883P_REG_ZOUT_MSB 0x06 -#define QMC5883P_REG_STATUS 0x09 -#define QMC5883P_REG_CONTROL1 0x0A -#define QMC5883P_REG_CONTROL2 0x0B - -// Range values (as defined in Adafruit lib) -#define QMC5883P_RANGE_30G 0x00 -#define QMC5883P_RANGE_12G 0x01 -#define QMC5883P_RANGE_8G 0x02 -#define QMC5883P_RANGE_2G 0x03 - -// Mode values -#define QMC5883P_MODE_CONTINUOUS 0x03 // Continuous measurement mode +#define QMC5883P_ADDRESS 0x2C +#define QMC5883P_DEFAULT_ADDRESS 0x2C +#define QMC5883P_CHIP_ID 0x80 + +#define QMC5883P_REG_CHIPID 0x00 +#define QMC5883P_REG_XOUT_LSB 0x01 +#define QMC5883P_REG_XOUT_MSB 0x02 +#define QMC5883P_REG_YOUT_LSB 0x03 +#define QMC5883P_REG_YOUT_MSB 0x04 +#define QMC5883P_REG_ZOUT_LSB 0x05 +#define QMC5883P_REG_ZOUT_MSB 0x06 +#define QMC5883P_REG_STATUS 0x09 +#define QMC5883P_REG_CONTROL1 0x0A +#define QMC5883P_REG_CONTROL2 0x0B + +#define QMC5883P_MODE_SUSPEND 0x00 +#define QMC5883P_MODE_NORMAL 0x01 +#define QMC5883P_MODE_SINGLE 0x02 +#define QMC5883P_MODE_CONTINUOUS 0x03 + +#define QMC5883P_ODR_10HZ 0x00 +#define QMC5883P_ODR_50HZ 0x01 +#define QMC5883P_ODR_100HZ 0x02 +#define QMC5883P_ODR_200HZ 0x03 + +#define QMC5883P_OSR_8 0x00 +#define QMC5883P_OSR_4 0x01 +#define QMC5883P_OSR_2 0x02 +#define QMC5883P_OSR_1 0x03 + +#define QMC5883P_DSR_1 0x00 +#define QMC5883P_DSR_2 0x01 +#define QMC5883P_DSR_4 0x02 +#define QMC5883P_DSR_8 0x03 + +#define QMC5883P_RANGE_30G 0x00 +#define QMC5883P_RANGE_12G 0x01 +#define QMC5883P_RANGE_8G 0x02 +#define QMC5883P_RANGE_2G 0x03 + +#define QMC5883P_SETRESET_ON 0x00 +#define QMC5883P_SETRESET_SETONLY 0x01 +#define QMC5883P_SETRESET_OFF 0x02 + +#define QMC5883P_CONTROL2_RESET_BIT 7 +#define QMC5883P_RESET_DELAY_MS 50 namespace Espfc { namespace Device { -class MagQMC5338P : public MagDevice +class MagQMC5338P: public MagDevice { -public: - int begin(BusDevice* bus) override + public: + int begin(BusDevice * bus) override { - return begin(bus, QMC5883P_DEFAULT_ADDRESS); + return begin(bus, QMC5883P_DEFAULT_ADDRESS); } - int begin(BusDevice* bus, uint8_t addr) override { - setBus(bus, addr); + int begin(BusDevice * bus, uint8_t addr) override + { + setBus(bus, addr); + + _currentRange = QMC5883P_RANGE_8G; + _currentOdr = QMC5883P_ODR_100HZ; + + delay(2); + if(!testConnection()) return 0; + if(!softReset()) return 0; - if (!testConnection()) return 0; + const uint8_t ctrl2 = makeControl2(_currentRange, QMC5883P_SETRESET_ON); + if(!_bus->writeByte(_addr, QMC5883P_REG_CONTROL2, ctrl2)) return 0; - // We use ±8G (0x02) as the default range for the drone. - _currentRange = QMC5883P_RANGE_8G; - setMode(_currentRange); + const uint8_t ctrl1 = makeControl1(QMC5883P_MODE_CONTINUOUS, _currentOdr, QMC5883P_OSR_1, QMC5883P_DSR_1); + if(!_bus->writeByte(_addr, QMC5883P_REG_CONTROL1, ctrl1)) return 0; - // Continuous operating mode - uint8_t ctrl1 = (QMC5883P_MODE_CONTINUOUS) | // bits [1:0] - (0x02 << 2) | // ODR = 100Hz (0x02) - (0x03 << 4) | // OSR = 1 (0x03) ← faster - (0x00 << 6); // DSR = 1 - _bus->writeByte(_addr, QMC5883P_REG_CONTROL1, ctrl1); + delay(10); - // Initial reading - uint8_t buffer[6]; - _bus->read(_addr, QMC5883P_REG_XOUT_LSB, 6, buffer); + // Prime the external sensor read window when the magnetometer is behind a master device. + if(_bus->read(_addr, QMC5883P_REG_XOUT_LSB, 6, _buffer) != 6) return 0; - return 1; + return 1; } - int readMag(VectorInt16& v) override { - uint8_t buffer[6]; - if (_bus->read(_addr, QMC5883P_REG_XOUT_LSB, 6, buffer) != 6) { - return 0; - } + int readMag(VectorInt16& v) override + { + if(_bus->readFast(_addr, QMC5883P_REG_XOUT_LSB, 6, _buffer) != 6) return 0; - // in QMC5883P: X = [MSB=0x02, LSB=0x01] → buffer[1], buffer[0] - v.x = (int16_t)((buffer[1] << 8) | buffer[0]); - v.y = (int16_t)((buffer[3] << 8) | buffer[2]); - v.z = (int16_t)((buffer[5] << 8) | buffer[4]); + v.x = (((int16_t)_buffer[1]) << 8) | _buffer[0]; + v.y = (((int16_t)_buffer[3]) << 8) | _buffer[2]; + v.z = (((int16_t)_buffer[5]) << 8) | _buffer[4]; - return 1; + return 1; } - const VectorFloat convert(const VectorInt16& v) const override { - float lsb_per_gauss; - switch (_currentRange) { - case QMC5883P_RANGE_30G: lsb_per_gauss = 1000.0f; break; - case QMC5883P_RANGE_12G: lsb_per_gauss = 2500.0f; break; - case QMC5883P_RANGE_8G: lsb_per_gauss = 3750.0f; break; - case QMC5883P_RANGE_2G: lsb_per_gauss = 15000.0f; break; - default: lsb_per_gauss = 3750.0f; // fallback to 8G - } - float scale = 1.0f / lsb_per_gauss; - return VectorFloat{ v.x * scale, v.y * scale, v.z * scale }; + const VectorFloat convert(const VectorInt16& v) const override + { + float lsbPerGauss = 3750.0f; + switch(_currentRange) + { + case QMC5883P_RANGE_30G: lsbPerGauss = 1000.0f; break; + case QMC5883P_RANGE_12G: lsbPerGauss = 2500.0f; break; + case QMC5883P_RANGE_8G: lsbPerGauss = 3750.0f; break; + case QMC5883P_RANGE_2G: lsbPerGauss = 15000.0f; break; + } + + return static_cast(v) * (1.0f / lsbPerGauss); } - int getRate() const override { - return 100; // Based on ODR = 100Hz + int getRate() const override + { + switch(_currentOdr) + { + case QMC5883P_ODR_10HZ: return 10; + case QMC5883P_ODR_50HZ: return 50; + case QMC5883P_ODR_200HZ: return 200; + case QMC5883P_ODR_100HZ: + default: + return 100; + } } - virtual MagDeviceType getType() const override { - return MAG_QMC5883P; + virtual MagDeviceType getType() const override + { + return MAG_QMC5883P; } - void setMode(uint8_t range) { - _currentRange = range; - // Set the bits to [3:2] in CONTROL2 - uint8_t ctrl2 = (range << 2); - _bus->writeByte(_addr, QMC5883P_REG_CONTROL2, ctrl2); + bool softReset() + { + if(!_bus->writeByte(_addr, QMC5883P_REG_CONTROL2, (1u << QMC5883P_CONTROL2_RESET_BIT))) return false; + delay(QMC5883P_RESET_DELAY_MS); + return testConnection(); } - bool testConnection() override { - uint8_t chip_id; - if (_bus->read(_addr, QMC5883P_REG_CHIPID, 1, &chip_id) != 1) { - return false; + bool testConnection() override + { + uint8_t chipId = 0; + for(uint8_t attempt = 0; attempt < 3; attempt++) + { + if(_bus->read(_addr, QMC5883P_REG_CHIPID, 1, &chipId) == 1 && chipId == QMC5883P_CHIP_ID) + { + return true; } - return chip_id == 0x80; + delay(2); + } + + return false; + } + + private: + static uint8_t makeControl1(uint8_t mode, uint8_t odr, uint8_t osr, uint8_t dsr) + { + return (mode & 0x03) | ((odr & 0x03) << 2) | ((osr & 0x03) << 4) | ((dsr & 0x03) << 6); + } + + static uint8_t makeControl2(uint8_t range, uint8_t setReset) + { + return ((range & 0x03) << 2) | (setReset & 0x03); } -private: uint8_t _currentRange = QMC5883P_RANGE_8G; + uint8_t _currentOdr = QMC5883P_ODR_100HZ; + uint8_t _buffer[6] = { 0 }; }; -} -} +} +} #endif