Rune Harlyk
82 lines
2.3 KiB
C++
82 lines
2.3 KiB
C++
#pragma once
|
|
|
|
#include <peripherals/i2c_bus.h>
|
|
#include <cmath>
|
|
|
|
class HMC5883LDriver {
|
|
public:
|
|
static constexpr uint8_t DEFAULT_ADDR = 0x1E;
|
|
|
|
HMC5883LDriver(uint8_t addr = DEFAULT_ADDR) : _addr(addr) {}
|
|
|
|
bool begin() {
|
|
if (!I2CBus::instance().probe(_addr)) return false;
|
|
|
|
uint8_t idA = readReg(REG_ID_A);
|
|
uint8_t idB = readReg(REG_ID_B);
|
|
uint8_t idC = readReg(REG_ID_C);
|
|
if (idA != 'H' || idB != '4' || idC != '3') return false;
|
|
|
|
writeReg(REG_CONFIG_A, 0x70);
|
|
writeReg(REG_CONFIG_B, 0x20);
|
|
writeReg(REG_MODE, 0x00);
|
|
|
|
_initialized = true;
|
|
return true;
|
|
}
|
|
|
|
bool update() {
|
|
if (!_initialized) return false;
|
|
|
|
uint8_t buf[6];
|
|
if (I2CBus::instance().readReg(_addr, REG_DATA_X_MSB, buf, 6) != ESP_OK) return false;
|
|
|
|
int16_t x = (buf[0] << 8) | buf[1];
|
|
int16_t z = (buf[2] << 8) | buf[3];
|
|
int16_t y = (buf[4] << 8) | buf[5];
|
|
|
|
_mag[0] = x * _scale;
|
|
_mag[1] = y * _scale;
|
|
_mag[2] = z * _scale;
|
|
|
|
_heading = atan2f(_mag[1], _mag[0]);
|
|
_heading += _declination;
|
|
if (_heading < 0) _heading += 2 * M_PI;
|
|
if (_heading > 2 * M_PI) _heading -= 2 * M_PI;
|
|
_heading *= 180.0f / M_PI;
|
|
|
|
return true;
|
|
}
|
|
|
|
void setDeclination(float dec) { _declination = dec; }
|
|
float getMagX() const { return _mag[0]; }
|
|
float getMagY() const { return _mag[1]; }
|
|
float getMagZ() const { return _mag[2]; }
|
|
float getHeading() const { return _heading; }
|
|
bool isInitialized() const { return _initialized; }
|
|
|
|
private:
|
|
static constexpr uint8_t REG_CONFIG_A = 0x00;
|
|
static constexpr uint8_t REG_CONFIG_B = 0x01;
|
|
static constexpr uint8_t REG_MODE = 0x02;
|
|
static constexpr uint8_t REG_DATA_X_MSB = 0x03;
|
|
static constexpr uint8_t REG_ID_A = 0x0A;
|
|
static constexpr uint8_t REG_ID_B = 0x0B;
|
|
static constexpr uint8_t REG_ID_C = 0x0C;
|
|
static constexpr float _scale = 0.92f;
|
|
|
|
void writeReg(uint8_t reg, uint8_t val) { I2CBus::instance().writeReg(_addr, reg, &val, 1); }
|
|
|
|
uint8_t readReg(uint8_t reg) {
|
|
uint8_t val = 0;
|
|
I2CBus::instance().readReg(_addr, reg, &val, 1);
|
|
return val;
|
|
}
|
|
|
|
uint8_t _addr;
|
|
bool _initialized = false;
|
|
float _mag[3] = {0};
|
|
float _heading = 0;
|
|
float _declination = 0.22f;
|
|
};
|