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;
|
||
}
|