313 lines
12 KiB
C++
313 lines
12 KiB
C++
#include "tle5012b.h"
|
|
#include "stdbool.h"
|
|
|
|
#define SPI_TX_OFF() \
|
|
do { \
|
|
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_5); \
|
|
} while(0)
|
|
#define SPI_TX_ON() \
|
|
do { \
|
|
gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_5); \
|
|
} while(0)
|
|
|
|
#define SPI_CS_ENABLE() gpio_bit_reset(GPIOA, GPIO_PIN_15)
|
|
#define SPI_CS_DISABLE() gpio_bit_set(GPIOA, GPIO_PIN_15)
|
|
|
|
|
|
/**
|
|
* @brief SPI command definitions for TLI5012B sensor
|
|
* @note All commands are 16-bit values following the TLI5012B protocol
|
|
*/
|
|
|
|
/* SPI operation types */
|
|
static constexpr uint16_t WRITE = 0; /**< SPI Write Operation */
|
|
static constexpr uint16_t READ = 1; /**< SPI Read Operation */
|
|
|
|
/* SPI communication constants */
|
|
static constexpr uint16_t DUMMY_BYTE = 0xFFFF; /**< Dummy byte for SPI read operations */
|
|
|
|
static constexpr uint16_t WR_REG = 0x0000; /**< Write command - MSB is 0 */
|
|
static constexpr uint16_t RD_REG = 0x8000; /**< Read command - MSB is 1 */
|
|
|
|
static constexpr uint16_t LockValue_LADDR = 0x0000; /**< Lock value for addresses 0x00-0x04 */
|
|
static constexpr uint16_t LockValue_HADDR = 0x5000; /**< Lock value for addresses 0x05-0x11 */
|
|
|
|
/* Update buffer access command */
|
|
static constexpr uint16_t UPD_CMD = 0x0400; /**< Update command - bit 10 is 1, access update buffer */
|
|
|
|
/* Read command values */
|
|
static constexpr uint16_t READ_STAT_VALUE = 0x8001; /**< Read status register */
|
|
static constexpr uint16_t READ_ACSTAT_VALUE = 0x8011; /**< Read activation status register */
|
|
static constexpr uint16_t READ_ANGLE_VALUE = 0x8021; /**< Read angle value register */
|
|
static constexpr uint16_t READ_SPEED_VALUE = 0x8031; /**< Read speed value register */
|
|
|
|
/* Write command values and their corresponding data */
|
|
static constexpr uint16_t WRITE_MOD1_VALUE = 0x5061; /**< Write to MOD1 register (0_1010_0_000110_0001) */
|
|
static constexpr uint16_t MOD1_VALUE = 0x0001; /**< MOD1 register configuration value */
|
|
|
|
static constexpr uint16_t WRITE_MOD2_VALUE = 0x5081; /**< Write to MOD2 register (0_1010_0_001000_0001) */
|
|
static constexpr uint16_t MOD2_VALUE = 0x0809; /**< MOD2 register configuration value */
|
|
|
|
static constexpr uint16_t WRITE_MOD3_VALUE = 0x5091; /**< Write to MOD3 register (0_1010_0_001001_0001) */
|
|
static constexpr uint16_t MOD3_VALUE = 0x0000; /**< MOD3 register configuration value */
|
|
|
|
static constexpr uint16_t WRITE_MOD4_VALUE = 0x50E1; /**< Write to MOD4 register (0_1010_0_001110_0001) */
|
|
static constexpr uint16_t MOD4_VALUE = 0x0080; /**< MOD4 register value: 12-bit 4096, absolute count disable 0x0080; enable 0x0000 */
|
|
|
|
/**
|
|
* @brief Send or read half word through SPI
|
|
*
|
|
* @param[in] spi_periph SPI peripheral
|
|
* @param[in] i_HalfWord data
|
|
*/
|
|
uint16_t spi_rw_half_word(uint32_t spi_periph, const uint16_t i_HalfWord)
|
|
{
|
|
uint32_t to;
|
|
uint16_t data = 0xFFFF;
|
|
bool ok = true;
|
|
|
|
/* ---------- Send data ---------- */
|
|
to = 200;
|
|
|
|
|
|
while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TBE)) && --to);
|
|
if (to == 0) ok = false;
|
|
|
|
if (ok)
|
|
{
|
|
if(RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE))
|
|
{
|
|
(void)spi_i2s_data_receive(spi_periph); /* throw Dummy */
|
|
}
|
|
spi_i2s_data_transmit(spi_periph, i_HalfWord);
|
|
/* wait rx succ*/
|
|
to = 200;
|
|
while ((RESET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE)) && --to);
|
|
if (to == 0) ok = false;
|
|
else
|
|
data = spi_i2s_data_receive(spi_periph); /* throw Dummy */
|
|
}
|
|
|
|
/* ---------- wait bsu idle ---------- */
|
|
if (ok)
|
|
{
|
|
to = 200;
|
|
while ((SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_TRANS)) \
|
|
&& SET == spi_i2s_flag_get(spi_periph, SPI_FLAG_RBNE) && --to);
|
|
if (to == 0) ok = false;
|
|
}
|
|
|
|
return ok ? data : 0xFFFF;
|
|
}
|
|
/**
|
|
* @brief Write data to TLI5012B register
|
|
* @param i_Cmd Command word to send
|
|
* @param i_Data Data to write to register
|
|
* @return Register response value, 0xFFFF if error
|
|
* @note This function performs a complete SPI write transaction:
|
|
*/
|
|
uint16_t cdd_TLI5012_WriteReg(uint16_t i_Cmd, uint16_t i_Data)
|
|
{
|
|
uint16_t result = 0xFFFF; /* Default error code */
|
|
SPI_CS_ENABLE();
|
|
result = spi_rw_half_word(SPI2, i_Cmd);
|
|
result = spi_rw_half_word(SPI2, i_Data);
|
|
SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */
|
|
result = spi_rw_half_word(SPI2, DUMMY_BYTE);
|
|
SPI_CS_DISABLE();
|
|
SPI_TX_ON(); /* Restore MOSI as output */
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* @brief Read data from TLI5012B register
|
|
* @param i_Cmd Command word to send
|
|
* @param i_Data Not used in read operation (kept for compatibility)
|
|
* @return Register data value, 0xFFFF if error
|
|
* @note This function performs a complete SPI read transaction:
|
|
*/
|
|
uint16_t cdd_TLI5012_ReadReg(uint16_t i_Cmd, uint16_t * const i_Data)
|
|
{
|
|
uint16_t result = 0xFFFF; /* Default error code */
|
|
SPI_CS_ENABLE();
|
|
*i_Data = spi_rw_half_word(SPI2, i_Cmd);
|
|
SPI_TX_OFF(); /* Configure MOSI as input to avoid conflicts */
|
|
*i_Data = spi_rw_half_word(SPI2, DUMMY_BYTE);
|
|
result = spi_rw_half_word(SPI2, DUMMY_BYTE);
|
|
SPI_CS_DISABLE();
|
|
SPI_TX_ON(); /* Restore MOSI as output */
|
|
return result;
|
|
}
|
|
|
|
void tle5012b_gpio_init(void)
|
|
{
|
|
rcu_periph_clock_enable(RCU_SPI2);
|
|
|
|
gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
|
|
|
|
/*pin configuration: PA15 CS */
|
|
rcu_periph_clock_enable(RCU_GPIOA);
|
|
gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
|
|
/*rcu_periph_clock_enable*/
|
|
rcu_periph_clock_enable(RCU_GPIOB);
|
|
/* PB3 SCLK, PB5 MOSI, */
|
|
gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_3 | GPIO_PIN_5);
|
|
/* PB4 MISO, */
|
|
gpio_init(GPIOB, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_4);
|
|
|
|
}
|
|
/**
|
|
* @brief Initialize SPI module
|
|
*
|
|
* @details Configures SPI hardware and initializes necessary settings
|
|
*/
|
|
void tle5012b_spi_init(void)
|
|
{
|
|
spi_parameter_struct spi_init_str;
|
|
spi_struct_para_init(&spi_init_str);
|
|
|
|
/* SPI2 parameter config */
|
|
spi_init_str.device_mode = SPI_MASTER;
|
|
spi_init_str.trans_mode = SPI_TRANSMODE_FULLDUPLEX;
|
|
spi_init_str.frame_size = SPI_FRAMESIZE_16BIT;
|
|
spi_init_str.clock_polarity_phase = SPI_CK_PL_LOW_PH_2EDGE;
|
|
spi_init_str.nss = SPI_NSS_SOFT;
|
|
spi_init_str.endian = SPI_ENDIAN_MSB;
|
|
|
|
spi_init_str.prescale = SPI_PSC_16;
|
|
spi_init(SPI2, &spi_init_str);
|
|
spi_nss_output_enable(SPI2);
|
|
spi_nssp_mode_enable(SPI2);
|
|
spi_enable(SPI2);
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
* @brief Initialize TLI5012B magnetic angle sensor
|
|
* @note This function configures the sensor registers for proper operation
|
|
* @return None
|
|
*/
|
|
void cdd_TLI5012_Init(void)
|
|
{
|
|
uint16_t u16Command = 0xFFFF;
|
|
uint16_t u16Data = 0x0000;
|
|
uint16_t u16Safe = 0x0000;
|
|
|
|
/* ---------- Step 1: Read and configure STAT register ---------- */
|
|
/* Read STAT register to get current status */
|
|
u16Command = READ_STAT_VALUE;
|
|
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
/* Write STAT register, set Slave_Number = 00 */
|
|
u16Command = 0x0001;
|
|
u16Data &= 0x9FFF; /* Clear bits 13-14 (Slave_Number) */
|
|
u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
|
|
printf("\r\n [TLI5012Init] [STAT] write u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
/* Read STAT register again to verify write operation */
|
|
u16Command = READ_STAT_VALUE;
|
|
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
printf("\r\n [TLI5012Init] [STAT] read u16Data: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
/* ---------- Step 2: Read ACSTAT register ---------- */
|
|
u16Command = READ_ACSTAT_VALUE;
|
|
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
printf("\r\n [TLI5012Init] read [ACSTAT]: 0x%04X SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
/* ---------- Step 3: Configure MOD1 register ---------- */
|
|
/* Read current MOD1 register value */
|
|
u16Command = RD_REG | WRITE_MOD1_VALUE;
|
|
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Write MOD1 register with configuration value */
|
|
// u16Command = WRITE_MOD1_VALUE;
|
|
// u16Data = MOD1_VALUE;
|
|
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
|
|
// printf("\r\n [TLI5012Init] [MOD1] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Verify MOD1 register write operation */
|
|
// u16Command = RD_REG | WRITE_MOD1_VALUE;
|
|
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
// printf("\r\n [TLI5012Init] read MOD1: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* ---------- Step 4: Configure MOD2 register ---------- */
|
|
// /* Read current MOD2 register value */
|
|
// u16Command = RD_REG | WRITE_MOD2_VALUE;
|
|
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
// printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Write MOD2 register with configuration value */
|
|
// u16Command = WRITE_MOD2_VALUE;
|
|
// u16Data = MOD2_VALUE;
|
|
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
|
|
// printf("\r\n [TLI5012Init] [MOD2] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Verify MOD2 register write operation */
|
|
// u16Command = RD_REG | WRITE_MOD2_VALUE;
|
|
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
// printf("\r\n [TLI5012Init] read MOD2: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* ---------- Step 5: Configure MOD3 register ---------- */
|
|
// /* Read current MOD3 register value */
|
|
// u16Command = RD_REG | WRITE_MOD3_VALUE;
|
|
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
// printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Write MOD3 register with configuration value */
|
|
// u16Command = WRITE_MOD3_VALUE;
|
|
// u16Data = MOD3_VALUE;
|
|
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
|
|
// printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Verify MOD3 register write operation */
|
|
// u16Command = RD_REG | WRITE_MOD3_VALUE;
|
|
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
// printf("\r\n [TLI5012Init] read MOD3: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Optional: Configure MOD3 register with SSC_OD = 1 (commented out) */
|
|
|
|
// u16Command = 0x5091;
|
|
// u16Data |= 0x0004; // Set SSC_OD bit
|
|
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
|
|
// printf("\r\n [TLI5012Init] [MOD3] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
|
|
// /* ---------- Step 6: Configure MOD4 register ---------- */
|
|
// /* Read current MOD4 register value */
|
|
// u16Command = RD_REG | WRITE_MOD4_VALUE;
|
|
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
// printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Write MOD4 register with configuration value */
|
|
// u16Command = WRITE_MOD4_VALUE;
|
|
// u16Data = MOD4_VALUE;
|
|
// u16Safe = cdd_TLI5012_WriteReg(u16Command, u16Data);
|
|
// printf("\r\n [TLI5012Init] [MOD4] write u16Data: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
|
|
// /* Verify MOD4 register write operation */
|
|
// u16Command = RD_REG | WRITE_MOD4_VALUE;
|
|
// u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
// printf("\r\n [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);
|
|
}
|
|
|
|
|
|
|
|
void Test_TLI5012B(void)
|
|
{
|
|
uint16_t u16Command = READ_ANGLE_VALUE;
|
|
uint16_t u16Data = 0x0000;
|
|
uint16_t u16Safe = 0x0000;
|
|
float angle = 0;
|
|
u16Command = READ_ANGLE_VALUE;
|
|
u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
|
|
u16Data = u16Data & 0x7FFF;
|
|
printf ( "\r\n AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
|
|
// angle = 360*( ((int16_t)(u16Data << 1)) >> 1)/32768;
|
|
angle = ( 360*( ( (int16_t)(u16Data << 1) ) >> 1 ) ) >> 15;
|
|
printf ( "\r\n angle = %f, AVAL = %d \r\n", angle, (((int16_t)(u16Data << 1)) >> 1));
|
|
}
|