AC-Fan-JingDa-FU6562T/User/Source/RSDDetect.c

/**************************** (C) COPYRIGHT 2018 Fortiortech shenzhen *****************************
    File Name          : RSDDetect.c
    Author             : Bruce,Fortiortech  RD
    Version            : V1.0
    Date               : 2018-07-01
    Description        : This file contains init speed detection used for Motor Control.
***************************************************************************************************
    All Rights Reserved
**************************************************************************************************/


/* Includes -------------------------------------------------------------------------------------*/
#include <MyProject.h>





#if (TAILWIND_MODE == RSDMethod)



MotorRSDTypeDef    mcRsd;

static void Time2_RSD_Init(void);
static void CMP_RSD_Init(void);
static void RSDFOCCloseLoopStart(void);





/**
    @brief        RSD    启动处理函数
    @date         2022-07-13
*/
MotStateType RSDStartProcess(void)
{
    MotStateType returnStatus;
    returnStatus = mcTailWind;
    
    if (mcRsd.SpeedUpdate == 1)
    {
        if (mcRsd.Speed < S_Value(-50))  // 逆风
        {
            mcFocCtrl.State_Count            = 1000;
            mcFocCtrl.Start_Mode            = HEADWIND_START;      // 逆风启动
            ClrBit(CMP_CR2, CMP0EN);                               // 关闭比较器
            ClrBit(TIM2_CR1, T2CEN);                               // 0，停止计数；1,使能计数
            returnStatus =  mcStart;
        }
        else if (mcRsd.Speed > S_Value(1000))  //正转
        {
            mcRsd.SpeedUpdate = 0;
        }
        else if (mcRsd.Speed > MOTOR_LOOP_RPM)  //正转
        {
            if (mcRsd.HighSpdStart == 2) //启动完成，切状态机
            {
                mcFocCtrl.Start_Mode    = TAILWIND_START;
                returnStatus =       mcStart;
            }
            
            if (mcRsd.HighSpdStart == 0)
            {
                mcRsd.HighSpdStart = 1; // 设置启动
            }
        }
        else//其他，如静止
        {
            mcFocCtrl.Start_Mode    = STATIC_START;
            ClrBit(CMP_CR2, CMP0EN);                               // 关闭比较器
            ClrBit(TIM2_CR1, T2CEN);                               // 0，停止计数；1,使能计数
            #if (PosCheckEnable == Enable)
            returnStatus                             = mcPosiCheck;
            McStaSet.SetFlag.PosiCheckSetFlag   = 0;
            mcFocCtrl.mcPosCheckAngle           = 0xffff;                     // 角度赋初值
            #elif (ALIGN_MOME != ALIGN_DSIABLE)
            mcFocCtrl.mcPosCheckAngle           = Align_Angle;
            returnStatus                             = mcAlign;
            mcFocCtrl.State_Count               = Align_Time;
            #else
            returnStatus = mcStart;
            #endif
        }
    }
    
    return      returnStatus;
}



/**
    @brief        RSD初始化函数，电机状态切入 顺逆风检测状态时 运行一次
    @date         2022-07-13
*/
void RSDDetectInit(void)
{
    MOE                         = 0;
    mcRsd.SpeedUpdate           = 0;
    mcRsd.Times                 = 0;
    mcRsd.Period                = 0;
    mcRsd.Count                 = 0;
    mcRsd.CountPre              = 0;
    mcRsd.State                 = STATIC;
    mcRsd.Speed                 = 0;
    mcRsd.HighSpdStart          = 0;
    mcRsd.RSDStep               = 0;
    mcRsd.RSDSpeedBase          = RSDSpeedCalBase;
    mcRsd.SetFR                 = FR_MODE;
    ClrBit(DRV_CR, FOCEN);      // 关闭FOC
    CMP_RSD_Init();
    Time2_RSD_Init();           // RSD用的是Time2
}



/**
    @brief        RSD 比较器初始化
    @date         2022-07-13
*/
static void CMP_RSD_Init(void)
{
    /*  -------------------------------------------------------------------------------------------------
        CMP Input Pin Mode
        P1.4--CMP0_IN+, P1.6--CMP1_IN+, P2.1--CMP2_IN+
        P1.5--CMP0_IN-, P1.7--CMP1_IN-, P2.2--CMP2_IN-
        P1.3--CMP1P2
        -------------------------------------------------------------------------------------------------*/
    SetBit(P1_AN, P14 | P15 | HBMOD);
    ClrBit(P1_OE, P13);
    /*  -------------------------------------------------------------------------------------------------
        CMP0_MOD：
        00： 无内置虚拟中心点电阻的BEMF模式
        01： 内置虚拟中心点电阻的BEMF模式
        10： 3差分比较器模式
        11： 2比较器模式RSD
        -------------------------------------------------------------------------------------------------*/
    SetReg(CMP_CR2, CMP0MOD0 | CMP0MOD1, CMP0MOD0 | CMP0MOD1);
    /*  -------------------------------------------------------------------------------------------------
        比较器输出选择配置，与CMP0_MOD配合使用
        CMP0_SEL[1:0]=00，比较器0工作在2比较器轮询模式，正端在CMP0P、CMP1P2之间自动轮流选择，负端固定接CMP0M，
                      其输出结果分别送至CMP0_OUT、CMP1_OUT
        CMP0_SEL[1:0]=01，比较器0选择CMP0对应的端口组合，即正端接CMP0P，负端接CMP0M，输出接CMP0_OUT
        CMP0_SEL[1:0]=10，比较器0选择CMP1对应的端口组合，即正端接CMP1P2，负端接CMP0M，输出接CMP1_OUT
        -----------------------------------------------------------------------------*/
    SetReg(CMP_CR2, CMP0SEL0 | CMP0SEL1, 0x00);
    /*  -------------------------------------------------------------------------------------------------
        比较器迟滞电压选择
        000: 无迟滞     001: ±2.5mV   010: -5mV   011: +5mV
        100: +-5mV   101: -10mV   110: +10mV   111: +-10mV
        -------------------------------------------------------------------------------------------------*/
    //SetReg(CMP_CR1, CMP0HYS0 | CMP0HYS1 | CMP0HYS2, CMP0HYS0 | CMP0HYS1 | CMP0HYS2 );
    /*  -------------------------------------------------------------------------------------------------
        CMP0的轮询时间设置
        -------------------------------------------------------------------------------------------------*/
    SetReg(CMP_CR1, CMP0CSEL0 | CMP0CSEL1, 0x00);
    EA = 0;
    /*  ------------------------------------------------------
        使能比较器CMP0,CMP1,CMP2和ADC在pwm on/off采样功能
    
        00：在on和off均采样，没有延迟采样开启
        01：只在off采样，根据CMP_SAMR延迟采样开启
        10：只在on采样，根据CMP_SAMR延迟采样开启
        11：在on和off均采样，根据CMP_SAMR延迟采样开启
        ---------------------------------------------------------**/
    //    SetReg(CMP_CR3, SAMSEL0 | SAMSEL1, SAMSEL1);
    /* 采样延迟设置 */
    CMP_SAMR = 0x10;
    SetBit(CMP_CR2, CMP0EN); //使能比较器
}



/**
    @brief        Rsd Timer2功能初始化
    @date         2022-07-13
*/
static void Time2_RSD_Init(void)
{
    /*  -------------------------------------------------------------------------------------------------
        先停止计数，配置完寄存器后，最后启动计数
        -------------------------------------------------------------------------------------------------*/
    ClrBit(TIM2_CR1, T2CEN);        // 0，停止计数；1,使能计数
    /*  -------------------------------------------------------------------------------------------------
        时钟分频设置(T2PSC)
        000:cpuclk(24MHz)         001:cpuclk/2^1(12MHz)   010:cpuclk/2^2(6MHz)    011:cpuclk/2^3(3MHz)
        100:cpuclk/2^4(1.5MHz)    101:cpuclk/2^5(750KHz)  110:cpuclk/2^6(375KHz)  111:cpuclk/2^7(187.5KHz)
        -------------------------------------------------------------------------------------------------*/
    SetReg(TIM2_CR0, T2PSC0 | T2PSC1 | T2PSC2, T2PSC0 | T2PSC1 | T2PSC2);
    /*  -------------------------------------------------------------------------------------------------
        /模式选择
        T2MODE1，T2MODE0
        00--输入Timer模式；01--输出模式
        10--输入Count模式；11--QEP或者RSD模式
        -------------------------------------------------------------------------------------------------*/
    SetReg(TIM2_CR0, T2MOD0 | T2MOD1, T2MOD0 | T2MOD1);
    ClrBit(TIM2_CR1, T2FE);                               // 滤波使能
    /*  -------------------------------------------------------------------------------------------------
        清除中断标志位
        禁止PWM周期检测中断使能
        使能计数器上溢中断使能
        -----------------------------------------------------------------------------------------------------*/
    ClrBit(TIM2_CR1, T2IR  | T2IF | T2IP);  // 清除中断标志位
    ClrBit(TIM2_CR0, T2CES | T2IRE);       // 清零脉冲计数器不使能
    SetBit(TIM2_CR1, T2IPE | T2IFE);       // 输入有效边沿变化中断使能和基本计数器上溢使能
    /*  -------------------------------------------------------------------------------------------------
        定时器2中断优先级配置及芯片中断总使能
        PTIM231-PTIM230，中断优先级控制值从0-3依次表示优先级从最低到最高，共4级优化级控制
        EA,芯片中断总使能
        -------------------------------------------------------------------------------------------------*/
    SetBit(IP1, PTIM21 );       // 输入有效边沿变化中断使能和基本计数器上溢使能
    SetBit(IP1, PTIM20 );       // 输入有效边沿变化中断使能和基本计数器上溢使能
    EA = 1;
    /*  -------------------------------------------------------------------------------------------------
        配置周期值、比较值、计数值
        -------------------------------------------------------------------------------------------------*/
    TIM2__CNTR = 0;
    /*-----------启动计数------------------------------------------------*/
    SetBit(TIM2_CR1, T2CEN);            //启动计数
}




/**
    @brief        RSD检测函数，使用Timer2 RSD功能，QEP接口相同，运行于Timer2 IP中断
    @date         2022-07-13
*/
void RsdProcess(void)
{
    uint16 temp_ARR         = 0;
    int16  temp_speedCal    = 0;
    mcRsd.CountPre  = mcRsd.Count;
    mcRsd.Count     = TIM2__CNTR;
    mcRsd.Status     = CMP_SR & 0x03;
    
    if (mcRsd.CountPre < 0) // 上一个状态为正转
    {
        if (mcRsd.Count < mcRsd.CountPre) // 当前为继续正转
        {
            if (mcRsd.Count < -8)
            {
                /* 连续单方向转动，则取出周期时。 一个反电势周期是4个RSD跳变状态 */
                mcRsd.Period = (mcRsd.StepTime[0] >> 2) + (mcRsd.StepTime[1] >> 2) + (mcRsd.StepTime[2] >> 2) + (mcRsd.StepTime[3] >> 2);
                
                if (mcRsd.SetFR == CW)
                {
                    mcRsd.State  = FORWARD;
                }
                else
                {
                    mcRsd.State  = REVERSE;
                }
            }
            
            if (mcRsd.ArrCnt > 3)
            {
                mcRsd.ArrCnt = 0;
            }
            
            temp_ARR                        = TIM2__ARR;
            mcRsd.StepTime[mcRsd.ArrCnt++]  = temp_ARR;    // 取出每个单独RSD电平状态对应的周期计数值
        }
        else //正转过程中发生反转
        {
            TIM2__CNTR              = 0;
            mcRsd.Count             = 0;
            mcRsd.Period            = 65535;
            mcRsd.State      = DETECTING;  // 检测中
        }
    }
    else if (mcRsd.CountPre > 0)              // 上一个状态为反转
    {
        if (mcRsd.Count > mcRsd.CountPre) // 当前为继续反转
        {
            if (mcRsd.Count > 8)
            {
                /* 连续单方向转动，则取出周期时。 一个周期反电势是4个RSD跳变状态 */
                mcRsd.Period = (mcRsd.StepTime[0] >> 2) + (mcRsd.StepTime[1] >> 2) + (mcRsd.StepTime[2] >> 2) + (mcRsd.StepTime[3] >> 2);
                
                if (mcRsd.SetFR == CW)
                {
                    mcRsd.State  = REVERSE;
                }
                else
                {
                    mcRsd.State  =  FORWARD;
                }
            }
            
            if (mcRsd.ArrCnt > 3)
            {
                mcRsd.ArrCnt = 0;
            }
            
            temp_ARR                        = TIM2__ARR;
            mcRsd.StepTime[mcRsd.ArrCnt++]  = temp_ARR;    // 取出每个单独RSD电平状态对应的周期计数值
        }
        else //  反转过程中发生正转
        {
            TIM2__CNTR              = 0;
            mcRsd.Count             = 0;
            mcRsd.Period            = 65535;
            mcRsd.State      = DETECTING;  // 检测中
        }
    }
    else
    {
        mcRsd.State      = DETECTING;  // 检测中
    }
    
    mcRsd.Times++;      // 多次中断次数
    
    if (mcRsd.SpeedUpdate == 0)
    {
        if (mcRsd.State == FORWARD || mcRsd.State == REVERSE )//处理速度
        {
            if (mcRsd.Period < RSDSpeedCalMaxSpeed)
            { mcRsd.Period = RSDSpeedCalMaxSpeed ; }// 防止mcRsd.Period 太小导致计算出错
            
            temp_speedCal = DivQ_L_MDU((mcRsd.RSDSpeedBase >> 16), mcRsd.RSDSpeedBase, mcRsd.Period);
            
            if (mcRsd.State == REVERSE)
            {
                mcRsd.Speed = -temp_speedCal;
            }
            else
            {
                mcRsd.Speed = temp_speedCal;
            }
            
            mcRsd.SpeedUpdate  = 1;
        }
    }
    
    if (mcRsd.HighSpdStart == 1)            // 高速顺风启动
    {
        if (mcRsd.Status == 0x03)           // 03状态 对应90启动
        {
            ClrBit(CMP_CR2, CMP0EN);        // 关闭比较器
            ClrBit(TIM2_CR1, T2CEN);        // 0，停止计数；1,使能计数
            RSDFOCCloseLoopStart();
            mcRsd.HighSpdStart = 2;         // 启动完成
        }
    }
}



/**
    @brief        RSD顺风启动函数
    @date         2022-07-13
*/

static void RSDFOCCloseLoopStart(void)
{
    /* FOC初始化 */
    FOC_Init();
    /* 启动电流、KP、KI */
    FOC_IDREF           = ID_RUN_CURRENT;                      // D轴启动电流
    FOC_IQREF           = 0;                      // Q轴启动电流
    mcFocCtrl.IqRef     = IQ_RUN_CURRENT;                      // Q轴启动电流
    FOC_QKP             = _Q12(0.5);
    FOC_QKI             = _Q15(0.005);
    FOC_DKP            = _Q12(0.5);
    FOC_DKI            = _Q15(0.005);
    FOC_EKP               = OBSW_KP_GAIN_RUN4;
    FOC_EKI               = OBSW_KI_GAIN_RUN4;
    FOC_EFREQACC        = 100;
    FOC_EFREQMIN        = 0;
    FOC_EFREQHOLD       = MOTOR_OMEGA_RAMP_END;
    SetBit(FOC_CR1, ANGM);                                     // 估算模式
    ClrBit(FOC_CR1, RFAE);                                     // 禁止强拉
    ClrBit(FOC_CR1, EFAE);                                     // 估算器强制输出
    //    FOC__EOME           = mcRsd.Speed;
    
    //    FOC_EKLPFMIN    = OBS_EA_KS_TAILWIND;
    
    if (mcRsd.Status == 3)
    {
        FOC__ETHETA         =   _Q15(90 / 180.0);
        FOC__THETA          =   _Q15(90 / 180.0);
    }
    
    #if (EstimateAlgorithm == SMO  || EstimateAlgorithm == AO)
    FOC_EKP               = OBSW_KP_GAIN_RUN4;
    FOC_EKI               = OBSW_KI_GAIN_RUN4;
    #elif (EstimateAlgorithm == PLL)
    FOC_EKP           = OBSW_KP_GAIN_RUN4;
    FOC_EKI           = OBSW_KI_GAIN_RUN4;
    mcFocCtrl.mcIqref = IQ_RUN_CURRENT;
    #endif  // end    EstimateAlgorithm
    FOC_OMEKLPF        = SPEED_KLPF;
    mcFocCtrl.State_Count = 50;
    mcFocCtrl.CtrlMode = 0;
    /* 使能输出 */
    DRV_CMR |= 0x3F;                         // U、V、W相输出
    MOE = 1;
}
#endif