44 lines
1.6 KiB
C++
44 lines
1.6 KiB
C++
#include "AS5045.h"
|
|
|
|
|
|
MagneticSensorAS5045::MagneticSensorAS5045(uint16_t as5040_cs, uint16_t as5040_mosi, uint16_t as5040_miso,
|
|
uint16_t as5040_sclk): AS5040_CS_(as5040_cs),
|
|
AS5040_MOSI_(as5040_mosi),
|
|
AS5040_MISO_(as5040_miso),
|
|
AS5040_SCLK_(as5040_sclk),
|
|
spi(nullptr),
|
|
settings(AS5145SSISettings) {
|
|
}
|
|
|
|
MagneticSensorAS5045::~MagneticSensorAS5045() = default;
|
|
|
|
auto MagneticSensorAS5045::init(SPIClass *_spi) -> void {
|
|
this->spi = _spi;
|
|
settings = AS5145SSISettings;
|
|
pinMode(AS5040_CS_, OUTPUT);
|
|
pinMode(AS5040_MISO_, INPUT);
|
|
pinMode(AS5040_MOSI_, OUTPUT);
|
|
pinMode(AS5040_SCLK_, OUTPUT);
|
|
|
|
spi->setMISO(AS5040_MISO_);
|
|
spi->setMOSI(AS5040_MOSI_);
|
|
spi->setSCLK(AS5040_SCLK_);
|
|
spi->begin();
|
|
this->Sensor::init();
|
|
}
|
|
|
|
float MagneticSensorAS5045::getSensorAngle() {
|
|
float angle_data = readRawAngleSSI();
|
|
angle_data = (static_cast<float>(angle_data) / AS5045_CPR) * _2PI;
|
|
return angle_data;
|
|
}
|
|
|
|
uint16_t MagneticSensorAS5045::readRawAngleSSI() const {
|
|
spi->beginTransaction(settings);
|
|
digitalWrite(AS5040_CS_, LOW);
|
|
uint16_t value = spi->transfer16(0x0000);
|
|
digitalWrite(AS5040_CS_, HIGH);
|
|
spi->endTransaction();
|
|
delayMicroseconds(30);
|
|
return (value >> 3) & 0x1FFF; // TODO(vanyabeat): Add error checking MAGNETIC OWERFLOW and etc.
|
|
}
|