STM32G4-DRV8301-FOC/FOC_Related/AS5600/AS5600.c

#include "AS5600.h"
#include "math.h"
#include "LowPass_Filter.h"
#include "foc.h"

extern int DIR; // <20><>ˢ<EFBFBD><CBA2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>

float full_rotations; // <20><>ǰ<EFBFBD><C7B0>תȦ<D7AA><C8A6>

float angle_prev;      // <20>ϴνǶ<CEBD>(<28><><EFBFBD><EFBFBD>λ<EFBFBD>û<EFBFBD>)
float vel_angle_prev;  // <20>ϴνǶ<CEBD>(<28><><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6>)

float angle_prev_Velocity;       // <20>ϴνǶ<CEBD>(<28><><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6>)
float vel_angle_prev_Velocity;   // <20>ϴνǶ<CEBD>(<28><><EFBFBD><EFBFBD><EFBFBD>ٶȻ<D9B6>)

unsigned long angle_prev_ts;      // <20>ϴε<CFB4><CEB5><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
unsigned long vel_angle_prev_ts;  // <20>ϴε<CFB4><CEB5><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>

float raw_angle; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ԭʼֵ
float angle;     // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>ֵ

// <20><>ȡ0x0c<30><63>ַԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
HAL_StatusTypeDef AS5600_ReadRawAngle(I2C_HandleTypeDef *hi2c1, float *raw_angle)
{
    uint8_t data[2]; // <20><><EFBFBD>ڴ洢<DAB4><E6B4A2>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    HAL_StatusTypeDef ret; // <20>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬

    // <20><>AS5600<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0x0c<30><63>ַ<EFBFBD><D6B7>ȡ2<C8A1>ֽ<EFBFBD><D6BD><EFBFBD><EFBFBD><EFBFBD>
    ret = HAL_I2C_Mem_Read(hi2c1, AS5600_I2C_ADDR << 1, AS5600_REG_RAW_ANGLE, I2C_MEMADD_SIZE_8BIT, data, 2, HAL_MAX_DELAY);
    if (ret != HAL_OK)
    {
        return ret; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡʧ<C8A1>ܣ<EFBFBD><DCA3>򷵻ش<F2B7B5BB><D8B4><EFBFBD>״̬
    }

    *raw_angle = (data[0] << 8) | data[1]; // <20><><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ݺϳ<DDBA>Ϊһ<CEAA><D2BB>16λ<36><CEBB>
    return HAL_OK; // <20><><EFBFBD>سɹ<D8B3>״̬
}

// <20><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
HAL_StatusTypeDef AS5600_ReadAngle(I2C_HandleTypeDef *hi2c1, float *angle)
{
    uint8_t data[2]; // <20><><EFBFBD>ڴ洢<DAB4><E6B4A2>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    HAL_StatusTypeDef ret; // <20>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬

    // <20><>AS5600<30><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0x0e<30><65>ַ<EFBFBD><D6B7>ȡ2<C8A1>ֽ<EFBFBD><D6BD><EFBFBD><EFBFBD><EFBFBD>
    ret = HAL_I2C_Mem_Read(hi2c1, AS5600_I2C_ADDR << 1, AS5600_REG_ANGLE, I2C_MEMADD_SIZE_8BIT, data, 2, HAL_MAX_DELAY);
    if (ret != HAL_OK)
    {
        return ret; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡʧ<C8A1>ܣ<EFBFBD><DCA3>򷵻ش<F2B7B5BB><D8B4><EFBFBD>״̬
    }

    *angle = (data[0] << 8) | data[1]; // <20><><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ݺϳ<DDBA>Ϊһ<CEAA><D2BB>16λ<36><CEBB>
    return HAL_OK; // <20><><EFBFBD>سɹ<D8B3>״̬
}


// <20><>ԭʼֵ<CABC><D6B5><EFBFBD><EFBFBD><EFBFBD>ɱ<EFBFBD><C9B1><EFBFBD><EFBFBD><EFBFBD>360<36><30>ֵ
float AS5600_GetAngle360(void)
{
    return raw_angle * 0.08789f; // <20><>ԭʼֵת<D6B5><D7AA>Ϊ0-360<36><30><EFBFBD>ĽǶ<C4BD>
}

float Debug_raw_angle;
// <20><>ȡ<EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ:(0-6.28)
float AS5600_GetAngle2PI(void)
{
    Debug_raw_angle = raw_angle * 0.08789f / 57.32484f;
    
    return raw_angle * 0.08789f / 57.32484f; // <20><>ԭʼֵת<D6B5><D7AA>Ϊ0-2<>еĻ<D0B5><C4BB><EFBFBD>
}

float val,d_angle;

// <20>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽǶ<C6BD><C7B6>ۼƼ<DBBC><C6BC><EFBFBD>:(0-<2D><>)
float AS5600_GetAngle(void)
{
     val = AS5600_GetAngle2PI(); // <20><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD>Ƕ<EFBFBD>
     d_angle = val - angle_prev; // <20><><EFBFBD><EFBFBD><EFBFBD>Ƕȱ仯

    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ȧ<EFBFBD><C8A6>
    // ͨ<><CDA8><EFBFBD>жϽǶȱ仯<C8B1>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>80%<25><>һȦ(0.8f*6.28318530718f)<29><><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>full_rotations<6E><73><EFBFBD><EFBFBD>1(<28><><EFBFBD><EFBFBD>d_angleС<65><D0A1>0)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1(<28><><EFBFBD><EFBFBD>d_angle<6C><65><EFBFBD><EFBFBD>0)
    if (fabs(d_angle) > (0.8f * 6.28318530718f))
        full_rotations += (d_angle > 0) ? -1 : 1; // <20><><EFBFBD>ݽǶȱ仯<C8B1><E4BBAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תȦ<D7AA><C8A6>

    angle_prev = val; // <20><><EFBFBD><EFBFBD><EFBFBD>ϴνǶ<CEBD>
    return (float)full_rotations * 6.28318530718f + angle_prev; // <20><><EFBFBD><EFBFBD><EFBFBD>ۼƽǶ<C6BD>
}

 float Ts, vel = 0.0f; // TsΪ<73><CEAA><EFBFBD><EFBFBD>ʱ<EFBFBD>䣬velΪ<6C>ٶ<EFBFBD>
// <20>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD>:(0-<2D><>)
float AS5600_GetVelocity(void)
{
    //float Ts, vel = 0.0f; // TsΪ<73><CEAA><EFBFBD><EFBFBD>ʱ<EFBFBD>䣬velΪ<6C>ٶ<EFBFBD>

    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
    angle_prev_ts = SysTick->VAL; // <20><>ȡ<EFBFBD><C8A1>ǰʱ<C7B0><CAB1><EFBFBD><EFBFBD>
    
    if (angle_prev_ts < vel_angle_prev_ts)
        Ts = (float)(vel_angle_prev_ts - angle_prev_ts); // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>
    else
        Ts = (float)(0xFFFFFF - angle_prev_ts + vel_angle_prev_ts); // <20><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

    // <20><><EFBFBD><EFBFBD><EFBFBD>޸<EFBFBD>΢С<CEA2><D0A1><EFBFBD><EFBFBD>
    if (Ts == 0 || Ts > 0.5f)
        Ts = 1e-3f; // <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>Ϊ0<CEAA><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

    angle_prev_Velocity = AS5600_GetAngle(); // <20><>ȡ<EFBFBD><C8A1>ǰ<EFBFBD>Ƕ<EFBFBD>
    vel = (angle_prev_Velocity - vel_angle_prev_Velocity) / Ts; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
    vel_angle_prev_Velocity = angle_prev_Velocity; // <20><><EFBFBD><EFBFBD><EFBFBD>ϴνǶ<CEBD>
    vel_angle_prev_ts = angle_prev_ts; // <20><><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>

    return vel; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
}

float vel_M0_flit;
// <20>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȵ<D9B6>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>:(0-<2D><>)
float AS5600_Get_Speed(void)
{
    float vel_M0_ori = AS5600_GetVelocity(); // <20>ٶ<EFBFBD>ԭʼ<D4AD><CABC><EFBFBD>ݲɼ<DDB2>
     vel_M0_flit = LowPass_Filter(DIR * vel_M0_ori); // ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD>
    return vel_M0_flit; // <20><><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>
}