56 lines
1.3 KiB
C++
56 lines
1.3 KiB
C++
#include "TLE5012.hpp"
|
|
#include "stm32g0xx_hal.h"
|
|
#include "stm32g0xx_hal_gpio.h"
|
|
#include "stm32g0xx_hal_spi.h"
|
|
#include <stdint.h>
|
|
|
|
|
|
Tle5012::Tle5012(GPIO_TypeDef * csPort, uint16_t csPin, SPI_HandleTypeDef *spiHandler){
|
|
this->csPort = csPort;
|
|
this->csPin = csPin;
|
|
this->spiHandler = spiHandler;
|
|
|
|
}
|
|
double Tle5012::getAngel(){
|
|
// enable chip select
|
|
HAL_GPIO_WritePin(this->csPort, this->csPin, GPIO_PIN_RESET);
|
|
|
|
|
|
|
|
// uint16_t command = 0b1000000000100001;
|
|
uint16_t command = RW | AVAL | SAFETY;
|
|
|
|
uint8_t formatedCommand[6] = {0};
|
|
formatedCommand[0] = (uint8_t)(command >> 8);
|
|
formatedCommand[1] = (uint8_t)command;
|
|
|
|
uint8_t angle[6] = {0};
|
|
uint16_t safety;
|
|
|
|
HAL_SPI_Transmit(this->spiHandler, formatedCommand, 2, 100);
|
|
|
|
HAL_SPI_Receive(this->spiHandler, angle, 4, 0xFFFF);
|
|
// HAL_SPI_Receive(&hspi1, (uint8_t *)(&safety), 2, 0xFF);
|
|
|
|
int16_t signedAngle = ((angle[0] << 8 | angle[1]) & 0x3FFF);
|
|
bool bitSet = 0;
|
|
if (angle[0] & 0b01000000){
|
|
signedAngle -= 16384;
|
|
bitSet = 1;
|
|
}
|
|
else{
|
|
bitSet = 0;
|
|
}
|
|
|
|
double angleDeg = 360.0 / 32768.0 * signedAngle;
|
|
// double angleDeg = 360.0 / 128.0 * ((angle[0] >> 8) & 0x7F);
|
|
|
|
|
|
// disable chip select
|
|
HAL_GPIO_WritePin(this->csPort, this->csPin, GPIO_PIN_SET);
|
|
|
|
return angleDeg;
|
|
|
|
}
|
|
|