139 lines
3.7 KiB
C
139 lines
3.7 KiB
C
|
#include "AS5600.h"
|
|||
|
|
#include "i2c.h"
|
|||
|
|
#include "main.h"
|
|||
|
|
#include <stdint.h>
|
|||
|
|
#include "log.h"
|
|||
|
|
|
|||
|
|
#define AS5600_ADDRESS 0x36 << 1 // AS5600的I2C从机地址（左移1位）
|
|||
|
|
#define AS5600_REG_ZPOS_H 0x01 // 起始位置11-8
|
|||
|
|
#define AS5600_REG_ZPOS_L 0x02 // 起始位置7-0
|
|||
|
|
#define AS5600_REG_MPOS_H 0x03 // 终止位置11-8
|
|||
|
|
#define AS5600_REG_MPOS_L 0x04 // 终止位置7-0
|
|||
|
|
#define AS5600_REG_MANG_H 0x05 // 最大角度11-8
|
|||
|
|
#define AS5600_REG_MANG_L 0x06 // 最大角度7-0
|
|||
|
|
#define AS5600_REG_CFG_1 0x07 // CONF高位寄存器
|
|||
|
|
#define AS5600_REG_CFG_2 0x08 // CONF低位寄存器
|
|||
|
|
#define AS5600_REG_RAW_ANGLE_H 0x0C // 原始角度11-8(读)
|
|||
|
|
#define AS5600_REG_RAW_ANGLE_L 0x0D // 原始角度7-0(读)
|
|||
|
|
#define AS5600_REG_ANGLE_H 0x0E // 角度11-8(读)
|
|||
|
|
#define AS5600_REG_ANGLE_L 0x0F // 角度7-0(读)
|
|||
|
|
#define AS5600_REG_STATUS 0x0B // 状态寄存器(读)
|
|||
|
|
#define AS5600_REG_AGC 0x1A // AGC寄存器(读)
|
|||
|
|
#define AS5600_REG_MAGNITUDE_1 0x1B // MAGNITUDE寄存器(读)
|
|||
|
|
#define AS5600_REG_MAGNITUDE_2 0x1C // MAGNITUDE寄存器(读)
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
* 函 数：AS5600初始化
|
|||
|
|
*/
|
|||
|
|
int AS5600_Init(void)
|
|||
|
|
{
|
|||
|
|
int ret = -1;
|
|||
|
|
ret = AS5600_CheckMagnet();
|
|||
|
|
if (ret == 0)
|
|||
|
|
{
|
|||
|
|
// _dbg_printf("AS5600 Init Error: Magnet not detected!\r\n");
|
|||
|
|
return -1; // 磁体异常，返回错误
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
* 函 数：AS5600写寄存器
|
|||
|
|
* 参 数：RegAddress 寄存器地址
|
|||
|
|
* 参 数：Data 要写入寄存器的数据
|
|||
|
|
*/
|
|||
|
|
void AS5600_WriteReg(uint8_t RegAddress, uint8_t Data)
|
|||
|
|
{
|
|||
|
|
HAL_I2C_Mem_Write(&hi2c2, AS5600_ADDRESS, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, 1000);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
* 函 数：AS5600读寄存器
|
|||
|
|
* 参 数：RegAddress 寄存器地址
|
|||
|
|
* 返 回 值：读取寄存器的数据
|
|||
|
|
*/
|
|||
|
|
uint8_t AS5600_ReadReg(uint8_t RegAddress)
|
|||
|
|
{
|
|||
|
|
uint8_t Data;
|
|||
|
|
HAL_I2C_Mem_Read(&hi2c2, AS5600_ADDRESS, RegAddress, I2C_MEMADD_SIZE_8BIT, &Data, 1, 1000);
|
|||
|
|
return Data;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
* 函 数：AS5600获取原始角度
|
|||
|
|
* 参 数：无
|
|||
|
|
* 返 回 值：12位原始角度值（0-4095）
|
|||
|
|
*/
|
|||
|
|
uint16_t AS5600_GetRawAngle(void)
|
|||
|
|
{
|
|||
|
|
uint8_t AngleH, AngleL;
|
|||
|
|
uint16_t RawAngle;
|
|||
|
|
|
|||
|
|
// 读取角度高位寄存器（0x0E）
|
|||
|
|
AngleH = AS5600_ReadReg(AS5600_REG_ANGLE_H);
|
|||
|
|
// 读取角度低位寄存器（0x0F）
|
|||
|
|
AngleL = AS5600_ReadReg(AS5600_REG_ANGLE_L);
|
|||
|
|
|
|||
|
|
// 组合12位角度值（高4位 + 低8位）
|
|||
|
|
RawAngle = ((uint16_t)(AngleH & 0x0F) << 8) | AngleL;
|
|||
|
|
|
|||
|
|
return RawAngle;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* 函 数：AS5600获取状态
|
|||
|
|
* 参 数：无
|
|||
|
|
* 返 回 值：状态寄存器值
|
|||
|
|
* 说 明：bit5(MD)=1表示磁体在有效范围
|
|||
|
|
*/
|
|||
|
|
uint8_t AS5600_GetStatus(void)
|
|||
|
|
{
|
|||
|
|
return AS5600_ReadReg(AS5600_REG_STATUS);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* 函 数：AS5600获取AGC值
|
|||
|
|
* 参 数：无
|
|||
|
|
* 返 回 值：AGC值（0-255）
|
|||
|
|
* 说 明：反映磁场强度，理想值100-150
|
|||
|
|
*/
|
|||
|
|
uint8_t AS5600_GetAGC(void)
|
|||
|
|
{
|
|||
|
|
return AS5600_ReadReg(AS5600_REG_AGC);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* 函 数：AS5600获取处理后的角度
|
|||
|
|
* 参 数：无
|
|||
|
|
* 返 回 值：12位角度值（0-4095）
|
|||
|
|
* 说 明：经过滤波和零位校准后的角度
|
|||
|
|
*/
|
|||
|
|
uint16_t AS5600_GetAngle(void)
|
|||
|
|
{
|
|||
|
|
uint8_t AngleH, AngleL;
|
|||
|
|
uint16_t Angle;
|
|||
|
|
|
|||
|
|
// 读取角度高位寄存器（0x0E）
|
|||
|
|
AngleH = AS5600_ReadReg(AS5600_REG_ANGLE_H);
|
|||
|
|
// 读取角度低位寄存器（0x0F）
|
|||
|
|
AngleL = AS5600_ReadReg(AS5600_REG_ANGLE_L);
|
|||
|
|
|
|||
|
|
// 组合12位角度值（高4位 + 低8位）
|
|||
|
|
Angle = ((uint16_t)(AngleH & 0x0F) << 8) | AngleL;
|
|||
|
|
|
|||
|
|
return Angle;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* 函 数：AS5600检查磁体状态
|
|||
|
|
* 参 数：无
|
|||
|
|
* 返 回 值：1-磁体正常，0-磁体丢失或太弱
|
|||
|
|
*/
|
|||
|
|
uint8_t AS5600_CheckMagnet(void)
|
|||
|
|
{
|
|||
|
|
uint8_t status = AS5600_GetStatus();
|
|||
|
|
// 检查状态寄存器的bit5(MD)
|
|||
|
|
return (status & 0x20) ? 1 : 0;
|
|||
|
|
}
|