Motor_v2/Robot_V2/device/tle5012b.cpp

#include "tle5012b.h"
#include "core_cmInstr.h"
#include "gd32c10x_gpio.h"
#include "gd32c10x_spi.h"
#include "stdbool.h"
#include "systick.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();
    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
    result = spi_rw_half_word(SPI0, i_Cmd);
    result = spi_rw_half_word(SPI0, i_Data);
    SPI_TX_OFF();  /* Configure MOSI as input to avoid conflicts */
    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
    result = spi_rw_half_word(SPI0, DUMMY_BYTE);
    SPI_CS_DISABLE();
    SPI_TX_ON();  /* Restore MOSI as output */
    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
    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();
    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
    *i_Data = spi_rw_half_word(SPI0, i_Cmd);
    SPI_TX_OFF();  /* Configure MOSI as input to avoid conflicts */
    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP();
    __NOP();__NOP();__NOP();__NOP();__NOP();__NOP(); // 增加延时
    *i_Data = spi_rw_half_word(SPI0, DUMMY_BYTE);    
    result = spi_rw_half_word(SPI0, DUMMY_BYTE);
    SPI_CS_DISABLE();
    SPI_TX_ON();  /* Restore MOSI as output */
    __NOP(); __NOP(); __NOP(); // 添加小延时确保CS稳定
    return result;
}

void tle5012b_gpio_init(void)
{
	rcu_periph_clock_enable(RCU_GPIOA);
    rcu_periph_clock_enable(RCU_GPIOB);
    rcu_periph_clock_enable(RCU_SPI0);
    rcu_periph_clock_enable(RCU_AF);
    /* 关闭 JTAG，仅保留 SWD（释放 PA15、PB3、PB4） */
    gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);

    /* -------- SPI0 全 remap：PB3/4/5 + PA15 -------- */
    gpio_pin_remap_config(GPIO_SPI0_REMAP, ENABLE);
    /* PA15 → NSS, 推挽输出 */
    gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_15);
    /* PB3 → SCK, 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 | GPIO_PIN_10);

}
/**
 * @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);

    /* SPI参数：16bit, 主机, 全双工, CPOL=0 CPHA=1, MSB first */
    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_HIGH_PH_2EDGE;
    spi_init_str.nss                  = SPI_NSS_SOFT;      
    spi_init_str.endian               = SPI_ENDIAN_MSB;

    spi_init_str.prescale = SPI_PSC_16; /* 8MHz @ 128MHz */
    spi_init(SPI0, &spi_init_str);
    spi_nss_output_enable(SPI0);  
    spi_nssp_mode_enable(SPI0);
    spi_enable(SPI0); 
} 



/**
 * @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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [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 @Joe [TLI5012Init] read MOD3: 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 @Joe [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 @Joe [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 @Joe [TLI5012Init] read MOD4: 0x%04X, SafeWord: 0x%04X \r\n", u16Data, u16Safe);    
}



void Test_TLI5012B(void)
{
    uint16_t u16Command = 0xFFFF;
    uint16_t u16Data = 0x0000;
    uint16_t u16Safe = 0x0000;
    float angle = 0;  
    u16Command = READ_ANGLE_VALUE;
    u16Safe = cdd_TLI5012_ReadReg(u16Command, &u16Data);
    
    // 输出原始读取的数据，用于诊断
    printf("\r\n @Joe RAW AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
    
    // 检查是否有错误标志位
    if (u16Safe != 0xFFFF && u16Safe != 0x0000) {
        printf("\r\n @Joe Warning: Non-ideal SafeWord detected \r\n");
    }
    
    // 检查错误位（bit 15）
    if (u16Data & 0x8000) {
        printf("\r\n @Joe Error: Communication error detected (bit 15 set) \r\n");
    }
    
    // 提取角度值(15位)
    u16Data = u16Data & 0x7FFF;
    printf ( "\r\n @Joe AVAL = 0x%04X, SafeWord = 0x%04X \r\n", u16Data, u16Safe);
    
    // 将15位角度值转换为角度(0-360度)
    // 0x7FFF = 32767 对应 360度
    angle = ((float)u16Data * 360.0f) / 32767.0f;
    printf ( "\r\n @Joe angle = %f, AVL = %d \r\n", angle, (int16_t)u16Data);
    
    // 添加额外的传感器状态检查
    uint16_t status_data, status_safe;
    u16Command = READ_STAT_VALUE;
    status_safe = cdd_TLI5012_ReadReg(u16Command, &status_data);
    printf("\r\n @Joe STATUS = 0x%04X, Status SafeWord = 0x%04X \r\n", status_data, status_safe);
}