269 lines
6.0 KiB
C
269 lines
6.0 KiB
C
/*
|
|
* AS5048A.c
|
|
*
|
|
* Created: 10/07/2021 15:40:53
|
|
* Author: Nick-XMG
|
|
*/
|
|
|
|
/*
|
|
* Read a register from the sensor
|
|
* Takes the address of the register as a 16 bit word
|
|
* Returns the value of the register
|
|
|
|
**
|
|
* Utility function used to calculate even parity of word
|
|
*/
|
|
|
|
static const float AS5048A_MAX_VALUE = 8191.0;
|
|
|
|
#include "AS5048A.h"
|
|
|
|
uint8_t errorFlag = 0;
|
|
|
|
bool AS5048A_init(void){
|
|
if(spi_m_sync_get_io_descriptor(&SPI_0, &io)) {
|
|
return false;
|
|
}
|
|
spi_m_sync_set_mode(&SPI_0,SPI_MODE_1);
|
|
spi_m_sync_enable(&SPI_0);
|
|
|
|
as5048a.errorFlag = 0;
|
|
as5048a.position = 0;
|
|
return true;
|
|
}
|
|
|
|
uint8_t spiCalcEvenParity(uint16_t value){
|
|
uint8_t cnt = 0;
|
|
uint8_t i;
|
|
for (i = 0; i < 16; i++)
|
|
{
|
|
if (value & 0x1)
|
|
{
|
|
cnt++;
|
|
}
|
|
value >>= 1;
|
|
}
|
|
return cnt & 0x1;
|
|
}
|
|
|
|
|
|
|
|
uint16_t read(uint16_t registerAddress)
|
|
{
|
|
|
|
uint8_t in_buf[2], out_buf[2];
|
|
|
|
volatile uint16_t command = 0b0100000000000000; // PAR=0 R/W=R
|
|
command = command | registerAddress;
|
|
|
|
//Add a parity bit on the the MSB
|
|
command |= ((uint16_t)spiCalcEvenParity(command)<<15);
|
|
|
|
//Split the command into two bytes
|
|
out_buf[1] = command & 0xFF;
|
|
out_buf[0] = ( command >> 8 ) & 0xFF;
|
|
|
|
struct spi_xfer xfer;
|
|
xfer.rxbuf = in_buf;
|
|
xfer.txbuf = out_buf;
|
|
xfer.size = 2;
|
|
|
|
|
|
/* Send Command */
|
|
gpio_set_pin_level(AS5048A_CS, false);
|
|
|
|
spi_m_sync_transfer(&SPI_0, &xfer);
|
|
gpio_set_pin_level(AS5048A_CS, true);
|
|
// spi_m_sync_transfer(&SPI_0, &xfer);
|
|
//HAL_SPI_Transmit(_spi, (uint8_t *)&data, 2, 0xFFFF);
|
|
////while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {}
|
|
//gpio_set_pin_level(AS5048A_CS, true);
|
|
//delay_us(1);
|
|
//gpio_set_pin_level(AS5048A_CS, false);
|
|
////xfer.txbuf = dummy_packet;
|
|
//spi_m_sync_transfer(&SPI_0, &xfer);
|
|
/////* Send dummy data to get reply */
|
|
//gpio_set_pin_level(AS5048A_CS, true);
|
|
//////xfer.txbuf[0] = 0x00;
|
|
//////xfer.txbuf[1] = 0x00;
|
|
//////spi_m_sync_transfer(&SPI_0, &xfer);
|
|
//////HAL_SPI_Receive(_spi, (uint8_t *)&data, 2, 0xFFFF);
|
|
//////while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {}
|
|
////gpio_set_pin_level(AS5048A_CS, true);
|
|
|
|
if (in_buf[1] & 0x40) {
|
|
as5048a.errorFlag = 1;
|
|
} else {
|
|
as5048a.errorFlag = 0;
|
|
}
|
|
|
|
//Return the data, stripping the parity and error bits
|
|
|
|
//return (in_buf[1]<<8u)|in_buf[0];
|
|
//return (( ( data[1] & 0xFF ) << 8 ) | ( data[0] & 0xFF )) & ~0xC000;
|
|
return (( ( in_buf[1] & 0xFF ) << 8 ) | ( in_buf[0] & 0xFF )) & ~0xC000;
|
|
}
|
|
|
|
/*
|
|
* Write to a register
|
|
* Takes the 16-bit address of the target register and the 16 bit word of data
|
|
* to be written to that register
|
|
* Returns the value of the register after the write has been performed. This
|
|
* is read back from the sensor to ensure a sucessful write.
|
|
*/
|
|
//uint16_t write(uint16_t registerAddress, uint16_t data) {
|
|
//
|
|
//uint8_t dat[2];
|
|
//
|
|
//uint16_t command = 0b0000000000000000; // PAR=0 R/W=W
|
|
//command |= registerAddress;
|
|
//
|
|
////Add a parity bit on the the MSB
|
|
//command |= ((uint16_t)spiCalcEvenParity(command)<<15);
|
|
//
|
|
////Split the command into two bytes
|
|
//dat[1] = command & 0xFF;
|
|
//dat[0] = ( command >> 8 ) & 0xFF;
|
|
//
|
|
////Start the write command with the target address
|
|
//EN_SPI;
|
|
//HAL_SPI_Transmit(_spi, (uint8_t *)&dat, 2, 0xFFFF);
|
|
//while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {}
|
|
//DIS_SPI;
|
|
//
|
|
//uint16_t dataToSend = 0b0000000000000000;
|
|
//dataToSend |= data;
|
|
//
|
|
////Craft another packet including the data and parity
|
|
//dataToSend |= ((uint16_t)spiCalcEvenParity(dataToSend)<<15);
|
|
//dat[1] = command & 0xFF;
|
|
//dat[0] = ( command >> 8 ) & 0xFF;
|
|
//
|
|
////Now send the data packet
|
|
//EN_SPI;
|
|
//HAL_SPI_Transmit(_spi, (uint8_t *)&dat, 2, 0xFFFF);
|
|
//while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {}
|
|
//DIS_SPI;
|
|
//
|
|
////Send a NOP to get the new data in the register
|
|
//dat[1] = 0x00;
|
|
//dat[0] = 0x00;
|
|
//EN_SPI;
|
|
//HAL_SPI_Transmit(_spi, (uint8_t *)&dat, 2, 0xFFFF);
|
|
//while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {}
|
|
//HAL_SPI_Receive(_spi, (uint8_t *)&dat, 2, 0xFFFF);
|
|
//while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {}
|
|
//DIS_SPI;
|
|
//
|
|
////Return the data, stripping the parity and error bits
|
|
//return (( ( dat[1] & 0xFF ) << 8 ) | ( dat[0] & 0xFF )) & ~0xC000;
|
|
//}
|
|
|
|
/**
|
|
* Returns the raw angle directly from the sensor
|
|
*/
|
|
uint16_t getRawRotation(){
|
|
return read(AS5048A_ANGLE);
|
|
}
|
|
|
|
/**
|
|
* Get the rotation of the sensor relative to the zero position.
|
|
*
|
|
* @return {int} between -2^13 and 2^13
|
|
*/
|
|
int getRotation(){
|
|
volatile uint16_t data;
|
|
volatile int rotation;
|
|
|
|
data = getRawRotation();
|
|
rotation = (int)data - (int)as5048a.position;
|
|
if(rotation > 8191) rotation = -((0x3FFF)-rotation); //more than -180
|
|
//if(rotation < -0x1FFF) rotation = rotation+0x3FFF;
|
|
|
|
return rotation;
|
|
}
|
|
|
|
/**
|
|
* returns the value of the state register
|
|
* @return 16 bit word containing flags
|
|
*/
|
|
uint16_t getState(){
|
|
return read(AS5048A_DIAG_AGC);
|
|
}
|
|
|
|
/*
|
|
* Check if an error has been encountered.
|
|
*/
|
|
uint8_t error(){
|
|
return errorFlag;
|
|
}
|
|
|
|
/**
|
|
* Returns the value used for Automatic Gain Control (Part of diagnostic
|
|
* register)
|
|
*/
|
|
uint8_t getGain(){
|
|
uint16_t data = getState();
|
|
return (uint8_t) data & 0xFF;
|
|
}
|
|
|
|
/*
|
|
* Get and clear the error register by reading it
|
|
*/
|
|
uint16_t getErrors(){
|
|
return read(AS5048A_CLEAR_ERROR_FLAG);
|
|
}
|
|
|
|
/*
|
|
* Set the zero AS5048A.position
|
|
*/
|
|
void setZeroPosition(uint16_t arg_position){
|
|
as5048a.position = arg_position % 0x3FFF;
|
|
}
|
|
|
|
/*
|
|
* Returns the current zero AS5048A.position
|
|
*/
|
|
uint16_t getZeroPosition(){
|
|
return as5048a.position;
|
|
}
|
|
|
|
/*
|
|
* Returns normalized angle value
|
|
*/
|
|
float normalize(float angle) {
|
|
// http://stackoverflow.com/a/11498248/3167294
|
|
#ifdef ANGLE_MODE_1
|
|
angle += 180;
|
|
#endif
|
|
angle = fmod(angle, 360);
|
|
if (angle < 0) {
|
|
angle += 360;
|
|
}
|
|
#ifdef ANGLE_MODE_1
|
|
angle -= 180;
|
|
#endif
|
|
return angle;
|
|
}
|
|
|
|
/*
|
|
* Returns calculated angle value
|
|
*/
|
|
float read2angle(uint16_t angle) {
|
|
/*
|
|
* 14 bits = 2^(14) - 1 = 16.383
|
|
*
|
|
* https://www.arduino.cc/en/Reference/Map
|
|
*
|
|
*/
|
|
return (float)angle * ((float)360 / 16383);
|
|
};
|
|
|
|
float getRotationInDegrees()
|
|
{
|
|
int16_t rotation = getRotation();
|
|
float degrees = 360.0 * (rotation + AS5048A_MAX_VALUE) / (AS5048A_MAX_VALUE * 2.0);
|
|
printf("%f\r\n", degrees);
|
|
return degrees;
|
|
|
|
} |