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

/**
 * @copyright None
 * @file      AddFunction.c
 * @author    Comment Vivre
 * @date      2024-08-28
 * @brief     None
 */
#include <MyProject.h>

/* Public variables --------------------------------------------------------- */

bit isCtrlPowOn = false;           ///< 开关机控制
PWMINPUTCAL xdata mcPwmInput;            ///< PWM捕获结构体变量
FOCCTRL xdata mcFocCtrl;                 ///< FOC电机控制相关结构体变量
debugONOFFTypeDef xdata debug_ONOFFTest; ///< ONOFF启停测试小工具结构体变量


RefRamp data LoopRefRamp;                   ///< 控制指令爬坡结构体相关变量

/**
    @brief        对变量取16位的绝对值
    @param[in]    value
    @return       绝对值
    @date         2022-07-13
*/
uint16 Abs_F16(int16 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
    @brief        对变量取32位的绝对值
    @param[in]    value
    @return       绝对值
    @date         2022-07-13
*/
uint32 Abs_F32(int32 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
    @brief        PWM调速信号计算，本例程提供Duty计算，如需频率信号可自行使用mcPwmInput.Period周期值计算
    @date         2022-07-14
*/
void PWMDutyCal(void)
{
    static uint16 dutyTemp = 0;
    
    if (mcPwmInput.isUpdate) // 有新的duty更新
    {
        if ((Abs_F32(mcPwmInput.TimerDR - mcPwmInput.TimerDROld) < 0xFF)       // 误差在1个Byte之间再处理
            && (Abs_F32(mcPwmInput.TimerARROld - mcPwmInput.TimerARR) < 0xFF)) // 误差在1个Byte之间再处理
        {
            mcPwmInput.Compare = mcPwmInput.TimerDR; // 读取DR与ARR值
            mcPwmInput.Period = mcPwmInput.TimerARR;
            mcPwmInput.Duty = DivQ_L_MDU(mcPwmInput.Compare >> 1, 0x0000, mcPwmInput.Period);
            /***速度随PWM增大而增大***/
            #if (PWMDUTY_POLARITY == NegaPWMDUTY)
            {
                dutyTemp = 32768 - mcPwmInput.Duty;
            }
            /***速度随PWM增大而减小***/
            #else
            {
                dutyTemp = mcPwmInput.Duty;
            }
            #endif
            
            if ((dutyTemp > ONPWMDuty) && (dutyTemp <= OFFPWMDutyHigh))
            {
                isCtrlPowOn = 1; // 开机
            }
            else if ((dutyTemp < OFFPWMDuty) || (dutyTemp > OFFPWMDutyHigh))
            {
                isCtrlPowOn = 0; // 关机
            }
            else
            {
                // 不做处理，保持前一个状态
            }
            
            // 转速曲线计算
            if (isCtrlPowOn)
            {
                if (dutyTemp <= MINPWMDuty)
                {
                    mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM;
                }
                else if (dutyTemp >= MAXPWMDuty)
                {
                    mcFocCtrl.Ref = MOTOR_SPEED_MAX_RPM;
                }
                else
                {
                    mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM + SPEED_K * (dutyTemp - MINPWMDuty);
                }
            }
            else
            {
                mcFocCtrl.Ref = 0;
            }
        }
        
        mcPwmInput.isUpdate = 0;
        mcPwmInput.TimerDROld = mcPwmInput.TimerDR;   // 将此次比较值赋值给上次比较值
        mcPwmInput.TimerARROld = mcPwmInput.TimerARR; // 将此次周期值赋值给上次周期值
    }
}

/**
    @brief        VSP调速信号处理
    @date         2022-07-14
*/
void VSPSample(void)
{
    static int16 VSP = 0;
    VSP = LPFFunction(ADC7_DR, VSP, 10); // 注意低通滤波器系数范围为0---127
    
    if (VSP > ONPWMDuty)
    {
        isCtrlPowOn = 1; // 开机
    }
    else if ((VSP < OFFPWMDuty)) // 电机停机
    {
        isCtrlPowOn = 0; // 关机
    }
    
    // 转速曲线计算
    if (isCtrlPowOn) //
    {
        #if (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
        {
            if (VSP <= MINPWMDuty) // 最小转速运行
            {
                mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM;
            }
            else if (VSP < MAXPWMDuty) // 调速
            {
                mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM + SPEED_K * (VSP - MINPWMDuty);
            }
            else // 最大转速运行
            {
                mcFocCtrl.Ref = MOTOR_SPEED_MAX_RPM;
            }
        }
        #endif
    }
    else
    {
        mcFocCtrl.Ref = 0;
    }
}

/**
 * @function     GetSrefKeyGear
 * @brief        根据电压挡位确定输出
 * @param[in]    None
 * @return       None
 * @date         2024-08-28
*/
void GetSrefKeyGear(void)
{
    static uint8 powONHoldCnt   = 0;
    static uint8 powOFFHoldCnt  = 0;
    
    if (isCtrlPowOn) // 开机状态
    {
        if (mcFocCtrl.SREFValue < VSP_OFF_MIN)
        {
            powOFFHoldCnt ++;
            
            if (powOFFHoldCnt > 50)
            {
                powOFFHoldCnt = 0;
                isCtrlPowOn = false; // 关机
            }
            
            powONHoldCnt = 0;
        }
    }
    else    // 关机状态
    {
        if (mcFocCtrl.SREFValue > VSP_ON_MIN)
        {
            powONHoldCnt ++;
            
            if (powONHoldCnt > 50)
            {
                powONHoldCnt = 0;
                isCtrlPowOn = true; // 开机
            }
            
            powOFFHoldCnt = 0;
        }
    }
    
    //转速曲线计算
    if (isCtrlPowOn)
    {
		// VSP测试代码
		// mcFocCtrl.Ref = mcFocCtrl.SREFValue;
		
        // 计算公式为
        // Ref = ( 区间UQ差 / 区间调速电压差) * (VSP - 调速下限) + 区间最小UQ值
		if(mcFocCtrl.SREFValue <=GearMinVol)
		{ mcFocCtrl.Ref = 3094; }
		else if(mcFocCtrl.SREFValue <=GearA)
		{ mcFocCtrl.Ref = 0.462 * (mcFocCtrl.SREFValue - GearMinVol) + 3094; }
		else if(mcFocCtrl.SREFValue <=GearB)
		{ mcFocCtrl.Ref = 0.734 * (mcFocCtrl.SREFValue - GearA) + 9454; }
		else if(mcFocCtrl.SREFValue <=GearC)
		{ mcFocCtrl.Ref = 0.949 * (mcFocCtrl.SREFValue - GearB) + 13303; }
		else if(mcFocCtrl.SREFValue <=GearD)
		{ mcFocCtrl.Ref = 0.795 * (mcFocCtrl.SREFValue - GearC) + 18280; }
		else if(mcFocCtrl.SREFValue <=GearE)
		{ mcFocCtrl.Ref = 1.011 * (mcFocCtrl.SREFValue - GearD) + 20887; }
		else
		{ mcFocCtrl.Ref = 22875; }
    }
    else
    { mcFocCtrl.Ref = 0; }
}

/**
    @brief        启停测试工具，用于测试启动可靠性
    @date         2022-07-14
*/
void ONOFF_Test(void)
{
    if (debug_ONOFFTest.State == 1) // 开机状态
    {
        debug_ONOFFTest.TimeCnt++;
        
        if (debug_ONOFFTest.TimeCnt > ONOFFTEST_ON_TIME)
        {
            debug_ONOFFTest.Times++;   // 启停次数+1
            debug_ONOFFTest.State = 0; // 切换到关机状态
            debug_ONOFFTest.TimeCnt = 0;
            mcFocCtrl.Ref = 0;   // 目标值也给0
            isCtrlPowOn = 0; // 关机
        }
    }
    else // 关机状态
    {
        debug_ONOFFTest.TimeCnt++;
        
        if (debug_ONOFFTest.TimeCnt > ONOFFTEST_OFF_TIME)
        {
            debug_ONOFFTest.TimeCnt = 0;
            
            if (mcState != mcFault)
            {
                debug_ONOFFTest.State = 1; // 切换到开机状态
                mcFocCtrl.Ref = ONOFFTEST_REF;
                isCtrlPowOn = 1; // 开机
                mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
            }
        }
    }
}

/**
    @brief        调速信号处理包含：开关机控制、将调速信号处理成控制目标给定信号
    @date         2022-07-14
*/
void GetTargetRef(void)
{
    #if (SPEED_MODE == PWMMODE)
    {
        PWMDutyCal();
        mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
    }
    #elif (SPEED_MODE == SREFMODE)
    {
        VSPSample();
        mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
    }
    #elif (SPEED_MODE == SREFKEYMODE)
    {
        GetSrefKeyGear();
        mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
    }
    #elif (SPEED_MODE == NONEMODE)
    {
        isCtrlPowOn = true;
        mcFocCtrl.Ref = NONE_MODE_SPEED;
        mcFocCtrl.PowerLimitValue = POWERLPFLIMIT;
    }
    #elif (SPEED_MODE == ONOFFTEST)
    {
        ONOFF_Test();
    }
    #endif
}

/**
 * @function     LoopResponse
 * @brief        闭环控制函数
 * @param[in]    None
 * @return       None
 * @date         2024-08-28
*/
void LoopResponse(void)
{
    static int16 refRampOut = 0;
    
    switch (mcFocCtrl.CtrlMode)
    {
        case 0:
        {
            if (mcFocCtrl.SpeedFlt > MOTOR_LOOP_RPM)
            {
                mcFocCtrl.Mode0HoldCnt++;
                
                if (mcFocCtrl.Mode0HoldCnt > 10)
                {
                    FOC_QKP = QKP;
                    FOC_QKI = QKI;
                    FOC_DKP = DKP;
                    FOC_DKI = DKI;
                    // FOC_THECOMP = _Q15(-25.0 / 180.0); // SMO 估算补偿角
                    // 启动电流环与外环给定衔接
                    #if (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
                    LoopRefRamp.Out = mcFocCtrl.SpeedFlt;
                    #elif (MOTOR_CTRL_MODE == POWER_LOOP_CONTROL)
                    LoopRefRamp.Out = mcFocCtrl.Power;
                    #elif (MOTOR_CTRL_MODE == UQ_LOOP_CONTROL)
                    LoopRefRamp.Out = mcFocCtrl.UqFlt;
                    #elif (MOTOR_CTRL_MODE == UQ_POWER_CONTROL)
                    LoopRefRamp.Out = mcFocCtrl.UqFlt;
                    #endif
                    mcFocCtrl.LoopTime = LOOP_TIME;
                    LoopRefRamp.Inc = RAMP_INC;
                    LoopRefRamp.Dec = RAMP_DEC;
                    mcFocCtrl.IqRef = FOC_IQREF;
                    FOC_IDREF = ID_RUN_CURRENT; // D轴启动电流
                    PI1_UKH = mcFocCtrl.IqRef;
                    PI2_Init(); // PI初始化
                    // 弱磁
                    #if (MotorFiledWeakenEn)
                    FiledWeakenInit();
                    #endif
                    // 电压补偿
                    VoltageComp.Undervoltage_flag = 0;
                    VoltageComp.IncVoltage = _Q15(40.0 / HW_BOARD_VOLT_MAX);
                    VoltageComp.LineAngel =  LinearCompensationAngel;
                    VoltageComp.LineAngelMax =  LinearCompensationAngel_MAX;
                    VoltageComp.LineAngelMin =  LinearCompensationAngel_MIN;
                    VoltageComp.VCDelayCnt =  VoltageCompensationDelayCnt;
                    mcFocCtrl.CtrlMode = 1;
                }
            }
            else
            { mcFocCtrl.Mode0HoldCnt = 0; }
            
            break;
        }
        
        case 1:
        {
            mcFocCtrl.LoopTime++;
            
            if (mcFocCtrl.LoopTime >= LOOP_TIME)
            {
                mcFocCtrl.LoopTime = 0;
                refRampOut = LoopRamp(mcFocCtrl.Ref); // 控制命令爬坡函数，用于实现调速信号之间平滑过渡
                #if (MOTOR_CTRL_MODE == CURRENT_LOOP_CONTROL)
                {
                    mcFocCtrl.IqRef = refRampOut;
                    FOC_IQREF = mcFocCtrl.IqRef;
                }
                #elif (MOTOR_CTRL_MODE == SPEED_LOOP_CONTROL)
                {
                    mcFocCtrl.IqSpeedRef = HW_One_PI(refRampOut - mcFocCtrl.SpeedFlt);
                    mcFocCtrl.LimitIqOut = HW_One_PI2(mcFocCtrl.PowerLimitValue - mcFocCtrl.Power); // 限制功率
                
                    if ((mcFocCtrl.LimitIqOut < mcFocCtrl.IqSpeedRef)) // 限制输出电流
                    {
                        mcFocCtrl.ExtDec = (mcFocCtrl.IqRef - mcFocCtrl.LimitIqOut) / 3;
                        mcFocCtrl.IqRef -= mcFocCtrl.ExtDec;
                    }
                    else
                    { mcFocCtrl.IqRef = mcFocCtrl.IqSpeedRef; }
                
                    #if (MotorFiledWeakenEn)
                    FileWeakenControl();
                    #else
                    FOC_IQREF = mcFocCtrl.IqRef;
                    #endif
                }
                #elif (MOTOR_CTRL_MODE == POWER_LOOP_CONTROL)
                {
                    mcFocCtrl.IqRef = HW_One_PI(refRampOut - mcFocCtrl.Power);
                    FOC_IQREF = mcFocCtrl.IqRef;
                }
                #elif (MOTOR_CTRL_MODE == UQ_LOOP_CONTROL)
                {
                    mcFocCtrl.IqRef = HW_One_PI(refRampOut - mcFocCtrl.UqFlt);
                    FOC_IQREF = mcFocCtrl.IqRef;
                }
                #elif (MOTOR_CTRL_MODE == UQ_POWER_CONTROL)
                {
					mcFocCtrl.LoopCalcInValue = mcFocCtrl.UqFlt*0.45 + mcFocCtrl.Power*0.65;
                    mcFocCtrl.LoopCalcOutValue = HW_One_PI(refRampOut - mcFocCtrl.LoopCalcInValue);
					mcFocCtrl.LimitIqOut = HW_TWO_PI(mcFocCtrl.PowerLimitValue - mcFocCtrl.Power);
					// 限制输出电流
                    if ((mcFocCtrl.LimitIqOut < mcFocCtrl.LoopCalcOutValue)) 
                    {
                        mcFocCtrl.ExtDec = (mcFocCtrl.IqRef - mcFocCtrl.LimitIqOut) / 3;
                        mcFocCtrl.IqRef -= mcFocCtrl.ExtDec;
                    }
                    else
                    { mcFocCtrl.IqRef = mcFocCtrl.LoopCalcOutValue; }
					
                    FOC_IQREF = mcFocCtrl.IqRef;
                }
                #endif
            }
            
            #if (SVPWM_5_Segment_Run_Enale == 1) // 开启五段式
            {
                if (mcFocCtrl.SpeedFlt > Motor_F5SEG_Speed)
                { SetBit(FOC_CR2, F5SEG); }
                else if (mcFocCtrl.SpeedFlt < Motor_F7SEG_Speed)
                { ClrBit(FOC_CR2, F5SEG); }
            }
            #endif
            break;
        }
    }
}

/**
    @brief        控制给定爬坡函数
                 以浮点进行计算，解决整数爬坡由于精度的影响，导致爬坡结果阶梯变化
                 函数控制周期默认为闭环控制周期，建议使用默认1ms周期运行
    @param[in]    ref 给定目标值
    @return       爬坡结果（int16）
    @date         2022-07-14
*/
int16 LoopRamp(int16 Xn0)
{
    LoopRefRamp.In = Xn0; // 爬坡函数输入
    
    // 数据爬坡处理
    if ((LoopRefRamp.Out + LoopRefRamp.Inc) < LoopRefRamp.In)
    { LoopRefRamp.Out += LoopRefRamp.Inc; }
    else if ((LoopRefRamp.Out - LoopRefRamp.Dec) > LoopRefRamp.In)
    { LoopRefRamp.Out -= LoopRefRamp.Dec; }
    
    return (int16)LoopRefRamp.Out;
}

/**
    @brief        启动ATO爬坡函数，用于静止启动时候对ATO进行爬坡，提高启动可靠性
    @date         2022-07-14
*/
void ATORamp(void)
{
    if (mcFocCtrl.State_Count == (ATO_RAMP_PERIOD << 2))
    {
        FOC_EKP = OBSW_KP_GAIN_RUN1; // 估算器里的PI的KP
        FOC_EKI = OBSW_KI_GAIN_RUN1; // 估算器里的PI的KI
    }
    else if (mcFocCtrl.State_Count == ((ATO_RAMP_PERIOD << 1) + ATO_RAMP_PERIOD))
    {
        FOC_EKP = OBSW_KP_GAIN_RUN2; // 估算器里的PI的KP
        FOC_EKI = OBSW_KI_GAIN_RUN2; // 估算器里的PI的KI
    }
    else if (mcFocCtrl.State_Count == (ATO_RAMP_PERIOD << 1))
    {
        FOC_EKP = OBSW_KP_GAIN_RUN3; // 估算器里的PI的KP
        FOC_EKI = OBSW_KI_GAIN_RUN3; // 估算器里的PI的KI
    }
    else if (mcFocCtrl.State_Count <= ATO_RAMP_PERIOD && mcFocCtrl.Flg_ATORampEnd == 0)
    {
        FOC_EKP = OBSW_KP_GAIN_RUN4;  // 估算器里的PI的KP
        FOC_EKI = OBSW_KI_GAIN_RUN4;  // 估算器里的PI的KI
        mcFocCtrl.Flg_ATORampEnd = 1; // ATO 爬坡结束
    }
}