403 lines
8.9 KiB
C++
403 lines
8.9 KiB
C++
/**
|
|
*
|
|
* @license MIT License
|
|
*
|
|
* Copyright (c) 2022 lewis he
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*
|
|
* @file SensorQMC6310.hpp
|
|
* @author Lewis He (lewishe@outlook.com)
|
|
* @date 2022-10-16
|
|
*
|
|
*/
|
|
#pragma once
|
|
|
|
#include "REG/QMC6310Constants.h"
|
|
#include "SensorCommon.tpp"
|
|
|
|
class Polar
|
|
{
|
|
public:
|
|
Polar(): polar(0), Gauss(0), uT(0) {}
|
|
Polar(float polar, float Gauss, float uT): polar(polar), Gauss(Gauss), uT(uT) {}
|
|
float polar;
|
|
float Gauss;
|
|
float uT;
|
|
};
|
|
|
|
|
|
class SensorQMC6310 :
|
|
public SensorCommon<SensorQMC6310>
|
|
{
|
|
friend class SensorCommon<SensorQMC6310>;
|
|
public:
|
|
|
|
|
|
enum SensorMode {
|
|
MODE_SUSPEND,
|
|
MODE_NORMAL,
|
|
MODE_SINGLE,
|
|
MODE_CONTINUOUS,
|
|
};
|
|
|
|
// Unit:Guass
|
|
enum MagRange {
|
|
RANGE_30G,
|
|
RANGE_12G,
|
|
RANGE_8G,
|
|
RANGE_2G,
|
|
};
|
|
|
|
enum OutputRate {
|
|
DATARATE_10HZ,
|
|
DATARATE_50HZ,
|
|
DATARATE_100HZ,
|
|
DATARATE_200HZ,
|
|
};
|
|
|
|
enum CtrlReg {
|
|
SET_RESET_ON,
|
|
SET_ONLY_ON,
|
|
SET_RESET_OFF,
|
|
};
|
|
|
|
enum OverSampleRatio {
|
|
OSR_8,
|
|
OSR_4,
|
|
OSR_2,
|
|
OSR_1,
|
|
};
|
|
|
|
enum DownSampleRatio {
|
|
DSR_1,
|
|
DSR_2,
|
|
DSR_4,
|
|
DSR_8,
|
|
};
|
|
|
|
#if defined(ARDUINO)
|
|
SensorQMC6310(PLATFORM_WIRE_TYPE &w, int sda = DEFAULT_SDA, int scl = DEFAULT_SCL, uint8_t addr = QMC6310_SLAVE_ADDRESS)
|
|
{
|
|
__wire = &w;
|
|
__sda = sda;
|
|
__scl = scl;
|
|
__addr = addr;
|
|
}
|
|
#endif
|
|
|
|
SensorQMC6310()
|
|
{
|
|
#if defined(ARDUINO)
|
|
__wire = &Wire;
|
|
__sda = DEFAULT_SDA;
|
|
__scl = DEFAULT_SCL;
|
|
#endif
|
|
__addr = QMC6310_SLAVE_ADDRESS;
|
|
}
|
|
|
|
~SensorQMC6310()
|
|
{
|
|
deinit();
|
|
}
|
|
|
|
#if defined(ARDUINO)
|
|
bool init(PLATFORM_WIRE_TYPE &w, int sda = DEFAULT_SDA, int scl = DEFAULT_SCL, uint8_t addr = QMC6310_SLAVE_ADDRESS)
|
|
{
|
|
return SensorCommon::begin(w, addr, sda, scl);
|
|
}
|
|
#endif
|
|
|
|
|
|
void deinit()
|
|
{
|
|
// end();
|
|
}
|
|
|
|
void reset()
|
|
{
|
|
writeRegister(QMC6310_REG_CMD2, 0x80);
|
|
delay(10);
|
|
writeRegister(QMC6310_REG_CMD2, 0x00);
|
|
}
|
|
|
|
uint8_t getChipID()
|
|
{
|
|
return readRegister(QMC6310_REG_CHIP_ID);
|
|
}
|
|
|
|
|
|
int getStatus()
|
|
{
|
|
return readRegister(QMC6310_REG_STAT);
|
|
}
|
|
|
|
bool isDataReady()
|
|
{
|
|
if (readRegister(QMC6310_REG_STAT) & 0x01) {
|
|
return true;
|
|
}
|
|
LOG("No ready!\n");
|
|
return false;
|
|
}
|
|
|
|
bool isDataOverflow()
|
|
{
|
|
if (readRegister(QMC6310_REG_STAT) & 0x02) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int setSelfTest(bool en)
|
|
{
|
|
return en ? setRegisterBit(QMC6310_REG_CMD2, 1)
|
|
: clrRegisterBit(QMC6310_REG_CMD2, 1);
|
|
}
|
|
|
|
int setMode(SensorMode m)
|
|
{
|
|
return writeRegister(QMC6310_REG_CMD1, 0xFC, m);
|
|
}
|
|
|
|
int setCtrlRegister(CtrlReg c)
|
|
{
|
|
return writeRegister(QMC6310_REG_CMD2, 0xFC, c);
|
|
}
|
|
|
|
int setDataOutputRate(OutputRate odr)
|
|
{
|
|
return writeRegister(QMC6310_REG_CMD1, 0xF3, (odr << 2));
|
|
}
|
|
|
|
int setOverSampleRate(OverSampleRatio osr)
|
|
{
|
|
return writeRegister(QMC6310_REG_CMD1, 0xCF, (osr << 4));
|
|
}
|
|
|
|
int setDownSampleRate(DownSampleRatio dsr)
|
|
{
|
|
return writeRegister(QMC6310_REG_CMD1, 0x3F, (dsr << 6));
|
|
}
|
|
|
|
// Define the sign for X Y and Z axis
|
|
int setSign(uint8_t x, uint8_t y, uint8_t z)
|
|
{
|
|
int sign = x + y * 2 + z * 4;
|
|
return writeRegister(QMC6310_REG_SIGN, sign);
|
|
}
|
|
|
|
int configMagnetometer(SensorMode mode, MagRange range, OutputRate odr,
|
|
OverSampleRatio osr, DownSampleRatio dsr)
|
|
{
|
|
if (setMagRange(range) != DEV_WIRE_NONE) {
|
|
return DEV_WIRE_ERR;
|
|
}
|
|
if (writeRegister(QMC6310_REG_CMD1, 0xFC, mode) != DEV_WIRE_NONE) {
|
|
return DEV_WIRE_ERR;
|
|
}
|
|
if (writeRegister(QMC6310_REG_CMD1, 0xF3, (odr << 2)) != DEV_WIRE_NONE) {
|
|
return DEV_WIRE_ERR;
|
|
}
|
|
if (writeRegister(QMC6310_REG_CMD1, 0xCF, (osr << 4)) != DEV_WIRE_NONE) {
|
|
return DEV_WIRE_ERR;
|
|
}
|
|
if (writeRegister(QMC6310_REG_CMD1, 0x3F, (dsr << 6)) != DEV_WIRE_NONE) {
|
|
return DEV_WIRE_ERR;
|
|
}
|
|
return DEV_WIRE_NONE;
|
|
}
|
|
|
|
int setMagRange(MagRange range)
|
|
{
|
|
switch (range) {
|
|
case RANGE_30G:
|
|
sensitivity = 0.1;
|
|
break;
|
|
case RANGE_12G:
|
|
sensitivity = 0.04;
|
|
break;
|
|
case RANGE_8G:
|
|
sensitivity = 0.026;
|
|
break;
|
|
case RANGE_2G:
|
|
sensitivity = 0.0066;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return writeRegister(QMC6310_REG_CMD2, 0xF3, (range << 2));
|
|
}
|
|
|
|
void setOffset(int x, int y, int z)
|
|
{
|
|
x_offset = x; y_offset = y; z_offset = z;
|
|
}
|
|
|
|
int readData()
|
|
{
|
|
uint8_t buffer[6];
|
|
int16_t x, y, z;
|
|
if (readRegister(QMC6310_REG_LSB_DX, buffer,
|
|
6) != DEV_WIRE_ERR) {
|
|
x = (int16_t)(buffer[1] << 8) | (buffer[0]);
|
|
y = (int16_t)(buffer[3] << 8) | (buffer[2]);
|
|
z = (int16_t)(buffer[5] << 8) | (buffer[4]);
|
|
|
|
if (x == 32767) {
|
|
x = -((65535 - x) + 1);
|
|
}
|
|
x = (x - x_offset);
|
|
if (y == 32767) {
|
|
y = -((65535 - y) + 1);
|
|
}
|
|
y = (y - y_offset);
|
|
if (z == 32767) {
|
|
z = -((65535 - z) + 1);
|
|
}
|
|
z = (z - z_offset);
|
|
|
|
raw[0] = x;
|
|
raw[1] = y;
|
|
raw[2] = z;
|
|
|
|
mag[0] = (float)x * sensitivity;
|
|
mag[1] = (float)y * sensitivity;
|
|
mag[2] = (float)z * sensitivity;
|
|
return DEV_WIRE_NONE;
|
|
}
|
|
return DEV_WIRE_ERR;
|
|
}
|
|
|
|
void setDeclination(float dec)
|
|
{
|
|
_declination = dec;
|
|
}
|
|
|
|
bool readPolar(Polar &p)
|
|
{
|
|
if (isDataReady()) {
|
|
readData();
|
|
float x = getX();
|
|
float y = getY();
|
|
float z = getZ();
|
|
float angle = (atan2(x, -y) / PI) * 180.0 + _declination;
|
|
angle = _convertAngleToPositive(angle);
|
|
float magnitude = sqrt(x * x + y * y + z * z);
|
|
p = Polar(angle, magnitude * 100, magnitude);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int16_t getRawX()
|
|
{
|
|
return raw[0];
|
|
}
|
|
|
|
int16_t getRawY()
|
|
{
|
|
return raw[1];
|
|
}
|
|
|
|
int16_t getRawZ()
|
|
{
|
|
return raw[2];
|
|
}
|
|
|
|
float getX()
|
|
{
|
|
return mag[0];
|
|
}
|
|
|
|
float getY()
|
|
{
|
|
return mag[1];
|
|
}
|
|
|
|
float getZ()
|
|
{
|
|
return mag[2];
|
|
}
|
|
|
|
void getMag(float &x, float &y, float &z)
|
|
{
|
|
x = mag[0];
|
|
y = mag[1];
|
|
z = mag[2];
|
|
}
|
|
|
|
void dumpCtrlRegister()
|
|
{
|
|
uint8_t buffer[2];
|
|
readRegister(QMC6310_REG_CMD1, buffer, 2);
|
|
for (int i = 0; i < 2; ++i) {
|
|
#if defined(ARDUINO)
|
|
Serial.printf("CMD%d: 0x%02x", i + 1, buffer[i]);
|
|
#else
|
|
printf("CTRL%d: 0x%02x", i + 1, buffer[i]);
|
|
#endif
|
|
#if defined(ARDUINO)
|
|
Serial.print("\t\t BIN:");
|
|
Serial.println(buffer[i], BIN);
|
|
#else
|
|
LOG("\n");
|
|
#endif
|
|
}
|
|
#if defined(ARDUINO)
|
|
Serial.println();
|
|
#else
|
|
printf("\n");
|
|
#endif
|
|
}
|
|
|
|
private:
|
|
|
|
float _convertAngleToPositive(float angle)
|
|
{
|
|
if (angle >= 360.0) {
|
|
angle = angle - 360.0;
|
|
}
|
|
if (angle < 0) {
|
|
angle = angle + 360.0;
|
|
}
|
|
return angle;
|
|
}
|
|
|
|
|
|
bool initImpl()
|
|
{
|
|
reset();
|
|
return getChipID() == QMC6310_DEFAULT_ID;
|
|
}
|
|
|
|
int getReadMaskImpl()
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
protected:
|
|
int16_t raw[3];
|
|
float mag[3];
|
|
float _declination;
|
|
float sensitivity;
|
|
int16_t x_offset = 0, y_offset = 0, z_offset = 0;
|
|
};
|
|
|