57 lines
1.2 KiB
C++
57 lines
1.2 KiB
C++
|
|
#include "MT6816.h"
|
|
|
|
MT6816::MT6816(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
|
|
};
|
|
|
|
MT6816::~MT6816() {
|
|
};
|
|
|
|
void MT6816::init(SPIClass* _spi) {
|
|
spi = _spi;
|
|
if (nCS >= 0) {
|
|
pinMode(nCS, OUTPUT);
|
|
digitalWrite(nCS, HIGH);
|
|
spi->begin();
|
|
}
|
|
};
|
|
|
|
uint16_t MT6816::readRawAngle() {
|
|
uint16_t angle_data = 0;
|
|
angle_data = spi_transfer16(MT6816_READ_REG_03) << 8;
|
|
angle_data |= spi_transfer16(MT6816_READ_REG_04);
|
|
|
|
if ((angle_data & MT6816_NO_MAGNET_WARNING_BIT) == MT6816_NO_MAGNET_WARNING_BIT) {
|
|
this->no_magnetic_reading = true;
|
|
} else {
|
|
this->no_magnetic_reading = false;
|
|
}
|
|
|
|
if (!this->parityCheck(angle_data)) {
|
|
return 0;
|
|
}
|
|
|
|
return (angle_data >> 2);
|
|
}
|
|
|
|
bool MT6816::parityCheck(uint16_t data) {
|
|
data ^= data >> 8;
|
|
data ^= data >> 4;
|
|
data ^= data >> 2;
|
|
data ^= data >> 1;
|
|
|
|
return (~data) & 1;
|
|
}
|
|
|
|
uint16_t MT6816::spi_transfer16(uint16_t outdata) {
|
|
if (nCS>=0)
|
|
digitalWrite(nCS, 0);
|
|
spi->beginTransaction(settings);
|
|
uint16_t result = spi->transfer16(outdata);
|
|
spi->endTransaction();
|
|
if (nCS>=0)
|
|
digitalWrite(nCS, 1);
|
|
|
|
return result;
|
|
}
|